Merge branch 'develop'

.gitignore

@@ -1,8 +1,9 @@
 *.o
-Debug
 bin
 wpilib
 CMakeCache.txt
 CMakeFiles
 cmake_install.cmake
 vision
+latex
+html

CommandBase.cpp

@@ -1,25 +0,0 @@
-#include "CommandBase.h"
-#include "Subsystems/Drivetrain.h"
-#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) {
-}
-CommandBase::CommandBase() : Command() {
-}
-void CommandBase::init(){
-  drivetrain = new Drivetrain();
-  collector = new Collector();
-  elevator = new Elevator();
-  binElevator = new BinElevator();
-  pneumatics = new Pneumatics();
-  oi = new OI();
-}
-// vim: ts=2:sw=2:et

CommandBase.h

@@ -1,26 +0,0 @@
-#ifndef COMMAND_BASE_H
-#define COMMAND_BASE_H
-
-#include <string>
-#include "Subsystems/Drivetrain.h"
-#include "Subsystems/Collector.h"
-#include "Subsystems/Elevator.h"
-#include "Subsystems/BinElevator.h"
-#include "Subsystems/Pneumatics.h"
-#include "OI.h"
-#include "WPILib.h"
-
-class CommandBase: public Command {
-  public:
-    CommandBase(char const *name);
-    CommandBase();
-    static void init();
-    static Drivetrain *drivetrain;
-    static Collector *collector;
-    static Elevator *elevator;
-    static BinElevator *binElevator;
-    static Pneumatics *pneumatics;
-    static OI *oi;
-};
-#endif
-// vim: ts=2:sw=2:et

Commands/Autonomous/AutoDrive.cpp

@@ -1,23 +1,26 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive(double duration, double xtmp=0, double ytmp=-0.75) : Command("AutoDrive"){
+AutoDrive::AutoDrive(double duration, double xtmp, double ytmp, double ztmp, bool useGyro): Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(duration);
   x=xtmp;
   y=ytmp;
+  z=ztmp;
+  gyro=useGyro;
 }
 void AutoDrive::Initialize(){
+  DentRobot::drivetrain->ResetGyro();
 }
 void AutoDrive::Execute(){
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro);
 }
 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);
 }
 void AutoDrive::Interrupted(){
   End();

Commands/Autonomous/AutoDrive.h

@@ -2,20 +2,53 @@
 #define AUTODRIVE_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Drives the robot without a joystick
+ *
+ * Drives the robot given a timeout and joystick values
+ */
 class AutoDrive: public Command{
   private:
-    double x, y;
+    double x, //<! The x value of the simulated joystick value
+           y, //<! The y value of the simulated joystick value
+           z; //<! The z value of the simulated joystick value
+    bool gyro;
   public:
-    AutoDrive(double);
-    AutoDrive(double, double, double);
+    /**
+     * @brief Constructs AutoDrive
+     *
+     * @param duration Timeout in seconds
+     * @param xtmp Joystick x value (default: 0.0)
+     * @param ytmp Joystick y value (default: 0.75)
+     * @param ztmp Joystick z value (default: 0.0)
+     * @param useGyro Use the gyro when driving
+     */
+    AutoDrive(double duration, double xtmp = 0.0, double ytmp = -0.75, double ztmp = 0.0, bool useGyro = true);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Drives the robot
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the drivetrain to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/Autonomous/Autonomous.cpp

@@ -11,50 +11,81 @@
 #include "CollectTote.h"
 #include "ReleaseTote.h"
 Autonomous::Autonomous(int seq){
-  //SmartDashboard::GetNumber("Auto Wait Time");
+  Wait(SmartDashboard::GetNumber("Auto Wait Time"));
   switch(seq){
     case 0:
       // Just for testing
-      // Strafe at .25 power
-      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
+      // Turn testing
+      AddSequential(new Turn(3.8));
       break;
     case 1:
       // Drive to Auto Zone (TM)
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8, 0.01));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 2:
-      // Collect a tote, turn, drive to Auto Zone (TM)
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      // Lower BinElevator, collect bin, turn, drive to AutoZone (TM)
+      AddSequential(new BinLower(0.5));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Bin Distance"), 0.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)
       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:
-      // 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));
+    case 4:
+      // Collect one, two, or three totes, drive to Auto Zone (TM), release totes
+      AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
+      if(SmartDashboard::GetBoolean("Two totes")){
+        AddParallel(new Turn(0.81));
+        AddSequential(new Raise(3.5));
+        AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
         AddSequential(new CollectTote());
-        AddSequential(new Lower());
-        AddSequential(new Raise());
+        AddSequential(new Lower(3.0));
+        AddSequential(new Raise(3.5));
+        if(SmartDashboard::GetBoolean("Three totes")){
+          AddSequential(new Turn(3.8));
+          AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75));
+          AddSequential(new CollectTote());
+          AddSequential(new Lower(3.0));
+          AddSequential(new Raise(3.5));
+        }
       }
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.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;
+    case 8:
+      //Use rear elevator to move tote
+      AddSequential(new Turn(1.8));
+      AddSequential(new AutoDrive(2.3,0.0,-0.75));
+      AddSequential(new Turn(1.8));
+      break;
+    case 9:
+      //Use rear elevator to move bin
+      AddSequential(new BinLower(.1));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.0,0.75));
+      AddSequential(new Turn(2.1));
+      break;
     default:
       printf("Invalid seq: %d\n", seq);
       break;

Commands/Autonomous/Autonomous.h

@@ -2,13 +2,32 @@
 #define AUTONOMOUS_H
 
 #include "Commands/CommandGroup.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief The autonomous period of the robot
