Made auto more modular

Commands/Autonomous/AutoDrive.cpp

@@ -1,27 +1,17 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
+AutoDrive::AutoDrive(double duration, double p=-0.75) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
+  SetTimeout(duration);
-  power=5;
-}
-AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
-  Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
   power=p;
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  //if(power==NULL){
-  if(power==5){
-    DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
-  }else{
   DentRobot::drivetrain->DriveMecanum(0.0,power,0.0,0.9,0.0);
 }
-}
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
 }

Commands/Autonomous/Autonomous.cpp

@@ -12,7 +12,7 @@ Autonomous::Autonomous(int seq){
     case 0:
       // Just for testing
       AddSequential(new CollectTote());
-      AddSequential(new Turn());
+      AddSequential(new Turn(90));
       //AddSequential(new Raise());
       //AddSequential(new Lower());
       //AddSequential(new Turn());
@@ -23,21 +23,18 @@ Autonomous::Autonomous(int seq){
     case 1:
       // Drive forward a bit, turn around, collect tote then bin
       AddSequential(new AutoDrive(0.5));
-      AddSequential(new Turn());
+      AddSequential(new Turn(180));
-      AddSequential(new Turn());
+      AddSequential(new RollIn(1.0));
-      AddSequential(new RollIn());
       AddSequential(new Raise());
       break;
     case 2:
       // Drive forward a bit, turn around, collect tote then bin
       AddParallel(new Raise());
       AddParallel(new AutoDrive(0.5));
-      AddSequential(new Turn());
+      AddSequential(new Turn(180));
-      AddSequential(new Turn());
+      AddSequential(new RollIn(1.0));
-      AddSequential(new RollIn());
       break;
     case 3:
-      // Wait a desigated value, drive to Auto Zone (TM)
       //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
       AddSequential(new AutoDrive(2.0));
       break;

Commands/Autonomous/Turn.cpp

@@ -1,8 +1,9 @@
 #include "Turn.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-Turn::Turn() : Command("Turn"){
+Turn::Turn(int k) : Command("Turn"){
   Requires(DentRobot::drivetrain);
+  degrees=k;
   SetTimeout(0.85);
 }
 void Turn::Initialize(){

Commands/Autonomous/Turn.h

@@ -7,8 +7,10 @@
 #include "WPILib.h"
 
 class Turn: public Command{
+  private:
+    int degrees;
   public:
-    Turn();
+    Turn(int);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Collector/CollectTote.cpp

@@ -4,6 +4,6 @@
 #include "RollIn.h"
 CollectTote::CollectTote(){
   AddParallel(new AutoDrive(1.0, -0.75));
-  AddSequential(new RollIn());
+  AddSequential(new RollIn(1.0));
 }
 // vim: ts=2:sw=2:et

Commands/Collector/RollIn.cpp

@@ -1,13 +1,13 @@
 #include "RollIn.h"
-RollIn::RollIn() : Command("RollIn"){
+RollIn::RollIn(double k) : Command("RollIn"){
+  rawSpeed=k;
 }
 void RollIn::Initialize(){
   printf("Initialized RollIn\n");
   SetTimeout(2.0);
 }
 void RollIn::Execute(){
-  double throttle=DentRobot::oi->GetLeftThrottle();
+  double cvt=(rawSpeed)*DentRobot::collector->GetSonarDistance()/0.4;
-  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
   if(cvt>=1.0){
     DentRobot::collector->MoveRollers(1.0);
   }else{

Commands/Collector/RollIn.h

@@ -7,8 +7,10 @@
 #include "WPILib.h"
 
 class RollIn: public Command{
+  private:
+    double rawSpeed;
   public:
-    RollIn();
+    RollIn(double);
     void Initialize();
     void Execute();
     bool IsFinished();

OI.cpp

@@ -5,6 +5,8 @@
 #include "Commands/Collector/RollOut.h"
 #include "Commands/BinElevator/BinLower.h"
 #include "Commands/BinElevator/BinRaise.h"
+#include "Commands/BinElevator/BinCloseArms.h"
+#include "Commands/BinElevator/BinOpenArms.h"
 
 OI::OI() {
   // Joysticks
@@ -13,7 +15,7 @@ OI::OI() {
   // Collector
   JoystickButton *left1=new JoystickButton(leftStick, 1);
   JoystickButton *left2=new JoystickButton(leftStick, 2);
-  left1->WhileHeld(new RollIn());
+  left1->WhileHeld(new RollIn(GetLeftThrottle()));
   left2->WhileHeld(new RollOut());
   // Elevator
   raise=new Raise();
@@ -27,12 +29,16 @@ OI::OI() {
   // BinElevator
   JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right5=new JoystickButton(rightStick, 5);
+  JoystickButton *right7=new JoystickButton(rightStick, 7);
+  JoystickButton *right8=new JoystickButton(rightStick, 8);
   binRaise=new BinRaise();
   binLower=new BinLower();
   right3->WhenPressed(binLower);
   right3->CancelWhenPressed(binRaise);
   right5->WhenPressed(binRaise);
   right5->CancelWhenPressed(binLower);
+  right7->WhenPressed(new BinOpenArms());
+  right8->WhenPressed(new BinCloseArms());
   // Cancel
   JoystickButton *right12=new JoystickButton(rightStick, 12);
   right12->CancelWhenPressed(raise);