Merge with feature/pretest

Commands/Autonomous/Autonomous.cpp

@@ -1,4 +1,5 @@
 #include "Autonomous.h"
+#include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
 #include "../Elevator/Raise.h"
 #include "../Elevator/Lower.h"

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

OI.cpp

@@ -7,7 +7,9 @@
 #include "Commands/BinElevator/BinRaise.h"
 #include "Commands/BinElevator/BinCloseArms.h"
 #include "Commands/BinElevator/BinOpenArms.h"
-
+#include "Commands/Collector/CollectTote.h"
+#include "Commands/Collector/ReleaseTote.h"
+#include "Commands/Test/CheckRobot.h"
 OI::OI() {
   // Joysticks
   leftStick=new Joystick(0);
@@ -45,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;

Subsystems/Drivetrain.cpp

@@ -20,4 +20,24 @@ 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){
+  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 DriveArcade(float, float);
+    void TestMotor(e_motors,float);
 };
 #endif
 // vim: ts=2:sw=2:et