Fixed sensor readings and added sonar

Commands/BinElevator/BinLower.cpp

@@ -10,7 +10,7 @@ void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);
 }
 bool BinLower::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorBottom()||*/IsTimedOut()){
     printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
     return true;
   }else{

Commands/BinElevator/BinRaise.cpp

@@ -10,7 +10,7 @@ void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);
 }
 bool BinRaise::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorTop()||*/IsTimedOut()){
     printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
     return true;
   }else{

Commands/Collector/RollIn.cpp

@@ -7,7 +7,7 @@ void RollIn::Initialize(){
 }
 void RollIn::Execute(){
   //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool RollIn::IsFinished(){
   return IsTimedOut();

Commands/Collector/RollOut.cpp

@@ -8,7 +8,7 @@ void RollOut::Initialize(){
 void RollOut::Execute(){
   //TODO check this value to move the motors in the right direction
   // Devide by 2 twice because this speed should be half the collector speed
-  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
+  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();

Commands/Elevator/Raise.cpp

@@ -10,8 +10,15 @@ void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
 }
 bool Raise::IsFinished(){
-  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+  if(!DentRobot::elevator->GetElevatorMiddle()){
-    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
+    DentRobot::elevator->stoppedAtSensor=true;
+  }
+  if ((DentRobot::elevator->stoppedAtSensor)){
+    printf("Stopped at the middle sensor\n");
+    DentRobot::elevator->stoppedAtSensor=false;
+    return true;
+  }else if (!DentRobot::elevator->GetElevatorTop()) {
+    printf("Stopping at the top sensor\n");
     return true;
   }else{
     return false;

RobotMap.h

@@ -30,6 +30,8 @@
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
+#define COLLECTOR_SONAR_DIO 6
+#define COLLECTOR_SONAR_ANALOG 3
 
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Collector.cpp

@@ -6,6 +6,8 @@ Collector::Collector() : Subsystem("Collector"){
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
   collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
+  sonarAnalog=new AnalogInput(COLLECTOR_SONAR_ANALOG);
+  sonarDigital=new DigitalOutput(COLLECTOR_RIGHT_CAN);
 }
 void Collector::InitDefaultCommand(){
 }
@@ -14,5 +16,9 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
+  GetSonarDistance();
+}
+float Collector::GetSonarDistance(){
+  printf("Sonar Distance %f\n",sonarAnalog->GetAverageValue());
 }
 // vim: ts=2:sw=2:et

Subsystems/Collector.h

@@ -6,10 +6,13 @@ class Collector: public Subsystem
 {
   private:
     CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRamp, *collectorMotorRight;
+    AnalogInput *sonarAnalog;
+    DigitalOutput *sonarDigital;
   public:
     Collector();
     void InitDefaultCommand();
     void MoveRollers(double);
+    float GetSonarDistance();
 };
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -4,9 +4,11 @@ Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
   elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
+  elevatorMiddle=new DigitalInput(ELEVATOR_MIDDLE_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
   // Checks if the elevator is drifting
   useEncoder=false;
+  stoppedAtSensor=false;
 }
 void Elevator::InitDefaultCommand(){
 }
@@ -26,6 +28,9 @@ double Elevator::GetHeight(){
 bool Elevator::GetElevatorBottom(){
   return elevatorBottom->Get();
 }
+bool Elevator::GetElevatorMiddle(){
+  return elevatorMiddle->Get();
+}
 bool Elevator::GetElevatorTop(){
   return elevatorTop->Get();
 }

Subsystems/Elevator.h

@@ -8,15 +8,17 @@ class Elevator{
     CANTalon *motor;
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    DigitalInput *elevatorBottom, *elevatorTop;
+    DigitalInput *elevatorBottom, *elevatorMiddle, *elevatorTop;
     bool useEncoder;
   public:
     Elevator();
+    bool stoppedAtSensor;
     void InitDefaultCommand();
     void Run(double);
     void ResetEncoder();
     double GetHeight();
     bool GetElevatorTop();
+    bool GetElevatorMiddle();
     bool GetElevatorBottom();
     void SetUseEncoder(bool);
     bool GetUseEncoder();