Added BinElevator commands

Commands/Autonomous/AutoDrive.cpp

@@ -4,12 +4,22 @@
 AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(wait);
+  power=NULL;
+}
+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
-  DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
+  if(power==NULL){
+    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/AutoDrive.h

@@ -7,8 +7,11 @@
 #include "WPILib.h"
 
 class AutoDrive: public Command{
+  private:
+    double *power;
   public:
     AutoDrive(double);
+    AutoDrive(double, double);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Collector/CollectTote.cpp

@@ -0,0 +1,11 @@
+#include "CollectTote.h"
+#include "../../DentRobot.h"
+#include "../Autonomous/AutoDrive.h"
+#include "RollIn.h"
+#include "CloseCollector.h"
+CollectTote::CollectTote(){
+  AddParallel(new RollIn());
+  AddParallel(new AutoDrive(0.5, -0.75));
+  AddSequential(new CloseCollector());
+}
+// vim: ts=2:sw=2:et

Commands/Collector/CollectTote.h

@@ -0,0 +1,14 @@
+#ifndef COLLECTTOTE_H
+#define COLLECTTOTE_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CollectTote: public CommandGroup{
+  public:
+    CollectTote();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/ReleaseTote.cpp

@@ -0,0 +1,11 @@
+#include "ReleaseTote.h"
+#include "../../DentRobot.h"
+#include "RollOut.h"
+#include "../Autonomous/AutoDrive.h"
+#include "CloseCollector.h"
+ReleaseTote::ReleaseTote(){
+  AddParallel(new RollOut());
+  AddParallel(new AutoDrive(0.5, 0.75));
+  AddSequential(new CloseCollector());
+}
+// vim: ts=2:sw=2:et

Commands/Collector/ReleaseTote.h

@@ -0,0 +1,14 @@
+#ifndef RELEASETOTE_H
+#define RELEASETOTE_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class ReleaseTote: public CommandGroup{
+  public:
+    ReleaseTote();
+};
+#endif
+// vim: ts=2:sw=2:et

OI.cpp

@@ -5,6 +5,8 @@
 #include "Commands/Collector/CloseCollector.h"
 #include "Commands/Collector/RollIn.h"
 #include "Commands/Collector/RollOut.h"
+#include "Commands/BinElevator/BinLower.h"
+#include "Commands/BinElevator/BinRaise.h"
 
 OI::OI() {
   // Joysticks
@@ -34,10 +36,21 @@ OI::OI() {
   right6->WhenPressed(raise);
   right5->CancelWhenPressed(lower);
   right6->CancelWhenPressed(lower);
+  // BinElevator
+  binRaise=new BinRaise();
+  binLower=new BinLower();
+  JoystickButton *right7=new JoystickButton(rightStick, 7);
+  JoystickButton *right8=new JoystickButton(rightStick, 8);
+  right7->WhenPressed(binLower);
+  right7->CancelWhenPressed(binRaise);
+  right8->WhenPressed(binRaise);
+  right8->CancelWhenPressed(binLower);
   // Cancel
   JoystickButton *right12=new JoystickButton(rightStick, 12);
   right12->CancelWhenPressed(raise);
   right12->CancelWhenPressed(lower);
+  right12->CancelWhenPressed(binRaise);
+  right12->CancelWhenPressed(binLower);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;

OI.h

@@ -12,7 +12,7 @@ class OI
     OI();
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
-    Command *raise, *lower;
+    Command *raise, *lower, *binLower, *binRaise;
 };
 #endif
 // vim: ts=2:sw=2:et

RobotMap.h

@@ -9,16 +9,16 @@
 #define ELEVATOR_COLELCT_TOTE_DIO 1
 #define ELEVATOR_READY_TOTE_DIO 2
 #define ELEVATOR_TOP_DIO 5
-#define ELEVATOR_ENCODERA 0
+#define ELEVATOR_ENCODERA 4
-#define ELEVATOR_ENCODERB 1
+#define ELEVATOR_ENCODERB 9
 
 // BinElevator
 #define BINELEVATOR_CAN 11
 #define BINELEVATOR_BOTTOM_DIO 6
 #define BINELEVATOR_COLELCT_BIN_DIO 7
 #define BINELEVATOR_TOP_DIO 8
-#define BINELEVATOR_ENCODERA 2
+#define BINELEVATOR_ENCODERA 10
-#define BINELEVATOR_ENCODERB 3
+#define BINELEVATOR_ENCODERB 11
 
 // Drivetrain
 #define DRIVE_FRONT_LEFT_CAN 2