+ *
+ * Contains three sequences selectable from the SmartDashboard
+ * All sequences will wait for the SmartDashboard value "Auto Wait Time"
+ *
+ * Sequence 0: Used for testing only
+ *
+ * Sequence 1: Drives forward "Auto Zone Distance" at -0.8 power
+ *
+ * Sequence 2: Collects a tote, turns, then drives to the auto zone
+ *
+ * Sequence 3: Collects two or three totes then drives to auto zone
+ */
 class Autonomous: public CommandGroup{
   public:
-    Autonomous(int);
+    /**
+     * @brief Constructs Autonomous
+     *
+     * @param seq The sequence to run
+     */
+    Autonomous(int seq=0);
 };
 #endif
 // vim: ts=2:sw=2:et

Commands/Autonomous/CollectTote.cpp

@@ -2,8 +2,8 @@
 #include "../../DentRobot.h"
 #include "AutoDrive.h"
 #include "../Collector/RollIn.h"
-CollectTote::CollectTote(){
-  AddParallel(new AutoDrive(1.0, -0.75, 0.0));
+CollectTote::CollectTote(double z){
+  AddParallel(new AutoDrive(SmartDashboard::GetNumber("DriveTime"), 0.0, 0.75, z, false));
   AddSequential(new RollIn(1.0));
 }
 // vim: ts=2:sw=2:et

Commands/Autonomous/CollectTote.h

@@ -2,13 +2,23 @@
 #define COLLECTTOTE_H
 
 #include "Commands/CommandGroup.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Collects one tote
+ *
+ * Rolls collector wheels in and drives forward in parallel
+ */
 class CollectTote: public CommandGroup{
   public:
-    CollectTote();
+    /**
+     * @brief Constructs CollectTote
+     * 
+     * @param z Joystick z value (default: 0.0)
+     */
+    CollectTote(double z = 0.0);
 };
 #endif
 // vim: ts=2:sw=2:et

Commands/Autonomous/ReleaseTote.cpp

@@ -3,7 +3,7 @@
 #include "AutoDrive.h"
 #include "../Collector/RollOut.h"
 ReleaseTote::ReleaseTote(){
-  AddParallel(new RollOut());
-  AddParallel(new AutoDrive(0.5, 0, 0.75));
+  AddParallel(new RollOut(2.0));
+  AddParallel(new AutoDrive(0.5, 0.0, -0.75));
 }
 // vim: ts=2:sw=2:et

Commands/Autonomous/ReleaseTote.h

@@ -2,12 +2,20 @@
 #define RELEASETOTE_H
 
 #include "Commands/CommandGroup.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Releases one tote
+ *
+ * Rolls collector wheels out and drives backwards in parallel
+ */
 class ReleaseTote: public CommandGroup{
   public:
+    /**
+     * @brief Constructs ReleaseTote
+     */
     ReleaseTote();
 };
 #endif

Commands/Autonomous/Turn.cpp

@@ -1,22 +1,21 @@
 #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 timeout): Command("Turn"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(k);
+  SetTimeout(timeout);
 }
 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, 0.6, 0.9, 0.0);
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9);
 }
 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);
 }
 void Turn::Interrupted(){
   End();

Commands/Autonomous/Turn.h

@@ -2,19 +2,45 @@
 #define TURN_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Turns the robot
+ *
+ * Turns the robot until a timeout is reached
+ */
 class Turn: public Command{
   private:
-    int degrees;
   public:
-    Turn(int);
+    /**
+     * @brief Constructs Turn
+     *
+     * @param timeout Timeout in seconds
+     */
+    Turn(double timeout);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Turns the robot
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the drivetrain to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/BinElevator/BinCloseArms.cpp

@@ -1,14 +1,13 @@
 #include "BinCloseArms.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
+BinCloseArms::BinCloseArms(double timeout): Command("BinCloseArms"){
+  SetTimeout(timeout);
 }
 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/BinCloseArms.h

@@ -4,13 +4,40 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Closes BinElevatorArms (NOT USED)
+ *
+ * Sets the solenoid to close the arms of the BinElevator
+ */
 class BinCloseArms: public Command{
   public:
-    BinCloseArms();
+    /**
+     * @brief Constructs BinCloseArms
+     *
+     * @param timeout The timeout
+     */
+    BinCloseArms(double timeout = 0.5);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Sets the solenoid to close the arms
+     */
     void Execute();
+    /**
+     * @brief Returns true to prevent solenoid damage
+     *
+     * @return True
+     */
     bool IsFinished();
+    /**
+     * @brief Ends the command
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 

Commands/BinElevator/BinLower.cpp

@@ -1,17 +1,16 @@
 #include "BinLower.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinLower::BinLower(float t) : Command("BinLower"){
-  timeout=t;
+BinLower::BinLower(float timeout): Command("BinLower"){
+  SetTimeout(timeout);
 }
 void BinLower::Initialize(){
-  SetTimeout(timeout);
 }
 void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);
 }
 bool BinLower::IsFinished(){
-  if (/*!DentRobot::binElevator->GetElevatorBottom()||*/IsTimedOut()){
+  if(/*!DentRobot::binElevator->GetElevatorBottom()||*/IsTimedOut()){
     printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
     return true;
   }else{

Commands/BinElevator/BinLower.h

@@ -4,15 +4,39 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Lowers the bin elevator until a timeout is reached
+ */
 class BinLower: public Command{
   private:
-    float timeout;
   public:
-    BinLower(float);
+    /**
+     * @brief Constructs BinLower
+     *
+     * @param timeout The timeout
+     */
+    BinLower(float timeout);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Lowers the bin elevator at -1.0 power
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the bin elevator to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/BinElevator/BinOpenArms.cpp

@@ -1,11 +1,10 @@
 #include "BinOpenArms.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
+BinOpenArms::BinOpenArms(double timeout): Command("BinOpenArms"){
+  SetTimeout(timeout);
 }
 void BinOpenArms::Initialize(){
-  //Should never need to use this
-  SetTimeout(0.5);
 }
 void BinOpenArms::Execute(){
   DentRobot::pneumatics->SetOpen(true);

Commands/BinElevator/BinOpenArms.h

@@ -4,15 +4,43 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Opens bin arms (unused)
+ */
 class BinOpenArms: public Command{
   public:
+    /**
+     * @brief Constructs BinOpenArms
+     *
+     * @param timeout The timeout
+     */
+    BinOpenArms(double timeout);
+    /**
+     * @brief Constructs BinOpenArms
+     */
     BinOpenArms();
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Sets the solenoid to open the arms
+     */
     void Execute();
+    /**
+     * @brief Returns true to prevent solenoid damage
+     *
+     * @return True
+     */
     bool IsFinished();
+    /**
+     * @brief Ends the command
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
-
 #endif
 // vim: ts=2:sw=2:et

Commands/BinElevator/BinRaise.cpp

@@ -1,17 +1,16 @@
 #include "BinRaise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinRaise::BinRaise(float t) : Command("BinRaise"){
-  timeout=t;
+BinRaise::BinRaise(double timeout): Command("BinRaise"){
+  SetTimeout(timeout);
 }
 void BinRaise::Initialize(){
-  SetTimeout(timeout);
 }
 void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);
 }
 bool BinRaise::IsFinished(){
-  if (/*!DentRobot::binElevator->GetElevatorTop()||*/IsTimedOut()){
+  if(/*!DentRobot::binElevator->GetElevatorTop()||*/IsTimedOut()){
     printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
     return true;
   }else{

Commands/BinElevator/BinRaise.h

@@ -4,17 +4,40 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Raises the bin elevator until a timeout is reached
+ */
 class BinRaise: public Command{
   private:
-    float timeout;
   public:
-    BinRaise(float);
+    /**
+     * @brief Constructs BinRaise
+     *
+     * @param timeout Timeout in seconds
+     */
+    BinRaise(double timeout);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Raises the bin elevator at 1.0 power
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the bin elevator to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
-
 #endif
 // vim: ts=2:sw=2:et

Commands/Collector/RollIn.cpp

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

Commands/Collector/RollIn.h

@@ -2,21 +2,45 @@
 #define ROLLIN_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Rolls collector motors in until a timeout is reached
+ */
 class RollIn: public Command{
   private:
-    double rawSpeed;
+    double rawSpeed; //<! Raw speed of the collector
   public:
-    RollIn(double);
+    /**
+     * @brief Constructs RollIn
+     *
+     * @param speed Speed to roll the collector
+     */
+    RollIn(double speed);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Rolls the four collector motors in
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the collector to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
-
 #endif
 // vim: ts=2:sw=2:et

Commands/Collector/RollOut.cpp

@@ -1,15 +1,14 @@
 #include "RollOut.h"
-RollOut::RollOut() : Command("RollOut"){
+RollOut::RollOut(double timeout): Command("RollOut"){
   Requires(DentRobot::collector);
+  SetTimeout(timeout);
 }
 void RollOut::Initialize(){
-  SetTimeout(2.0);
 }
 void RollOut::Execute(){
-  //TODO: figure out how to implement this with an Xbox controller
-  //DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
-  //Will just move at .8 power for now
-  DentRobot::collector->MoveRollers(-0.8);
+  // Divide by 2 twice because this speed should be half the collector speed
+  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
+  SmartDashboard::PutNumber("DriveThrottle", -DentRobot::oi->GetLeftThrottle());
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();

Commands/Collector/RollOut.h

@@ -2,19 +2,43 @@
 #define ROLLOUT_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Rolls collector motors out until a timeout is reached
+ */
 class RollOut: public Command{
   public:
-    RollOut();
+    /**
+     * @brief Constructs RollOut
+     *
+     * @param timeout The timeout
+     */
+    RollOut(double timeout=2.0);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Rolls the four collector motors out
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True only if the timeout was reached
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the collector to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
-
 #endif
 // vim: ts=2:sw=2:et

Commands/Drivetrain/Drive.cpp

@@ -1,6 +1,6 @@
 #include "Drive.h"
 #include "../../DentRobot.h"
-Drive::Drive() : Command("Drive"){
+Drive::Drive(): Command("Drive"){
   Requires(DentRobot::drivetrain);
 }
 void Drive::Initialize(){
@@ -10,15 +10,19 @@ void Drive::Execute(){
   x = DentRobot::oi->GetLeftAxis("left", "x");
   y = DentRobot::oi->GetLeftAxis("left", "y");
   z = DentRobot::oi->GetLeftAxis("right", "x");
-  //Code to lock the x axis when not holding button 1
-  //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
+  // Lock the x axis when not holding button 1
+  //if(DentRobot::oi->GetLeftStick()->GetRawButton(1)){
   //  x=0;
   //}
-  //if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
+  //if(DentRobot::oi->GetLeftStick()->GetRawButton(2)){
   //  y=0;
   //}
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
+  if(DentRobot::oi->GetLeftStick()->GetRawButton(11)){
+    x = -x;
+    y = -y;
+  }
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9);
 }
 bool Drive::IsFinished(){
   return IsTimedOut();

Commands/Drivetrain/Drive.h

@@ -2,17 +2,42 @@
 #define DRIVE_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief Drives the robot with a joystick
+ *
+ * Uses mecanum drive
+ */
 class Drive: public Command{
   public:
+    /**
+     * @brief Constructs Drive
+     */
     Drive();
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Drives the robot with joysticks from OI
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return False
+     */
     bool IsFinished();
+    /**
+     * @brief Ends the command
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/Elevator/Lower.cpp

@@ -1,17 +1,17 @@
 #include "Lower.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-Lower::Lower() : Command("Lower"){
+Lower::Lower(double timeout): Command("Lower"){
+  SetTimeout(timeout);
 }
 void Lower::Initialize(){
-  SetTimeout(3.0);
 }
 void Lower::Execute(){
   DentRobot::elevator->Run(-1.0);
 }
 bool Lower::IsFinished(){
-  if (!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){
-    printf("Robot stoped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom());
+  if(!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stopped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom());
     return true;
   }else{
     return false;

Commands/Elevator/Lower.h

@@ -4,13 +4,36 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Lowers the elevator until a timeout is reached
+ */
 class Lower: public Command{
   public:
-    Lower();
+    /**
+     * @brief Constructs Lower
+     */
+    Lower(double timeout=3.0);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Lowers the elevator at -1.0 power
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True if the timeout was reached or if the bottom elevator sensor was triggered
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the elevator to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/Elevator/Raise.cpp

@@ -1,10 +1,10 @@
 #include "Raise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-Raise::Raise() : Command("Raise"){
+Raise::Raise(double timeout): Command("Raise"){
+  SetTimeout(timeout);
 }
 void Raise::Initialize(){
-  SetTimeout(3.5);
 }
 void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
@@ -13,12 +13,12 @@ bool Raise::IsFinished(){
   //if(!DentRobot::elevator->GetElevatorMiddle()){
   //  DentRobot::elevator->stoppedAtSensor=true;
   //}
-  //if ((DentRobot::elevator->stoppedAtSensor)){
+  //if((DentRobot::elevator->stoppedAtSensor)){
   //  printf("Stopped at the middle sensor\n");
   //  DentRobot::elevator->stoppedAtSensor=false;
   //  return true;
-  //}else if (!DentRobot::elevator->GetElevatorTop()) {
-  if (!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
+  //}else if(!DentRobot::elevator->GetElevatorTop()){
+  if(!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
     printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle());
     return true;
   }else{

Commands/Elevator/Raise.h

@@ -4,15 +4,37 @@
 #include "Commands/Command.h"
 #include "WPILib.h"
 
+/**
+ * @brief Raises the elevator until a timeout is reached
+ */
 class Raise: public Command{
   public:
-    Raise();
+    /**
+     * @brief Constructs Raise
+     */
+    Raise(double timeout=3.5);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief Raises the elevator at 1.0 power
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return True if the timeout was reached or if the top elevator was triggered or if the middle elevator is triggered
+     */
     bool IsFinished();
+    /**
+     * @brief Sets the elevator to stop
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
-
 #endif
 // vim: ts=2:sw=2:et

Commands/Test/CheckDrive.cpp

@@ -3,7 +3,7 @@
 #include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
 #include "../../RobotMap.h"
-CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){
+CheckDrive::CheckDrive(int motorID): CommandGroup("CheckDrive"){
   Requires(DentRobot::drivetrain);
   motor = motorID;
 }

Commands/Test/CheckDrive.h

@@ -2,19 +2,44 @@
 #define CHECKDRIVE_H
 
 #include "Commands/Command.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief TODO
+ */
 class CheckDrive: public CommandGroup{
   private:
-    int motor;
+    int motor; //<! TODO
   public:
+    /**
+     * @brief TODO
+     *
+     * @param motorID
+     */
     CheckDrive(int);
+    /**
+     * @brief Initializes the class
+     */
     void Initialize();
+    /**
+     * @brief TODO
+     */
     void Execute();
+    /**
+     * @brief Checks if the command is finished
+     *
+     * @return TODO
+     */
     bool IsFinished();
+    /**
+     * @brief TODO
+     */
     void End();
+    /**
+     * @brief Calls End()
+     */
     void Interrupted();
 };
 #endif

Commands/Test/CheckRobot.h

@@ -2,12 +2,18 @@
 #define CHECKROBOT_H
 
 #include "Commands/CommandGroup.h"
-#include "../../CommandBase.h"
+#include "../../DentRobot.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
+/**
+ * @brief TODO
+ */
 class CheckRobot: public CommandGroup{
   public:
+    /**
+     * @brief TODO
+     */
     CheckRobot();
 };
 #endif

DentRobot.cpp

@@ -23,25 +23,33 @@ DentRobot::DentRobot(){
 void DentRobot::RobotInit(){
   SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
   // Autonomous
+  // Calibration
+  // Amount to turn while collecting the initial tote in auto 4
+  SmartDashboard::PutNumber("CollectToteTurn", 0.25);
+  // Amount of time to collect a tote
+  SmartDashboard::PutNumber("DriveTime", 1.3);
   // Sequence of autonomous command
-  SmartDashboard::PutNumber("Auto Sequence", 2.0);
-  SmartDashboard::PutNumber("Auto Wait Time", 3.0);
+  SmartDashboard::PutNumber("Auto Sequence", 9.0);
+  SmartDashboard::PutNumber("Auto Wait Time", 0.5);
   // If the robot will be picking up three totes in sequence 3
-  SmartDashboard::PutBoolean("Three totes", true);
-  // TODO: Calibrate the following two values
+  SmartDashboard::PutBoolean("Two totes", false);
+  SmartDashboard::PutBoolean("Three totes", false);
   // Distance (in time) to auto zone
-  SmartDashboard::PutNumber("Auto Zone Distance", 2.8);
+  SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
   // Distance (in time) to auto tote (used in sequence 3)
+  SmartDashboard::PutNumber("Two Tote Distance", 1.0);
+  SmartDashboard::PutNumber("Three Tote Distance", 2.5);
   SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
-  SmartDashboard::PutNumber("TurnAmount", 2.0);
-
+  SmartDashboard::PutNumber("TurnAmount", 2.6);
   // 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);
+  SmartDashboard::PutNumber("DriveSpeedReductionThresh", 2.0);
+  //Gyro
+  SmartDashboard::PutNumber("Gyro kP", -0.02);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
@@ -58,13 +66,13 @@ void DentRobot::AutonomousPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){
-  if (aut != NULL){
+  if(aut != NULL){
     aut->Cancel();
   }
 }
 void DentRobot::TeleopPeriodic(){
   Scheduler::GetInstance()->Run();
-  // TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
+  //TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
   if(elevator->GetUseEncoder()&&elevator->GetHeight()<=-1.0){
     // Raise the elevator if it dips below elevatorTop
     oi->raise->Start();

DentRobot.h

@@ -8,24 +8,77 @@
 #include "Subsystems/Collector.h"
 #include "Subsystems/Pneumatics.h"
 #include "Commands/Autonomous/Autonomous.h"
-class DentRobot: public IterativeRobot {
+/**
+ * @brief The Hitchhikers 2015 robot, Dent
+ *
+ * Features a 4-motor collector, 4-motor mecanum drivetrain, two one-motor elevators
+ */
+class DentRobot: public IterativeRobot{
   private:
+    /**
+     * @brief The default driving command
+     */
     Command *driveCommand = NULL;
   public:
+    /**
+     * @brief Constructs DentRobot
+     */
     DentRobot();
+    /**
+     * @brief The 2-joystick OI
+     */
     static OI* oi;
+    /**
+     * @brief The 4-motor Collector
+     */
     static Collector* collector;
+    /**
+     * @brief The 4-motor mecanum Drivetrain
+     */
     static Drivetrain* drivetrain;
+    /**
+     * @brief The main one-motor Elevator for lifting totes
+     */
     static Elevator* elevator;
+    /**
+     * @brief The back one-motor Elevator for lifting totes or bins
+     */
     static BinElevator* binElevator;
+    /**
+     * @brief The Pneumatics system (unused)
+     */
     static Pneumatics* pneumatics;
+    /**
+     * @brief The Autonomous command
+     */
     static CommandGroup* aut;
+    /**
+     * @brief Initializes the robot
+     */
     void RobotInit();
+    /**
+     * @brief Periodically run when disabled
+     */
     void DisabledPeriodic();
+    /**
+     * @brief Initializes the autonomous period
+     */
     void AutonomousInit();
+    /**
+     * @brief Periodically run when enabled in autonomous
+     */
     void AutonomousPeriodic();
+    /**
+     * @brief Initializes the teleop period
+     */
     void TeleopInit();
+    /**
+     * @brief Periodically run when enabled in autonomous
+     */
     void TeleopPeriodic();
+    /**
+     * @brief Periodically run when enabled in test mode
+     */
     void TestPeriodic();
 };
 #endif

Makefile

@@ -6,7 +6,8 @@ SOURCES=$(shell find -type f -name "*.cpp")
 OBJECTS=$(SOURCES:.cpp=.o)
 WPILIB=/var/frc/wpilib
 EXEC=bin/FRCUserProgram
-CLEANSER=rm -r
+CLEANSER=rm -f
+READER=$(shell which pv||which cat)
 
 all : $(OBJECTS)
 	if [ ! -d bin ];then mkdir bin; fi
@@ -19,13 +20,10 @@ clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
 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'
+		@$(READER) bin/FRCUserProgram | ssh admin@$(REMOTEIP) '(rm /home/lvuser/FRCUserProgram)</dev/null;cat > /home/lvuser/FRCUserProgram;chmod a+x /home/lvuser/FRCUserProgram && /usr/local/frc/bin/frcKillRobot.sh -r -t'
 
 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'
+		@$(READER) bin/FRCUserProgram | ssh admin@$(REMOTEIP) '(rm /home/lvuser/FRCUserProgram)</dev/null;cat > /home/lvuser/FRCUserProgram;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh'
 
 putkey:
 	@test -d ~/.ssh||mkdir ~/.ssh;test -f ~/.ssh/id_rsa||ssh-keygen -t rsa -f ~/.ssh/id_rsa -b 4096 -q -N '';cat ~/.ssh/id_rsa.pub|ssh -v admin@$(REMOTEIP) 'cat >> /tmp/key;mkdir -p ~/.ssh;cat /tmp/key >> ~/.ssh/authorized_keys;rm /tmp/key'
-
-updatemakefile:
-	@curl -s https://raw.githubusercontent.com/int0x191f2/nameless/master/configure.sh | sh

OI.cpp

@@ -10,7 +10,7 @@
 #include "Commands/Autonomous/CollectTote.h"
 #include "Commands/Autonomous/ReleaseTote.h"
 #include "Commands/Test/CheckRobot.h"
-OI::OI() {
+OI::OI(){
   // Joysticks
   leftController=new Joystick(0);
   rightStick=new Joystick(1);

OI.h

@@ -5,13 +5,18 @@
 #include "string"
 #include "Commands/Command.h"
 
-class OI
-{
+/**
+ * @brief Controls the robot with joysticks
+ */
+class OI{
   private:
     Joystick *leftController, *rightStick;
     // Named Xbox buttons
     JoystickButton *leftA, *leftB, *leftX, *leftY, *leftLB, *leftRB, *leftBack, *leftStart, *leftLPress, *leftRPress;
   public:
+    /**
+     * @brief Constructs OI
+     */
     OI();
     Command *raise, *lower, *binLower, *binRaise;
     float GetLeftAxis(std::string, std::string);

README.md

@@ -1,29 +1,75 @@
 # Dent
+[The Hitchhikers](http://team2059.org) 2015 robot
 
-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. 
+### Features
+#### Subsystems
++ [Mecanum drivetrain](Subsystems/Drivetrain.cpp)
++ [Collector](Subsystems/Collector.cpp) to collect totes in main elevator
++ [Internal elevator](Subsystems/Elevator.cpp) for totes
++ [External elevator](Subsystems/BinElevator.cpp) for bins or totes
++ [Pneumatics](Subsystems/Pneumatics.cpp) for opening/closing bin elevator arms (unused)
 
-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.
+#### Sensors
++ [Ultrasonic](Subsystems/Collector.cpp#L9) to check if a tote is in the robot (unused)
++ [hall effect sensors](Subsystems/Elevator.cpp#L6-L8) to check the elevator's position
 
-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. 
+#### Automated Commands
++ [AutoDrive](Commands/Autonomous/AutoDrive.cpp) to drive forward without a joystick
++ [CollectTote](Commands/Autonomous/CollectTote.cpp) to drive forwards and roll in collectors in parallel
++ [ReleaseTote](Commands/Autonomous/ReleaseTote.cpp) to drive backwards and roll out collectors in parallel
++ [Turn](Commands/Autonomous/Turn.cpp) to turn the robot
 
-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/).
+### Running the code
+#### Setup (for Linux)
++ Make sure you have the [toolchain](http://first.wpi.edu/FRC/roborio/toolchains/) installed
++ Edit the path of WPILib and the REMOTEIP on the Makefile
++ Run `make putkey` to put the public key on the rrio for faster deploying (optional)
 
+#### Building
++ Run `make && make deploy`
++ Run `ssh admin@rrio-ip.local '/home/lvuser/FRCUserProgram'` to execute the program
 
-### 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
+### Usage
+#### Left Joystick (USB 0)
++ X-Axis - Drive forwards and backwards
++ Y-Axis - Strafes left and right
++ Z-Axis - Turns left and right
++ Throttle - Adjusts collector speed
++ Button 1 - Collect totes
++ Button 2 - Eject totes
++ Button 7 - Check robot
 
-##### 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
+#### Right Joystick (USB 1)
++ Button 3 - Lowers bin elevator
++ Button 4 - Lowers main elevator
++ Button 5 - Raises bin elevator
++ Button 6 - Raises main elevator
++ Button 7 - Opens bin arms (unused)
++ Button 8 - Closes bin arms (unused)
++ Button 12 - Cancel raising and lowering for both elevators
+
+### Dashboard
+#### Configuration
++ CodeVersion - The current version of the code
++ Auto Wait Time - The amount of time to wait for any autonomous to run (default: 2.0)
++ Two totes - Collect a second tote if using Auto Sequence 4 or 5 (default: true)
++ Three totes - Collect a third tote if using Auto Sequence 4 or 5 (default: false)
++ Auto Zone Distance - Amount of time in seconds to drive to the auto zone (default: 2.1)
++ Auto Tote Distance - Amount of time in seconds to drive to a second or third tote if using Auto Sequence 4, 5, or 7 (default: 0.5)
++ Auto Bin Distance - Amount of time in seconds to drive to a bin if using Auto Sequence 6 or 7 (default: 0.5)
++ TurnAmount - Amount of time in seconds to turn the robot (default: 1.8)
++ Bin Elevator Bottom - Status of the bottom bin elevator sensor (unused)
++ Bin Elevator Top - Status of the top bin elevator sensor (unused)
++ Elevator Bottom - Status of the bottom elevator sensor
++ Elevator Top - Status of the top elevator sensor
++ DriveSpeedReductionThresh - Maximum y value of the joystick when driving
++ Auto Sequence - The sequence of autonomous to run
+
+#### Autonomous
+1. Drive to auto zone, turn
+2. Lower BinElevator, collect bin, turn, drive to AutoZone, turn
+3. Raise BinElevator, turn, drive to AutoZone, turn
+4. Collect 1, 2, or 3 totes, turn, drive to AutoZone, turn
+5. Same as auto 4, but navigate around bins (not implemented)
+6. Collect 1 bin then 1 tote (not implemented)
+7. Collect 3 totes, collect bin, drive to AutoZone (not implemented)

RobotMap.h

@@ -3,7 +3,7 @@
 
 #include "WPILib.h"
 
-#define CODE_VERSION 1.0
+#define CODE_VERSION 2.0
 
 // Elevator
 #define ELEVATOR_CAN 1
@@ -28,6 +28,7 @@
 #define DRIVE_BACK_LEFT_CAN 3
 #define DRIVE_FRONT_RIGHT_CAN 4
 #define DRIVE_BACK_RIGHT_CAN 5
+#define DRIVE_GYRO_ANALOG 0
 
 // Collector
 #define COLLECTOR_RAMP_CAN 7

Subsystems/BinElevator.h

@@ -3,19 +3,53 @@
 
 #include "WPILib.h"
 #include "Commands/PIDSubsystem.h"
+/**
+ * @brief Controls the bin elevator
+ */
 class BinElevator{
   private:
-    CANTalon *motor;
-    Encoder *elevatorEncoder;
-    static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    DigitalInput *elevatorBottom, *elevatorTop;
+    CANTalon *motor; //<! The bin elevator motor
+    Encoder *elevatorEncoder; //<! The bin elevator encoder (unused)
+    DigitalInput *elevatorBottom, //<! The bottom bin elevator sensor (unused)
+                 *elevatorTop; //<! The top bin elevator sensor (unused)
   public:
+    /**
+     * @brief Constructs BinElevator
+     */
     BinElevator();
+    /**
+     * @brief No action
+     */
     void InitDefaultCommand();
-    void Run(double);
+    /**
+     * @brief Runs the bin elevator
+     *
+     * @param power The power to run the bin elevator
+     * 
+     * Ranges from -1.0 to 1.0
+     */
+    void Run(double power);
+    /**
+     * @brief Sets the encoder value to 0 (unused)
+     */
     void ResetEncoder();
+    /**
+     * @brief Gets the height of the bin elevator
+     *
+     * @return The height of the bin elevator
+     */
     double GetHeight();
+    /**
+     * @brief Gets the status of the top sensor
+     *
+     * @return True if the sensor is activated
+     */
     bool GetElevatorTop();
+    /**
+     * @brief Gets the status of the bottom sensor
+     *
+     * @return True if the sensor is activated
+     */
     bool GetElevatorBottom();
 };
 #endif

Subsystems/Collector.cpp

@@ -1,7 +1,7 @@
 #include "Collector.h"
 #include "../RobotMap.h"
 
-Collector::Collector() : Subsystem("Collector"){
+Collector::Collector(): Subsystem("Collector"){
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
   collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
@@ -10,12 +10,12 @@ Collector::Collector() : Subsystem("Collector"){
 }
 void Collector::InitDefaultCommand(){
 }
-void Collector::MoveRollers(double a){
-  collectorMotorLeft->Set(a);
-  collectorMotorBottom->Set(-a);
-  collectorMotorRamp->Set(a);
-  collectorMotorRight->Set(-a);
-  printf("Roller power: %f\n", a);
+void Collector::MoveRollers(double power){
+  collectorMotorLeft->Set(power);
+  collectorMotorBottom->Set(-power);
+  collectorMotorRamp->Set(power);
+  collectorMotorRight->Set(-power);
+  printf("Roller power: %f\n", power);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();

Subsystems/Collector.h

@@ -2,16 +2,45 @@
 #define COLLECTOR_H
 
 #include "WPILib.h"
-class Collector: public Subsystem
-{
+/**
+ * @brief Collects totes
+ * 
+ * Uses four motors, two on the sides, one on the bottom, and one on the ramp to collect and eject totes
+ */
+class Collector: public Subsystem{
   private:
-    CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRamp, *collectorMotorRight;
+    CANTalon *collectorMotorLeft,   //<! Left collector motor
+             *collectorMotorBottom, //<! Bottom collector motor
+             *collectorMotorRamp,   //<! Ramp collector motor
+             *collectorMotorRight;  //<! Right collector motor
+    /**
+     * @brief Analog input for sonar (unused)
+     */
     AnalogInput *sonarAnalog;
+    /**
+     * @brief Digital output for sonar (unused)
+     */
     DigitalOutput *sonarDigital;
   public:
+    /**
+     * @brief Constructs Collector
+     */
     Collector();
+    /**
+     * @brief No action
+     */
     void InitDefaultCommand();
-    void MoveRollers(double);
+    /**
+     * @brief Moves the collectors
+     *
+     * @param power The speed to run the collectors
+     */
+    void MoveRollers(double power);
+    /**
+     * @brief Gets the distance of the sonar (unused)
+     *
+     * @return The sonar distance
+     */
     double GetSonarDistance();
 };
 #endif

Subsystems/Drivetrain.cpp

@@ -2,20 +2,28 @@
 #include "../RobotMap.h"
 #include "../Commands/Drivetrain/Drive.h"
 
-Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
+Drivetrain::Drivetrain(): Subsystem("Drivetrain"){
   rightFront = new CANTalon(DRIVE_FRONT_RIGHT_CAN);
   leftFront = new CANTalon(DRIVE_FRONT_LEFT_CAN);
   rightRear = new CANTalon(DRIVE_BACK_RIGHT_CAN);
   leftRear = new CANTalon(DRIVE_BACK_LEFT_CAN);
+  gyro = new Gyro(DRIVE_GYRO_ANALOG);
 }
 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, bool driveStraight){
+  //TODO: Find the correct value for kP
+  double kP=SmartDashboard::GetNumber("Gyro kP");
   double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
   double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
-  double correctZ = -z * 0.5;
-  // If they're holding the right button, slow down
+  double correctZ;
+  if(driveStraight){
+    printf("Driving straight at: %f\n", -gyro->GetAngle()*kP);
+    correctZ = -gyro->GetAngle()*kP;
+  }else{
+    correctZ = -z * 0.5;
+  }
   if (DentRobot::oi->GetLeftButton("rb")){
     correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
   }
@@ -24,9 +32,8 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa
   rightRear->Set((correctX + correctY - correctZ));
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
-
 //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);
@@ -44,4 +51,7 @@ void Drivetrain::TestMotor(e_motors motor, float power){
       break;
   }
 }
+void Drivetrain::ResetGyro(){
+  gyro->Reset();
+}
 // vim: ts=2:sw=2:et

Subsystems/Drivetrain.h

@@ -2,22 +2,57 @@
 #define DRIVETRAIN_H
 
 #include "WPILib.h"
+/**
+ * @brief Drives the robot
+ * 
+ * 4 wheel mecanum drive
+ */
 class Drivetrain: public Subsystem{
   private:
-    CANTalon *rightFront, *leftFront, *rightRear, *leftRear;
-    RobotDrive *drive;
+    CANTalon *rightFront, //<! Front right motor
+             *leftFront, //<! Front left motor
+             *rightRear, //<! Back right motor
+             *leftRear; //<! Back left motor
+    Gyro *gyro; //<! Gyro
   public:
+    /**
+     * @brief Constructs Drivetrain
+     */
     Drivetrain();
+    /**
+     * @brief Current motor to test
+     */
     enum e_motors{
-      FRONTRIGHT,
-      FRONTLEFT,
-      BACKRIGHT,
-      BACKLEFT
+      FRONTRIGHT, //<! Front right motor
+      FRONTLEFT, //<! Front left motor
+      BACKRIGHT, //<! Back right motor
+      BACKLEFT //<! Back left motor
     };
+    /**
+     * @brief No action
+     */
     void InitDefaultCommand();
-    void DriveMecanum(float, float, float, float, float);
-    void DriveArcade(float, float);
-    void TestMotor(e_motors, float);
+    /**
+     * @brief Drives the robot with mecanum
+     *
+     * @param x Joystick x value (-1.0 to 1.0)
+     * @param y Joystick y value (-1.0 to 1.0)
+     * @param z Joystick z value (-1.0 to 1.0)
+     * @param sensitivity Sensitivity (0.0 to 1.0)
+     * @param driveStraight Overrides z value to correct for motor lag
+     */
+    void DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight=false);
+    /**
+     * @brief Tests one motor
+     *
+     * @param motor Motor to test
+     * @param power Power to set motor
+     */
+    void TestMotor(e_motors motor, double power);
+    /**
+     * @brief Sets the gyro value to 0.0
+     */
+    void ResetGyro();
 };
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -8,7 +8,6 @@ Elevator::Elevator(){
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
   // Checks if the elevator is drifting
   useEncoder=false;
-  stoppedAtSensor=false;
 }
 void Elevator::InitDefaultCommand(){
 }
@@ -36,8 +35,8 @@ bool Elevator::GetElevatorTop(){
   SmartDashboard::PutBoolean("Elevator Top", !elevatorTop->Get());
   return elevatorTop->Get();
 }
-void Elevator::SetUseEncoder(bool param){
-  useEncoder=param;
+void Elevator::SetUseEncoder(bool use){
+  useEncoder=use;
 }
 bool Elevator::GetUseEncoder(){
   return useEncoder;

Subsystems/Elevator.h

@@ -2,25 +2,71 @@
 #define ELEVATOR_H
 
 #include "WPILib.h"
-#include "Commands/PIDSubsystem.h"
+/**
+ * @brief The main elevator for lifting totes
+ */
 class Elevator{
   private:
-    CANTalon *motor;
-    Encoder *elevatorEncoder;
-    static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    DigitalInput *elevatorBottom, *elevatorMiddle, *elevatorTop;
-    bool useEncoder;
+    CANTalon *motor; //<! The elevator motor
+    Encoder *elevatorEncoder; //<! The elevator encoder (unused)
+    DigitalInput *elevatorBottom, //<! The bottom elevator sensor
+                 *elevatorMiddle, //<! The middle elevator sensor
+                 *elevatorTop; //<! The top elevator sensor
+    bool useEncoder; //<! Use the elevator encoder (unused)
   public:
+    /**
+     * @brief Constructs Elevator
+     */
     Elevator();
-    bool stoppedAtSensor;
+    /**
+     * @brief No action
+     */
     void InitDefaultCommand();
-    void Run(double);
+    /**
+     * @brief Runs the elevator
+     *
+     * @param power The power to run the bin elevator (-1.0 to 1.0)
+     */
+    void Run(double power);
+    /**
+     * @brief Sets the encoder value to 0 (unused)
+     */
     void ResetEncoder();
+    /**
+     * @brief Gets the height of the elevator
+     *
+     * @return The hight of the elevator
+     */
     double GetHeight();
+    /**
+     * @brief Gets the status of the top sensor
+     *
+     * @return True if the sensor is activated
+     */
     bool GetElevatorTop();
+    /**
+     * @brief Gets the status of the middle sensor
+     *
+     * @return True if the sensor is activated
+     */
     bool GetElevatorMiddle();
+    /**
+     * @brief Gets the status of the bottom sensor
+     *
+     * @return True if the sensor is activated
+     */
     bool GetElevatorBottom();
-    void SetUseEncoder(bool);
+    /**
+     * @brief Use the elevator encoder (unused)
+     *
+     * @param use State to use encoder
+     */
+    void SetUseEncoder(bool use);
+    /**
+     * @brief Gets the state of useEncoder (unused)
+     *
+     * @return State of useEncoder
+     */
     bool GetUseEncoder();
 };
 #endif

Subsystems/Pneumatics.cpp

@@ -1,7 +1,7 @@
 #include "Pneumatics.h"
 #include "../RobotMap.h"
 
-Pneumatics::Pneumatics() : Subsystem("Pneumatics"){
+Pneumatics::Pneumatics(): Subsystem("Pneumatics"){
   solenoid1 = new Solenoid(BINELEVATOR_SOLDENOID_ONE);
   solenoid2 = new Solenoid(BINELEVATOR_SOLDENOID_TWO);
 }

Subsystems/Pneumatics.h

@@ -2,14 +2,30 @@
 #define PNEUMATICS_H
 
 #include "WPILib.h"
-class Pneumatics: public Subsystem
-{
+/**
+ * @brief Pneumatics on the robot (unused)
+* 
+* For opening or closing the bin arms
+ */
+class Pneumatics: public Subsystem{
   private:
-    Solenoid *solenoid1, *solenoid2;
+    Solenoid *solenoid1, //<! Solenoid 1
+             *solenoid2; //<! Solenoid 3
   public:
+    /**
+     * @brief Constructs Pneumatics
+     */
     Pneumatics();
+    /**
+     * @brief No action
+     */
     void InitDefaultCommand();
-    void SetOpen(bool);
+    /**
+     * @brief Sets the state of the arms
+     *
+     * @param state State of the arms
+     */
+    void SetOpen(bool state);
 };
 #endif
 // vim: ts=2:sw=2:et

mainpage.dox

@@ -0,0 +1,31 @@
+/**
+* @mainpage Dent
+* 
+* The Hitchhikers Team 2015 robot, Dent. Features an elevator, bin elevator, and mecanum drive
+* @section Subsystems
+*   -# @ref Drivetrain
+*   -# @ref BinElevator
+*   -# @ref Elevator
+*   -# @ref Collector
+*   -# @ref Pneumatics (unused)
+* @section Commands
+*   -# @ref AutoDrive
+*   -# @ref Turn
+*   -# @ref BinCloseArms
+*   -# @ref BinLower
+*   -# @ref BinOpenArms
+*   -# @ref BinRaise
+*   -# @ref RollIn
+*   -# @ref RollOut
+*   -# @ref Drive
+*   -# @ref Lower
+*   -# @ref Raise
+* @section Command Groups
+*   -# @ref Autonomous
+*   -# @ref CollectTote
+*   -# @ref ReleaseTote
+*   -# @ref CheckDrive
+*   -# @ref CheckRobot
+* 
+* Note: Recycling containers are referred to bins throughout the project
+*/