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);
+  SetTimeout(duration);
-  power=5;
+  x=xtmp;
-}
+  y=ytmp;
-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){
+  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
-  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();
 }
 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());
+      // Strafe at .25 power
-      AddSequential(new Turn());
+      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
-      //AddSequential(new Raise());
-      //AddSequential(new Lower());
-      //AddSequential(new Turn());
-      //AddParallel(new AutoDrive(0.5));
-      //AddSequential(new RollIn());
-      //AddSequential(new Turn());
       break;
     case 1:
-      // Drive forward a bit, turn around, collect tote then bin
+      // Drive to Auto Zone (TM)
-      AddSequential(new AutoDrive(0.5));
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new Turn());
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
-      AddSequential(new Turn());
-      AddSequential(new RollIn());
-      AddSequential(new Raise());
       break;
     case 2:
-      // Drive forward a bit, turn around, collect tote then bin
+      // Collect a tote, turn, drive to Auto Zone (TM)
-      AddParallel(new Raise());
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddParallel(new AutoDrive(0.5));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
-      AddSequential(new Turn());
+      AddSequential(new BinRaise(1.2));
-      AddSequential(new Turn());
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
-      AddSequential(new RollIn());
+      AddSequential(new BinLower(1.0));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 3:
-      // Wait a desigated value, drive to Auto Zone (TM)
+      // Collect three totes, drive to Auto Zone (TM)
-      //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(2.0));
+      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/Autonomous/ReleaseTote.cpp

@@ -1,9 +1,9 @@
 #include "ReleaseTote.h"
 #include "../../DentRobot.h"
-#include "RollOut.h"
+#include "AutoDrive.h"
-#include "../Autonomous/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();

Commands/Collector/CollectTote.cpp

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

Commands/Collector/RollIn.cpp

@@ -1,17 +1,17 @@
 #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{
