Merged develop with final dashboard values from Palmetto

Commands/Autonomous/Autonomous.cpp

@@ -25,7 +25,16 @@ Autonomous::Autonomous(int seq){
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 2:
-      // Collect a tote, turn, drive to Auto Zone (TM)
+      // Lower BinElevator, collect bin, turn, drive to AutoZone (TM) 
+      AddSequential(new BinLower(0.5));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Bin Distance"), 0, 0.75));
+      AddSequential(new BinRaise(1.0));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
+      break;
+    case 3:
+      // Collect a tote with BinElevator, turn, drive to Auto Zone (TM)
       Wait(SmartDashboard::GetNumber("Auto Wait Time"));
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       AddSequential(new BinRaise(1.2));
@@ -33,29 +42,41 @@ Autonomous::Autonomous(int seq){
       AddSequential(new BinLower(1.0));
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
-    case 3:
-      // Collect three totes, drive to Auto Zone (TM)
+    case 4:
+      // Collect one, two, or three totes, drive to Auto Zone (TM), release totes
       printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
       Wait(SmartDashboard::GetNumber("Auto Wait Time"));
       printf("Done");
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
       AddSequential(new CollectTote());
-      AddSequential(new Raise());
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
-      AddSequential(new CollectTote());
-      AddSequential(new Lower());
-      AddSequential(new Raise());
-      printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
-      if(SmartDashboard::GetBoolean("Three totes")){
+      if(SmartDashboard::GetBoolean("Two totes")){
+        AddSequential(new Raise());
         AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
         AddSequential(new CollectTote());
         AddSequential(new Lower());
         AddSequential(new Raise());
+        if(SmartDashboard::GetBoolean("Three totes")){
+          AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
+          AddSequential(new CollectTote());
+          AddSequential(new Lower());
+          AddSequential(new Raise());
+        }
       }
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
       AddSequential(new ReleaseTote());
       break;
+    case 5:
+      // Same as auto 4, but navigate around bins
+      //TODO: Implement this
+      break;
+    case 6:
+      // Collect 1 bin and 1 tote
+      //TODO: Implement this
+      break;
+    case 7:
+      // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
+      //TODO: Implement this
+      break;
     default:
       printf("Invalid seq: %d\n", seq);
       break;

Commands/Autonomous/Turn.cpp

@@ -1,7 +1,7 @@
 #include "Turn.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-Turn::Turn(int k) : Command("Turn"){
+Turn::Turn(double k) : Command("Turn"){
   Requires(DentRobot::drivetrain);
   SetTimeout(k);
 }

Commands/Autonomous/Turn.h

@@ -10,7 +10,7 @@ class Turn: public Command{
   private:
     int degrees;
   public:
-    Turn(int);
+    Turn(double);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/BinElevator/BinCloseArms.cpp

@@ -4,11 +4,10 @@
 BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
 }
 void BinCloseArms::Initialize(){
-  //Should never need to use this
   SetTimeout(0.5);
 }
 void BinCloseArms::Execute(){
-  DentRobot::pneumatics->SetOpen(true);
+  DentRobot::pneumatics->SetOpen(false);
 }
 bool BinCloseArms::IsFinished(){
   return true;

Commands/BinElevator/BinLower.cpp

@@ -1,7 +1,7 @@
 #include "BinLower.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinLower::BinLower(float t) : Command("BinLower"){
+BinLower::BinLower(double t) : Command("BinLower"){
   timeout=t;
 }
 void BinLower::Initialize(){

Commands/BinElevator/BinLower.h

@@ -6,9 +6,9 @@
 
 class BinLower: public Command{
   private:
-    float timeout;
+    double timeout;
   public:
-    BinLower(float);
+    BinLower(double);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/BinElevator/BinOpenArms.cpp

@@ -4,7 +4,6 @@
 BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
 }
 void BinOpenArms::Initialize(){
-  //Should never need to use this
   SetTimeout(0.5);
 }
 void BinOpenArms::Execute(){

Commands/BinElevator/BinRaise.cpp

@@ -1,7 +1,7 @@
 #include "BinRaise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinRaise::BinRaise(float t) : Command("BinRaise"){
+BinRaise::BinRaise(double t) : Command("BinRaise"){
   timeout=t;
 }
 void BinRaise::Initialize(){

Commands/BinElevator/BinRaise.h

@@ -6,9 +6,9 @@
 
 class BinRaise: public Command{
   private:
-    float timeout;
+    double timeout;
   public:
-    BinRaise(float);
+    BinRaise(double);
     void Initialize();
     void Execute();
     bool IsFinished();

DentRobot.cpp

@@ -25,14 +25,16 @@ void DentRobot::RobotInit(){
   // Autonomous
   // Sequence of autonomous command
   SmartDashboard::PutNumber("Auto Sequence", 1.0);
-  SmartDashboard::PutNumber("Auto Wait Time", 3.0);
+  SmartDashboard::PutNumber("Auto Wait Time", 2.0);
   // If the robot will be picking up three totes in sequence 3
-  SmartDashboard::PutBoolean("Three totes", true);
+  SmartDashboard::PutBoolean("Two totes", true);
+  SmartDashboard::PutBoolean("Three totes", false);
   // TODO: Calibrate the following two values
   // Distance (in time) to auto zone
   SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
   // Distance (in time) to auto tote (used in sequence 3)
   SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
+  SmartDashboard::PutNumber("Auto Bin Distance", 0.25);
   SmartDashboard::PutNumber("TurnAmount", 1.8);
 
   // Elevators

Subsystems/Drivetrain.cpp

@@ -11,7 +11,7 @@ Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
 void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
-void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
+void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, double gyro){
   double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
   double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
   double correctZ = -z * 0.5;
@@ -25,7 +25,7 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa
 }
 
 //Used in pretest
-void Drivetrain::TestMotor(e_motors motor, float power){
+void Drivetrain::TestMotor(e_motors motor, double power){
   switch(motor){
     case FRONTRIGHT:
       rightFront->Set(power);

Subsystems/Drivetrain.h

@@ -15,9 +15,9 @@ class Drivetrain: public Subsystem{
       BACKLEFT
     };
     void InitDefaultCommand();
-    void DriveMecanum(float, float, float, float, float);
-    void DriveArcade(float, float);
-    void TestMotor(e_motors, float);
+    void DriveMecanum(double, double, double, double, double);
+    void DriveArcade(double, double);
+    void TestMotor(e_motors, double);
 };
 #endif
 // vim: ts=2:sw=2:et