Minor fixes (untested)

Commands/Autonomous/AutoDrive.cpp

@@ -1,6 +1,7 @@
 #include "AutoDrive.h"
 #include <cmath>
 #include "../../DentRobot.h"
+// Drive for a short while then stop. Just for testing
 AutoDrive::AutoDrive() : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(1.0);

Commands/Collector/CollectTote.cpp

@@ -3,6 +3,7 @@ CollectTote::CollectTote() : Command("CollectTote"){
   Requires(DentRobot::collector);
 }
 void CollectTote::Initialize(){
+  SetTimeout(2.0);
 }
 void CollectTote::Execute(){
   //TODO check this value to move the motors in the right direction

Commands/Collector/OpenCollector.cpp

@@ -3,10 +3,11 @@ OpenCollector::OpenCollector() : Command("OpenCollector"){
   Requires(DentRobot::collector);
 }
 void OpenCollector::Initialize(){
+  SetTimeout(0.5);
 }
 void OpenCollector::Execute(){
   //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveArms(-0.1);
+  DentRobot::collector->MoveArms(-0.1f);
 }
 bool OpenCollector::IsFinished(){
   return DentRobot::collector->ArmSensor();

Commands/Collector/ReleaseTote.cpp

@@ -3,11 +3,13 @@ ReleaseTote::ReleaseTote() : Command("ReleaseTote"){
   Requires(DentRobot::collector);
 }
 void ReleaseTote::Initialize(){
+  SetTimeout(2.0);
 }
 void ReleaseTote::Execute(){
   //TODO check this value to move the motors in the right direction
   printf("releasing tote\n");
-  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  // 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);
 }
 bool ReleaseTote::IsFinished(){
   return DentRobot::collector->BoxCollected();

Subsystems/Collector.cpp

@@ -10,7 +10,7 @@ Collector::Collector() : Subsystem("Collector") {
 }
 void Collector::InitDefaultCommand() {
 }
-void Collector::MoveArms(float a){
+void Collector::MoveArms(double a){
   windowMotorLeft->Set(a);
   windowMotorRight->Set(-a);
 }

Subsystems/Collector.h

@@ -10,7 +10,7 @@ class Collector: public Subsystem
   public:
     Collector();
     void InitDefaultCommand();
-    void MoveArms(float);
+    void MoveArms(double);
     void MoveRollers(double);
     bool ArmSensor();
     bool BoxCollected();