-    DentRobot::collector->MoveRollers(cvt);
+    DentRobot::collector->MoveRollers(cvt*1.5);
   }
 }
 bool RollIn::IsFinished(){

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();

Commands/Collector/RollOut.cpp

@@ -8,7 +8,7 @@ void RollOut::Initialize(){
 void RollOut::Execute(){
   //TODO check this value to move the motors in the right direction
   // Devide by 2 twice because this speed should be half the collector speed
-  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle());
+  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();

Commands/Drivetrain/Drive.cpp

@@ -6,11 +6,11 @@ Drive::Drive() : Command("Drive"){
 void Drive::Initialize(){
 }
 void Drive::Execute(){
-  double x,y,z;
+  double x, y, z;
-  //Code to lock the x axis when not holding button 1
   x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
+  y = -DentRobot::oi->GetLeftStick()->GetRawAxis(1);
   z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
-  y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
+  //Code to lock the x axis when not holding button 1
   //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
   //  x=0;
   //}
@@ -18,7 +18,7 @@ void Drive::Execute(){
   //  y=0;
   //}
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
 }
 bool Drive::IsFinished(){
   return IsTimedOut();

Commands/Elevator/Lower.cpp

@@ -4,7 +4,7 @@
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(3.0);
 }
 void Lower::Execute(){
   DentRobot::elevator->Run(-1.0);

Commands/Elevator/Raise.cpp

@@ -4,7 +4,7 @@
 Raise::Raise() : Command("Raise"){
 }
 void Raise::Initialize(){
-  SetTimeout(3.0);
+  SetTimeout(3.5);
 }
 void Raise::Execute(){
   DentRobot::elevator->Run(1.0);

Commands/Test/CheckDrive.cpp

@@ -0,0 +1,38 @@
+#include "CheckDrive.h"
+#include <cmath>
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){
+  Requires(DentRobot::drivetrain);
+  motor = motorID;
+}
+void CheckDrive::Initialize(){
+  SetTimeout(1.0);
+}
+void CheckDrive::Execute(){
+  switch(motor){
+    case DRIVE_FRONT_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT, 1);
+      break;
+    case DRIVE_FRONT_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT, 1);
+      break;
+    case DRIVE_BACK_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT, 1);
+      break;
+    case DRIVE_BACK_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT, 1);
+      break;
+    default:
+      break;
+  }
+}
+bool CheckDrive::IsFinished(){
+  return false;
+}
+void CheckDrive::End(){
+}
+void CheckDrive::Interrupted(){
+  End();
+}

Commands/Test/CheckDrive.h

@@ -0,0 +1,20 @@
+#ifndef CHECKDRIVE_H
+#define CHECKDRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CheckDrive: public CommandGroup{
+  private:
+    int motor;
+  public:
+    CheckDrive(int);
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif

Commands/Test/CheckRobot.cpp

@@ -0,0 +1,13 @@
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+#include "../Elevator/Raise.h"
+#include "CheckRobot.h"
+#include "CheckDrive.h"
+CheckRobot::CheckRobot(){
+  AddSequential(new CheckDrive(DRIVE_FRONT_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_FRONT_RIGHT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_RIGHT_CAN));
+}
+// vim: ts=2:sw=2:et

Commands/Test/CheckRobot.h

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

DentRobot.cpp

@@ -1,5 +1,6 @@
 #include "DentRobot.h"
 #include "OI.h"
+#include "RobotMap.h"
 #include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
@@ -7,31 +8,53 @@ Drivetrain* DentRobot::drivetrain=NULL;
 Elevator* DentRobot::elevator=NULL;
 BinElevator* DentRobot::binElevator=NULL;
 CommandGroup* DentRobot::aut=NULL;
+Pneumatics* DentRobot::pneumatics=NULL;
 DentRobot::DentRobot(){
   oi=new OI();
   collector=new Collector();
   drivetrain=new Drivetrain();
   elevator=new Elevator();
   binElevator=new BinElevator();
-  aut=new Autonomous(0);
+  pneumatics=new Pneumatics();
-  CameraServer::GetInstance()->SetQuality(25);
+  //CameraServer::GetInstance()->SetQuality(25);
-  CameraServer::GetInstance()->StartAutomaticCapture("cam0");
+  //CameraServer::GetInstance()->StartAutomaticCapture("cam0");
-  //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
+  printf("The robot is on\n");
-  //SmartDashboard::PutNumber("Auto Sequence", 0);
-  printf("Initialized\n");
 }
 void DentRobot::RobotInit(){
-  //SmartDashboard::PutNumber("CodeVersion",1.0);
+  SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
+  // Autonomous
+  // Sequence of autonomous command
+  SmartDashboard::PutNumber("Auto Sequence", 2.0);
+  SmartDashboard::PutNumber("Auto Wait Time", 3.0);
+  // If the robot will be picking up three totes in sequence 3
+  SmartDashboard::PutBoolean("Three totes", true);
+  // TODO: Calibrate the following two values
+  // Distance (in time) to auto zone
+  SmartDashboard::PutNumber("Auto Zone Distance", 2.8);
+  // Distance (in time) to auto tote (used in sequence 3)
+  SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
+  SmartDashboard::PutNumber("TurnAmount", 2.0);
+
+  // Elevators
+  SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
+  SmartDashboard::PutBoolean("Bin Elevator Top", false);
+  SmartDashboard::PutBoolean("Elevator Bottom", false);
+  SmartDashboard::PutBoolean("Elevator Top", false);
+  //Drive speed
+  SmartDashboard::PutNumber("DriveSpeedReductionThresh",2);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::AutonomousInit(){
+  aut=new Autonomous(SmartDashboard::GetNumber("Auto Sequence"));
+  printf("Enabling Auto Sequence %f\n", SmartDashboard::GetNumber("Auto Sequence"));
   if(aut != NULL){
     aut->Start();
   }
 }
 void DentRobot::AutonomousPeriodic(){
+  printf("Running auto.\n");
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){

DentRobot.h

@@ -6,6 +6,7 @@
 #include "Subsystems/BinElevator.h"
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
+#include "Subsystems/Pneumatics.h"
 #include "Commands/Autonomous/Autonomous.h"
 class DentRobot: public IterativeRobot {
   private:
@@ -17,6 +18,7 @@ class DentRobot: public IterativeRobot {
     static Drivetrain* drivetrain;
     static Elevator* elevator;
     static BinElevator* binElevator;
+    static Pneumatics* pneumatics;
     static CommandGroup* aut;
     void RobotInit();
     void DisabledPeriodic();

Makefile

@@ -18,10 +18,10 @@ all : $(OBJECTS)
 clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
-deploy: all
+deploy:
 	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram'
 
-debug: all
+debug:
 	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh'
 
 putkey:

OI.cpp

@@ -5,7 +5,11 @@
 #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"
+#include "Commands/Autonomous/CollectTote.h"
+#include "Commands/Autonomous/ReleaseTote.h"
+#include "Commands/Test/CheckRobot.h"
 OI::OI() {
   // Joysticks
   leftStick=new Joystick(0);
@@ -13,7 +17,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,8 +31,12 @@ OI::OI() {
   // BinElevator
   JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right5=new JoystickButton(rightStick, 5);
-  binRaise=new BinRaise();
+  //JoystickButton *right7=new JoystickButton(rightStick, 7);
-  binLower=new BinLower();
+  //JoystickButton *right8=new JoystickButton(rightStick, 8);
+  //right7->WhenPressed(new BinOpenArms());
+  //right8->WhenPressed(new BinCloseArms());
+  binRaise=new BinRaise(3.0);
+  binLower=new BinLower(2.0);
   right3->WhenPressed(binLower);
   right3->CancelWhenPressed(binRaise);
   right5->WhenPressed(binRaise);
@@ -39,6 +47,10 @@ OI::OI() {
   right12->CancelWhenPressed(lower);
   right12->CancelWhenPressed(binRaise);
   right12->CancelWhenPressed(binLower);
+  // Basic motor test
+  CheckRobot* checkRobot=new CheckRobot();
+  JoystickButton *left7=new JoystickButton(leftStick, 7);
+  left7->WhenPressed(checkRobot);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;

README.md

@@ -0,0 +1,29 @@
+# Dent
+
+Dent was designed to have a fast mecanum [drivetrain](Subsystems/Drivetrain.cpp) with ground clearance to traverse the scoring platforms with ease—all while carrying a stack of totes. A main [internal elevator](Subsystems/Elevator.cpp) lifts totes up to six high within the robot, allowing us to move quickly to anywhere on the field without tipping. The [intake system](Subsystems/Collector.cpp) features a ramp leading to the floor with an active roller pulling the totes up to two collector wheels on either side of the robot, both pulling the totes in, and centering them simultaneously. 
+
+But Dent does not stop there; a [taller elevator](Subsystems/BinElevator.cpp) on the back of the robot allows us to lift either recycle containers or totes to a greater height. With this, we can create stacks both internally and externally, with each system providing a backup of the other in case anything breaks.
+
+Dent is programmed in C++ and uses many sensors to determine what to do. An [ultrasonic sensor](Subsystems/Collector.cpp#L9) mounted on the back of the robot looking forward automatically slows down the collector wheels as the totes fly into the internal elevator. Homemade [hall effect sensors](Subsystems/Elevator.cpp#L6-L8) line the elevator shafts of both elevators, allowing the driver to raise totes and containers to pre-programmed heights. 
+
+All aspects of Dent’s design come together to produce a robot ready to rank in qualifications, and still provide a fast and capable design for elimination rounds. With all parts made an code written for Dent in-house, this truly is a robot designed by, built by, and programmed by the students on Team 2059, [The Hitchhikers](http://team2059.org/).
+
+
+### Controls
+##### Driver Main Joystick (USB 0)
+- X-Axis - Drive forwards and backwards
+- Y-Axis - Strafes left and right
+- Z-Axis - Turns left and right
+- Throttle-Axis - Adjusts collector speed
+- Button 1 - Collects totes
+- Button 2 - Dispenses totes
+- Button 7 - Enable robot test
+
+##### Driver Secondary Joystick (USB 1)
+- Button 3 - Lowers bin elevator
+- Button 4 - Lowers tote elevator
+- Button 5 - Raises bin elevator
+- Button 6 - Raises tote elevator
+- Button 7 - Opens bin arms
+- Button 8 - Closes bin arms
+- Button 12 - Universal cancel button

RobotMap.h

@@ -3,6 +3,8 @@
 
 #include "WPILib.h"
 
+#define CODE_VERSION 1.0
+
 // Elevator
 #define ELEVATOR_CAN 1
 #define ELEVATOR_BOTTOM_DIO 9
@@ -12,23 +14,25 @@
 #define ELEVATOR_ENCODERB 9
 
 // BinElevator
-#define BINELEVATOR_CAN 11
+#define BINELEVATOR_CAN 10
 #define BINELEVATOR_BOTTOM_DIO 6
 #define BINELEVATOR_COLELCT_BIN_DIO 7
-#define BINELEVATOR_TOP_DIO 8
+#define BINELEVATOR_TOP_DIO 12
 #define BINELEVATOR_ENCODERA 10
 #define BINELEVATOR_ENCODERB 11
+#define BINELEVATOR_SOLDENOID_ONE   6
+#define BINELEVATOR_SOLDENOID_TWO   7
 
 // Drivetrain
-#define DRIVE_FRONT_LEFT_CAN 2
+#define DRIVE_FRONT_LEFT_CAN 8
 #define DRIVE_BACK_LEFT_CAN 3
 #define DRIVE_FRONT_RIGHT_CAN 4
 #define DRIVE_BACK_RIGHT_CAN 5
 
 // Collector
 #define COLLECTOR_RAMP_CAN 7
-#define COLLECTOR_LEFT_CAN 8
+#define COLLECTOR_LEFT_CAN 2
-#define COLLECTOR_BOTTOM_CAN 10
+#define COLLECTOR_BOTTOM_CAN 11
 #define COLLECTOR_RIGHT_CAN 9
 #define COLLECTOR_SONAR_ANALOG 3
 

Subsystems/BinElevator.cpp

@@ -2,7 +2,7 @@
 #include "../RobotMap.h"
 BinElevator::BinElevator(){
   motor=new CANTalon(BINELEVATOR_CAN);
-  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false);
+  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA, BINELEVATOR_ENCODERB, false);
   elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO);
   elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO);
 }
@@ -18,9 +18,11 @@ double BinElevator::GetHeight(){
   return elevatorEncoder->Get();
 }
 bool BinElevator::GetElevatorBottom(){
+  SmartDashboard::PutBoolean("Bin Elevator Bottom", elevatorBottom->Get());
   return elevatorBottom->Get();
 }
 bool BinElevator::GetElevatorTop(){
+  SmartDashboard::PutBoolean("Bin Elevator Top", elevatorTop->Get());
   return elevatorTop->Get();
 }
 // vim: ts=2:sw=2:et

Subsystems/Collector.cpp

@@ -15,7 +15,7 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(-a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  printf("Roller power: %f\n",a);
+  printf("Roller power: %f\n", a);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();

Subsystems/Drivetrain.cpp

@@ -12,12 +12,35 @@ void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
 void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
-  double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
+  double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
-  double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
+  double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
-  double correctZ = -z *.5;
+  double correctZ = -z * 0.5;
+  if (DentRobot::oi->GetLeftStick()->GetRawButton(9)){
+    correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
+  }
   rightFront->Set((-correctX + correctY - correctZ));
   leftFront->Set((correctX + correctY + correctZ)*-1);
   rightRear->Set((correctX + correctY - correctZ));
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
+
+//Used in pretest
+void Drivetrain::TestMotor(e_motors motor, float power){
+  switch(motor){
+    case FRONTRIGHT:
+      rightFront->Set(power);
+      break;
+    case FRONTLEFT:
+      leftFront->Set(power);
+      break;
+    case BACKRIGHT:
+      rightRear->Set(power);
+      break;
+    case BACKLEFT:
+      leftRear->Set(power);
+      break;
+    default:
+      break;
+  }
+}
 // vim: ts=2:sw=2:et

Subsystems/Drivetrain.h

@@ -8,9 +8,16 @@ class Drivetrain: public Subsystem{
     RobotDrive *drive;
   public:
     Drivetrain();
+    enum e_motors{
+      FRONTRIGHT,
+      FRONTLEFT,
+      BACKRIGHT,
+      BACKLEFT
+    };
     void InitDefaultCommand();
-    void DriveMecanum(float,float,float,float,float);
+    void DriveMecanum(float, float, float, float, float);
     void DriveArcade(float, float);
+    void TestMotor(e_motors, float);
 };
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -2,7 +2,7 @@
 #include "../RobotMap.h"
 Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
-  elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
+  elevatorEncoder=new Encoder(ELEVATOR_ENCODERA, ELEVATOR_ENCODERB, false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
   elevatorMiddle=new DigitalInput(ELEVATOR_MIDDLE_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
@@ -26,12 +26,14 @@ double Elevator::GetHeight(){
   return elevatorEncoder->Get();
 }
 bool Elevator::GetElevatorBottom(){
+  SmartDashboard::PutBoolean("Elevator Bottom", !elevatorBottom->Get());
   return elevatorBottom->Get();
 }
 bool Elevator::GetElevatorMiddle(){
   return elevatorMiddle->Get();
 }
 bool Elevator::GetElevatorTop(){
+  SmartDashboard::PutBoolean("Elevator Top", !elevatorTop->Get());
   return elevatorTop->Get();
 }
 void Elevator::SetUseEncoder(bool param){

Subsystems/Pneumatics.cpp

@@ -0,0 +1,19 @@
+#include "Pneumatics.h"
+#include "../RobotMap.h"
+
+Pneumatics::Pneumatics() : Subsystem("Pneumatics"){
+  solenoid1 = new Solenoid(BINELEVATOR_SOLDENOID_ONE);
+  solenoid2 = new Solenoid(BINELEVATOR_SOLDENOID_TWO);
+}
+void Pneumatics::InitDefaultCommand(){
+}
+void Pneumatics::SetOpen(bool state){
+  if(state){
+    solenoid1->Set(true);
+    solenoid2->Set(false);
+  }else{
+    solenoid1->Set(false);
+    solenoid2->Set(true);
+  }
+}
+// vim: ts=2:sw=2:et

Subsystems/Pneumatics.h

@@ -0,0 +1,15 @@
+#ifndef PNEUMATICS_H
+#define PNEUMATICS_H
+
+#include "WPILib.h"
+class Pneumatics: public Subsystem
+{
+  private:
+    Solenoid *solenoid1, *solenoid2;
+  public:
+    Pneumatics();
+    void InitDefaultCommand();
+    void SetOpen(bool);
+};
+#endif
+// vim: ts=2:sw=2:et