Merge branch 'develop'

.gitignore

@@ -1,3 +1,7 @@
 *.o
 bin
 wpilib
+CMakeCache.txt
+CMakeFiles
+cmake_install.cmake
+vision

CommandBase.cpp

@@ -3,10 +3,12 @@
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
 #include "Subsystems/BinElevator.h"
+#include "Subsystems/Pneumatics.h"
 Drivetrain* CommandBase::drivetrain = NULL;
 Collector* CommandBase::collector = NULL;
 Elevator* CommandBase::elevator = NULL;
 BinElevator* CommandBase::binElevator = NULL;
+Pneumatics* CommandBase::pneumatics=NULL;
 OI* CommandBase::oi = NULL;
 CommandBase::CommandBase(char const *name) : Command(name) {
 }
@@ -17,6 +19,7 @@ void CommandBase::init(){
   collector = new Collector();
   elevator = new Elevator();
   binElevator = new BinElevator();
+  pneumatics = new Pneumatics();
   oi = new OI();
 }
 // vim: ts=2:sw=2:et

CommandBase.h

@@ -6,6 +6,7 @@
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
 #include "Subsystems/BinElevator.h"
+#include "Subsystems/Pneumatics.h"
 #include "OI.h"
 #include "WPILib.h"
 
@@ -18,6 +19,7 @@ class CommandBase: public Command {
     static Collector *collector;
     static Elevator *elevator;
     static BinElevator *binElevator;
+    static Pneumatics *pneumatics;
     static OI *oi;
 };
 #endif

Commands/Autonomous/AutoDrive.cpp

@@ -1,32 +1,23 @@
 #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 xtmp=0, double ytmp=-0.75) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
-  power=5;
-}
-AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
-  Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
-  power=p;
+  SetTimeout(duration);
+  x=xtmp;
+  y=ytmp;
 }
 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);
-  }
+  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
 }
 void AutoDrive::End(){
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
 }
 void AutoDrive::Interrupted(){
   End();

Commands/Autonomous/AutoDrive.h

@@ -8,10 +8,10 @@
 
 class AutoDrive: public Command{
   private:
-    double power;
+    double x, y;
   public:
     AutoDrive(double);
-    AutoDrive(double, double);
+    AutoDrive(double, double, double);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Autonomous/Autonomous.cpp

@@ -1,45 +1,59 @@
 #include "Autonomous.h"
+#include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
 #include "../Elevator/Raise.h"
 #include "../Elevator/Lower.h"
+#include "../BinElevator/BinRaise.h"
+#include "../BinElevator/BinLower.h"
 #include "AutoDrive.h"
 #include "Turn.h"
 #include "../Collector/RollIn.h"
-#include "../Collector/CollectTote.h"
+#include "CollectTote.h"
+#include "ReleaseTote.h"
 Autonomous::Autonomous(int seq){
   //SmartDashboard::GetNumber("Auto Wait Time");
   switch(seq){
     case 0:
       // Just for testing
-      AddSequential(new CollectTote());
-      AddSequential(new Turn());
-      //AddSequential(new Raise());
-      //AddSequential(new Lower());
-      //AddSequential(new Turn());
-      //AddParallel(new AutoDrive(0.5));
-      //AddSequential(new RollIn());
-      //AddSequential(new Turn());
+      // Strafe at .25 power
+      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
       break;
     case 1:
-      // Drive forward a bit, turn around, collect tote then bin
-      AddSequential(new AutoDrive(0.5));
-      AddSequential(new Turn());
-      AddSequential(new Turn());
-      AddSequential(new RollIn());
-      AddSequential(new Raise());
+      // Drive to Auto Zone (TM)
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
       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());
-      AddSequential(new RollIn());
+      // Collect a tote, turn, drive to Auto Zone (TM)
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
+      AddSequential(new BinRaise(1.2));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
+      AddSequential(new BinLower(1.0));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 3:
-      // Wait a desigated value, drive to Auto Zone (TM)
-      //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(2.0));
+      // Collect three totes, drive to Auto Zone (TM)
+      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")){
+        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;
     default:
       printf("Invalid seq: %d\n", seq);

Commands/Autonomous/CollectTote.cpp

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

Commands/Collector/ReleaseTote.cpp → Commands/Autonomous/ReleaseTote.cpp

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

Commands/Autonomous/Turn.cpp

@@ -1,22 +1,22 @@
 #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);
-  SetTimeout(0.85);
+  SetTimeout(k);
 }
 void Turn::Initialize(){
 }
 void Turn::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9, 0.0);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();
 }
 void Turn::End(){
   // Stop driving
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
 }
 void Turn::Interrupted(){
   End();

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/BinElevator/BinCloseArms.cpp

@@ -0,0 +1,21 @@
+#include "BinCloseArms.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
+}
+void BinCloseArms::Initialize(){
+  //Should never need to use this
+  SetTimeout(0.5);
+}
+void BinCloseArms::Execute(){
+  DentRobot::pneumatics->SetOpen(true);
+}
+bool BinCloseArms::IsFinished(){
+  return true;
+}
+void BinCloseArms::End(){
+}
+void BinCloseArms::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinCloseArms.h

@@ -0,0 +1,18 @@
+#ifndef BINCLOSEARMS_H
+#define BINCLOSEARMS_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinCloseArms: public Command{
+  public:
+    BinCloseArms();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinLower.cpp

@@ -1,10 +1,11 @@
 #include "BinLower.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinLower::BinLower() : Command("BinLower"){
+BinLower::BinLower(float t) : Command("BinLower"){
+  timeout=t;
 }
 void BinLower::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(timeout);
 }
 void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);

Commands/BinElevator/BinLower.h

@@ -5,8 +5,10 @@
 #include "WPILib.h"
 
 class BinLower: public Command{
+  private:
+    float timeout;
   public:
-    BinLower();
+    BinLower(float);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/BinElevator/BinOpenArms.cpp

@@ -0,0 +1,21 @@
+#include "BinOpenArms.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
+}
+void BinOpenArms::Initialize(){
+  //Should never need to use this
+  SetTimeout(0.5);
+}
+void BinOpenArms::Execute(){
+  DentRobot::pneumatics->SetOpen(true);
+}
+bool BinOpenArms::IsFinished(){
+  return true;
+}
+void BinOpenArms::End(){
+}
+void BinOpenArms::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinOpenArms.h

@@ -0,0 +1,18 @@
+#ifndef BINOPENARMS_H
+#define BINOPENARMS_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinOpenArms: public Command{
+  public:
+    BinOpenArms();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinRaise.cpp

@@ -1,10 +1,11 @@
 #include "BinRaise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinRaise::BinRaise() : Command("BinRaise"){
+BinRaise::BinRaise(float t) : Command("BinRaise"){
+  timeout=t;
 }
 void BinRaise::Initialize(){
-  SetTimeout(3.0);
+  SetTimeout(timeout);
 }
 void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);

Commands/BinElevator/BinRaise.h

@@ -5,8 +5,10 @@
 #include "WPILib.h"
 
 class BinRaise: public Command{
+  private:
+    float timeout;
   public:
-    BinRaise();
+    BinRaise(float);
     void Initialize();
     void Execute();
     bool IsFinished();