fixes for frankie

Commands/Autonomous/AutoDrive.cpp

@@ -14,13 +14,11 @@ void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //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);
 }
 void AutoDrive::Interrupted(){
   End();

Commands/Autonomous/Turn.cpp

@@ -8,14 +8,12 @@ void Turn::Initialize(){
 }
 void Turn::Execute(){
   //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);
 }
 void Turn::Interrupted(){
   End();

Commands/Drivetrain/Drive.cpp

@@ -10,23 +10,7 @@ void Drive::Execute(){
   x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
   y = -DentRobot::oi->GetLeftStick()->GetRawAxis(1);
   z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
-  // Lock the x axis when not holding button 1
+  DentRobot::drivetrain->DriveMecanum(x, y, z);
-  //if(DentRobot::oi->GetLeftStick()->GetRawButton(1)){
-  //  x = 0;
-  //}
-  //if(DentRobot::oi->GetLeftStick()->GetRawButton(2)){
-  //  y = 0;
-  //}
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  if(DentRobot::oi->GetLeftStick()->GetRawButton(11)){
-    x = -x;
-    y = -y;
-  }
-  if(DentRobot::oi->GetLeftStick()->GetRawButton(7)){
-    DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9);
-  }else{
-    DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9);
-  }
 }
 bool Drive::IsFinished(){
   return IsTimedOut();

Subsystems/Drivetrain.cpp

@@ -3,52 +3,20 @@
 #include "../Commands/Drivetrain/Drive.h"
 
 Drivetrain::Drivetrain(): Subsystem("Drivetrain"){
-  rightFront = new CANTalon(DRIVE_FRONT_RIGHT_CAN);
+  rightFront = new Talon(DRIVE_FRONT_RIGHT_CAN);
-  leftFront = new CANTalon(DRIVE_FRONT_LEFT_CAN);
+  leftFront = new Talon(DRIVE_FRONT_LEFT_CAN);
-  rightRear = new CANTalon(DRIVE_BACK_RIGHT_CAN);
+  rightRear = new Talon(DRIVE_BACK_RIGHT_CAN);
-  leftRear = new CANTalon(DRIVE_BACK_LEFT_CAN);
+  leftRear = new Talon(DRIVE_BACK_LEFT_CAN);
   gyro = new Gyro(DRIVE_GYRO_ANALOG);
 }
 void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
-void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight){
+void Drivetrain::DriveMecanum(double x, double y, double z){
-  double kP = SmartDashboard::GetNumber("Gyro kP");
+  rightFront->Set(y+(x+z));
-  double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
+  rightRear->Set(y+(x+z));
-  double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
+  leftFront->Set(-y+(x+z));
-  double correctZ;
+  leftRear->Set(-y+(x+z));
-  if(driveStraight){
-    printf("Driving straight at: %f\n", -gyro->GetAngle()*kP);
-    correctZ = -gyro->GetAngle()*kP;
-  }else{
-    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, double 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;
-  }
 }
 void Drivetrain::ResetGyro(){
   gyro->Reset();

Subsystems/Drivetrain.h

@@ -9,7 +9,7 @@
  */
 class Drivetrain: public Subsystem{
   private:
-    CANTalon *rightFront, //<! Front right motor
+    Talon *rightFront, //<! Front right motor
              *leftFront, //<! Front left motor
              *rightRear, //<! Back right motor
              *leftRear; //<! Back left motor
@@ -32,23 +32,7 @@ class Drivetrain: public Subsystem{
      * @brief No action
      */
     void InitDefaultCommand();
-    /**
+    void DriveMecanum(double x, double y, double z);
-     * @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 (default: false)
-     */
-    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
      */