Minor improvements

Commands/Collector/RollIn.cpp

@@ -6,8 +6,13 @@ void RollIn::Initialize(){
   SetTimeout(2.0);
 }
 void RollIn::Execute(){
-  //TODO check this value to move the motors in the right direction
+  double throttle=DentRobot::oi->GetLeftThrottle();
-  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4/5;
+  if(cvt>=1.0){
+    DentRobot::collector->MoveRollers(1.0);
+  }else{
+    DentRobot::collector->MoveRollers(cvt);
+  }
 }
 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);
+  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle());
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();

Commands/Elevator/Raise.cpp

@@ -18,8 +18,8 @@ bool Raise::IsFinished(){
   //  DentRobot::elevator->stoppedAtSensor=false;
   //  return true;
   //}else if (!DentRobot::elevator->GetElevatorTop()) {
-  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+  if (!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
-    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
+    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle());
     return true;
   }else{
     return false;

OI.cpp

@@ -46,4 +46,10 @@ Joystick* OI::GetRightStick(){
 Joystick* OI::GetLeftStick(){
   return leftStick;
 }
+double OI::GetRightThrottle(){
+  return (-rightStick->GetRawAxis(3)+1.0)/2;
+}
+double OI::GetLeftThrottle(){
+  return (-leftStick->GetRawAxis(3)+1.0)/2;
+}
 // vim: ts=2:sw=2:et

OI.h

@@ -10,9 +10,11 @@ class OI
     Joystick *leftStick, *rightStick;
   public:
     OI();
+    Command *raise, *lower, *binLower, *binRaise;
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
-    Command *raise, *lower, *binLower, *binRaise;
+    double GetLeftThrottle();
+    double GetRightThrottle();
 };
 #endif
 // vim: ts=2:sw=2:et

RobotMap.h

@@ -6,8 +6,8 @@
 // Elevator
 #define ELEVATOR_CAN 1
 #define ELEVATOR_BOTTOM_DIO 9
-#define ELEVATOR_MIDDLE_DIO 7
+#define ELEVATOR_MIDDLE_DIO 8
-#define ELEVATOR_TOP_DIO 8
+#define ELEVATOR_TOP_DIO 7
 #define ELEVATOR_ENCODERA 4
 #define ELEVATOR_ENCODERB 9
 

Subsystems/Collector.cpp

@@ -15,9 +15,9 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  GetSonarDistance();
+  printf("%f\n",GetSonarDistance());
 }
-float Collector::GetSonarDistance(){
+double Collector::GetSonarDistance(){
-  printf("Sonar Distance %f\n",sonarAnalog->GetAverageVoltage());
+  return sonarAnalog->GetAverageVoltage();
 }
 // vim: ts=2:sw=2:et

Subsystems/Collector.h

@@ -12,7 +12,7 @@ class Collector: public Subsystem
     Collector();
     void InitDefaultCommand();
     void MoveRollers(double);
-    float GetSonarDistance();
+    double GetSonarDistance();
 };
 #endif
 // vim: ts=2:sw=2:et