Fixed poorly written code

CommandBase.cpp

@@ -9,7 +9,6 @@
 #include "Commands/OpenCollector.h"
 #include "Commands/CollectTote.h"
 #include "Commands/ReleaseTote.h"
-#include "Commands/Eject.h"
 #include "Commands/Raise.h"
 #include "Commands/Lower.h"
 #include "Commands/Calibrate.h"
@@ -23,8 +22,7 @@ CommandBase::CommandBase(char const *name) : Command(name) {
 }
 CommandBase::CommandBase() : Command() {
 }
-void CommandBase::init()
-{
+void CommandBase::init(){
   drivetrain = new Drivetrain();
   collector = new Collector();
   elevator = new Elevator();

Commands/CloseCollector.cpp

@@ -3,12 +3,13 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){
     Requires(DentRobot::collector);
 }
 void CloseCollector::Initialize(){
+  SetTimeout(0.5);
 }
 void CloseCollector::Execute(){
     DentRobot::collector->MoveArms(.1);
 }
 bool CloseCollector::IsFinished(){
-    return DentRobot::collector->ArmsDoneMoving();
+    return DentRobot::collector->ArmSensor();
 }
 void CloseCollector::End(){
 }

Commands/Compressor/StartCompressing.cpp

@@ -1,13 +1,12 @@
 #include "StartCompressing.h"
-#include <cmath>
 #include "../../DentRobot.h"
 StartCompressing::StartCompressing() : Command("StartCompressing"){
-        Requires(DentRobot::airCompressor);
+  Requires(DentRobot::airCompressor);
 }
 void StartCompressing::Initialize(){
 }
 void StartCompressing::Execute(){
-    DentRobot::airCompressor->CreateCompressedAir();
+  DentRobot::airCompressor->StartCompressing();
 }
 bool StartCompressing::IsFinished(){
   return false;

Commands/Compressor/StopCompressing.cpp

@@ -1,13 +1,12 @@
 #include "StopCompressing.h"
-#include <cmath>
 #include "../../DentRobot.h"
 StopCompressing::StopCompressing() : Command("StopCompressing"){
-        Requires(DentRobot::airCompressor);
+  Requires(DentRobot::airCompressor);
 }
 void StopCompressing::Initialize(){
 }
 void StopCompressing::Execute(){
-    DentRobot::airCompressor->StopCreatingCompressedAir();
+  DentRobot::airCompressor->StopCompressing();
 }
 bool StopCompressing::IsFinished(){
   return false;

Commands/Eject.cpp

@@ -1,16 +0,0 @@
-#include "Eject.h"
-#include "../DentRobot.h"
-Eject::Eject() : Command("Eject"){
-  Requires(DentRobot::collector);
-}
-void Eject::Initialize(){
-}
-void Eject::Execute(){
-}
-bool Eject::IsFinished(){
-  return false;
-}
-void Eject::End(){
-}
-void Eject::Interrupted(){
-}

Commands/Eject.h

@@ -1,17 +0,0 @@
-#ifndef EJECT_H
-#define EJECT_H
-
-#include "Commands/Command.h"
-#include "WPILib.h"
-
-class Eject: public Command{
-  public:
-    Eject();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-
-#endif

Commands/OpenCollector.cpp

@@ -1,15 +1,15 @@
 #include "OpenCollector.h"
 OpenCollector::OpenCollector() : Command("OpenCollector"){
-    Requires(DentRobot::collector);
+  Requires(DentRobot::collector);
 }
 void OpenCollector::Initialize(){
 }
 void OpenCollector::Execute(){
-    //TODO check this value to move the motors in the right direction
-    DentRobot::collector->MoveArms(-.1);
+  //TODO check this value to move the motors in the right direction
+  DentRobot::collector->MoveArms(-.1);
 }
 bool OpenCollector::IsFinished(){
-    return DentRobot::collector->ArmsDoneMoving();
+  return DentRobot::collector->ArmSensor();
 }
 void OpenCollector::End(){
 }

OI.cpp

@@ -1,7 +1,6 @@
 #include "OI.h"
 #include "Commands/Lower.h"
 #include "Commands/Raise.h"
-#include "Commands/Eject.h"
 #include "Commands/OpenCollector.h"
 #include "Commands/CloseCollector.h"
 #include "Commands/CollectTote.h"
@@ -13,7 +12,6 @@ OI::OI() {
   leftStick=new Joystick(0);
   rightStick=new Joystick(1);
   //TODO name these buttons to their functions rather to their number
-  JoystickButton *left10=new JoystickButton(leftStick, 10);
   JoystickButton *right1=new JoystickButton(rightStick, 1);
   JoystickButton *right2=new JoystickButton(rightStick, 2);
   JoystickButton *right3=new JoystickButton(rightStick, 3);
@@ -22,7 +20,6 @@ OI::OI() {
   right2->WhenPressed(new CloseCollector());
   right3->WhenPressed(new CollectTote());
   right4->WhenPressed(new ReleaseTote());
-  left10->WhenPressed(new Eject());
 }
 Joystick* OI::GetRightStick(){
   return rightStick;

RobotMap.h

@@ -3,4 +3,23 @@
 
 #include "WPILib.h"
 
+// Elevator
+#define ELEVATOR_CAN 0
+
+// Drivetrain
+#define DRIVE_FRONT_LEFT_CAN 41
+#define DRIVE_BACK_LEFT_CAN 43
+#define DRIVE_FRONT_RIGHT_CAN 40
+#define DRIVE_BACK_RIGHT_CAN 42
+
+// Collector
+#define COLLECTOR_WINDOW_LEFT_CAN 50
+#define COLLECTOR_WINDOW_RIGHT_CAN 51
+#define COLLECTOR_LEFT_CAN 52
+#define COLLECTOR_RIGHT_CAN 53
+#define COLLECTOR_BOXSWITCH_DIO 9
+
+// Compressor
+#define COMPRESSOR_CAN 31
+
 #endif

Subsystems/AirCompressor.cpp

@@ -2,17 +2,15 @@
 #include "../RobotMap.h"
 
 AirCompressor::AirCompressor() : Subsystem("AirCompressor") {
-    compressher = new Compressor(31);
+  compressor = new Compressor(COMPRESSOR_CAN);
 }
 void AirCompressor::InitDefaultCommand() {
 }
-int AirCompressor::CreateCompressedAir() {
-    printf("compressing and stuff\n");
-    compressher->Start();
-    return 0;
+void AirCompressor::StartCompressing() {
+  printf("Starting compressor\n");
+  compressor->Start();
 }
-int AirCompressor::StopCreatingCompressedAir() {
-    printf("not compressing and stuff\n");
-    compressher->Stop();
-    return 0;
+void AirCompressor::StopCompressing() {
+  printf("Stopping compressor\n");
+  compressor->Stop();
 }

Subsystems/AirCompressor.h

@@ -5,11 +5,11 @@
 class AirCompressor: public Subsystem
 {
   private:
-      Compressor *compressher;
+      Compressor *compressor;
   public:
     AirCompressor();
     void InitDefaultCommand();
-    int CreateCompressedAir();
-    int StopCreatingCompressedAir();
+    void StartCompressing();
+    void StopCompressing();
 };
 #endif

Subsystems/Collector.cpp

@@ -2,31 +2,25 @@
 #include "../RobotMap.h"
 
 Collector::Collector() : Subsystem("Collector") {
-  windowMotorLeft=new CANTalon(50);
-  windowMotorRight=new CANTalon(51);
-  collectorMotorLeft=new CANTalon(52);
-  collectorMotorRight=new CANTalon(53);
-  boxSwitch=new DigitalInput(9);
+  windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN);
+  windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN);
+  collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
+  collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
+  boxSwitch=new DigitalInput(COLLECTOR_BOXSWITCH_DIO);
 }
 void Collector::InitDefaultCommand() {
 }
 void Collector::MoveArms(float a){
   windowMotorLeft->Set(a);
   windowMotorRight->Set(-a);
-  a++;
 }
 void Collector::MoveRollers(float a){
   collectorMotorLeft->Set(a);
   collectorMotorRight->Set(-a);
-  r++;
 }
-bool Collector::ArmsDoneMoving(){
-  //TODO calibrate these values or find a sensor to use
-  if(a>=200){
-    return true;
-  }else{
-    return false;
-  }
+bool Collector::ArmSensor(){
+  // TODO: include limit switch code
+  return false;
 }
 bool Collector::BoxCollected(){
   return boxSwitch->Get();

Subsystems/Collector.h

@@ -7,13 +7,12 @@ class Collector: public Subsystem
   private:
       CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorRight;
       DigitalInput *boxSwitch;
-      int a,r;
   public:
     Collector();
     void InitDefaultCommand();
     void MoveArms(float);
     void MoveRollers(float);
-    bool ArmsDoneMoving();
+    bool ArmSensor();
     bool BoxCollected();
 };
 #endif

Subsystems/Drivetrain.cpp

@@ -3,20 +3,20 @@
 #include "../Commands/Drive.h"
 
 Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
-  rightFront = new CANTalon(40);
-  leftFront = new CANTalon(41);
-  rightRear = new CANTalon(42);
-  leftRear = new CANTalon(43);
+  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);
 }
 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 correctZ = -z *.5;
-    rightFront->Set((-correctX + correctY - correctZ));
-    leftFront->Set((correctX + correctY + correctZ)*-1);
-    rightRear->Set((correctX + correctY - correctZ));
-    leftRear->Set((-correctX + correctY + correctZ)*-1);
+  double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
+  double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
+  double correctZ = -z *.5;
+  rightFront->Set((-correctX + correctY - correctZ));
+  leftFront->Set((correctX + correctY + correctZ)*-1);
+  rightRear->Set((correctX + correctY - correctZ));
+  leftRear->Set((-correctX + correctY + correctZ)*-1);
 }

Subsystems/Elevator.cpp

@@ -1,7 +1,7 @@
 #include "Elevator.h"
 #include "../RobotMap.h"
 Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{
-  motor=new CANTalon(0);
+  motor=new CANTalon(ELEVATOR_CAN);
   elevatorEncoder=new Encoder(0,1,false);
   offset=0;
   height=0;