From 7e95ca8469324aa9142264c3a0a9bd5590de0115 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Thu, 17 Sep 2015 19:59:58 +0000
Subject: [PATCH] added arcade drive

---
 Commands/Autonomous/AutoDrive.cpp |  4 ++--
 Commands/Autonomous/Turn.cpp      |  4 ++--
 Commands/Drivetrain/Drive.cpp     |  6 +-----
 Subsystems/Drivetrain.cpp         | 23 +++++++++--------------
 Subsystems/Drivetrain.h           | 10 +++-------
 5 files changed, 17 insertions(+), 30 deletions(-)

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index c3921f4..cb30c46 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -14,13 +14,13 @@ 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);
+  DentRobot::drivetrain->DriveArcade(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);
+  DentRobot::drivetrain->DriveArcade(0.0, 0.0, 0.0, 0.9);
 }
 void AutoDrive::Interrupted() {
   End();
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
index 24d9890..2ca2e4b 100644
--- a/Commands/Autonomous/Turn.cpp
+++ b/Commands/Autonomous/Turn.cpp
@@ -8,14 +8,14 @@ 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);
+  DentRobot::drivetrain->DriveArcade(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);
+  DentRobot::drivetrain->DriveArcade(0.0, 0.0, 0.0, 0.9);
 }
 void Turn::Interrupted() {
   End();
diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp
index e5056e1..00f7559 100644
--- a/Commands/Drivetrain/Drive.cpp
+++ b/Commands/Drivetrain/Drive.cpp
@@ -22,11 +22,7 @@ void Drive::Execute() {
     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);
-  }
+  DentRobot::drivetrain->DriveArcade(x, y, z, 0.9);
 }
 bool Drive::IsFinished() {
   return IsTimedOut();
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index 205da60..566ead0 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/Subsystems/Drivetrain.cpp
@@ -3,8 +3,6 @@
 #include "../Commands/Drivetrain/Drive.h"
 
 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);
@@ -12,34 +10,31 @@ Drivetrain::Drivetrain(): Subsystem("Drivetrain") {
 void Drivetrain::InitDefaultCommand() {
   SetDefaultCommand(new Drive());
 }
-void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight) {
+void Drivetrain::DriveArcade(double x, double y, double z, double sensitivity, bool driveStraight) {
   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;
+  double correctZ = 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 /= 2.0;
   }
-  rightFront->Set((-correctX + correctY - correctZ));
-  leftFront->Set((correctX + correctY + correctZ)*-1);
-  rightRear->Set((correctX + correctY - correctZ));
-  leftRear->Set((-correctX + correctY + correctZ)*-1);
+  int leftPower=((correctY+(correctX+correctZ))/2+1)*127+1;
+  int rightPower=((correctY-(correctX+correctZ))/2+1)*127+1;
+  printf("rightPower:%d\n",rightPower);
+  printf("leftPower:%d\n",leftPower);
+  leftRear->Set(leftPower);
+  rightRear->Set(rightPower);
 }
 //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;
diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h
index 3c20530..5e6b3c6 100644
--- a/Subsystems/Drivetrain.h
+++ b/Subsystems/Drivetrain.h
@@ -9,9 +9,7 @@
  */
 class Drivetrain: public Subsystem {
  private:
-  CANTalon *rightFront, //<! Front right motor
-           *leftFront, //<! Front left motor
-           *rightRear, //<! Back right motor
+  CANTalon *rightRear, //<! Back right motor
            *leftRear; //<! Back left motor
   Gyro *gyro; //<! Gyro
  public:
@@ -23,8 +21,6 @@ class Drivetrain: public Subsystem {
    * @brief Current motor to test
    */
   enum e_motors {
-    FRONTRIGHT, //<! Front right motor
-    FRONTLEFT, //<! Front left motor
     BACKRIGHT, //<! Back right motor
     BACKLEFT //<! Back left motor
   };
@@ -33,7 +29,7 @@ class Drivetrain: public Subsystem {
    */
   void InitDefaultCommand();
   /**
-   * @brief Drives the robot with mecanum
+   * @brief Drives the robot with arcade
    *
    * @param x Joystick x value (-1.0 to 1.0)
    * @param y Joystick y value (-1.0 to 1.0)
@@ -41,7 +37,7 @@ class Drivetrain: public Subsystem {
    * @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);
+  void DriveArcade(double x, double y, double z, double sensitivity, bool driveStraight = false);
   /**
    * @brief Tests one motor
    *