diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index 01718f2..0eb43d6 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -1,6 +1,7 @@ #include "AutoDrive.h" #include #include "../../DentRobot.h" +// Drive for a short while then stop. Just for testing AutoDrive::AutoDrive() : Command("AutoDrive"){ Requires(DentRobot::drivetrain); SetTimeout(1.0); diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp index 9780b39..e7fad0f 100644 --- a/Commands/Collector/CollectTote.cpp +++ b/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 diff --git a/Commands/Collector/OpenCollector.cpp b/Commands/Collector/OpenCollector.cpp index 54928c5..6e41cd4 100644 --- a/Commands/Collector/OpenCollector.cpp +++ b/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(); diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp index 90ceaab..0ee9ce1 100644 --- a/Commands/Collector/ReleaseTote.cpp +++ b/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(); diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp index b05c76c..26dc2d7 100644 --- a/Subsystems/Collector.cpp +++ b/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); } diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h index ef5acf3..931744f 100644 --- a/Subsystems/Collector.h +++ b/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();