Merge branch clenaup

Commands/Autonomous/AutoDrive.cpp

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

Commands/Autonomous/Autonomous.cpp

@@ -15,44 +15,44 @@ Autonomous::Autonomous(int seq){
   switch(seq){
     case 0:
       // Just for testing
-      AddSequential(new AutoDrive(.5,0,.25));
+      // Strafe at .25 power
+      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
       break;
     case 1:
-      // Wait a desigated value, drive to Auto Zone (TM)
+      // Drive to Auto Zone (TM)
       Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.8,0.0));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
       break;
     case 2:
-      // Wait a desigated value, drive to Auto Zone (TM)
+      // 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.75,0.0));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
-      //AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0,-1.0));
       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"));
+      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.75,0));
+      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.75,0));
+      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"));
+      printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
       if(SmartDashboard::GetBoolean("Three totes")){
-        AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
+        AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
         AddSequential(new CollectTote());
         AddSequential(new Lower());
         AddSequential(new Raise());
       }
-      AddSequential(new Turn(90));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
       AddSequential(new ReleaseTote());
       break;
     default:

Commands/Autonomous/CollectTote.cpp

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

Commands/Autonomous/ReleaseTote.cpp

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

Commands/Autonomous/Turn.cpp

@@ -9,14 +9,14 @@ 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);
+  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/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()*.8);
+  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/Test/CheckDrive.cpp

@@ -13,16 +13,16 @@ void CheckDrive::Initialize(){
 void CheckDrive::Execute(){
   switch(motor){
     case DRIVE_FRONT_LEFT_CAN:
-      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT,1);
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT, 1);
       break;
     case DRIVE_FRONT_RIGHT_CAN:
-      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT,1);
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT, 1);
       break;
     case DRIVE_BACK_LEFT_CAN:
-      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT,1);
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT, 1);
       break;
     case DRIVE_BACK_RIGHT_CAN:
-      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT,1);
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT, 1);
       break;
     default:
       break;

DentRobot.cpp

@@ -18,16 +18,14 @@ DentRobot::DentRobot(){
   pneumatics=new Pneumatics();
   //CameraServer::GetInstance()->SetQuality(25);
   //CameraServer::GetInstance()->StartAutomaticCapture("cam0");
-  //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
-  //SmartDashboard::PutNumber("Auto Sequence", 0);
   printf("The robot is on\n");
 }
 void DentRobot::RobotInit(){
-  SmartDashboard::PutNumber("CodeVersion",CODE_VERSION);
+  SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
   // Autonomous
   // Sequence of autonomous command
-  SmartDashboard::PutNumber("Auto Sequence",2.0);
+  SmartDashboard::PutNumber("Auto Sequence", 2.0);
-  SmartDashboard::PutNumber("Auto Wait Time",3.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
@@ -35,7 +33,7 @@ void DentRobot::RobotInit(){
   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);
+  SmartDashboard::PutNumber("TurnAmount", 2.0);
 
   // Elevators
   SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
@@ -50,7 +48,7 @@ void DentRobot::DisabledPeriodic(){
 }
 void DentRobot::AutonomousInit(){
   aut=new Autonomous(SmartDashboard::GetNumber("Auto Sequence"));
-  printf("Enabling Auto Sequence %f\n",SmartDashboard::GetNumber("Auto Sequence"));
+  printf("Enabling Auto Sequence %f\n", SmartDashboard::GetNumber("Auto Sequence"));
   if(aut != NULL){
     aut->Start();
   }

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

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,9 +12,9 @@ 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");
   }

Subsystems/Drivetrain.h

@@ -15,9 +15,9 @@ class Drivetrain: public Subsystem{
       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);
+    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);