Implemented gyro (untested)

Commands/Autonomous/AutoDrive.cpp

@@ -10,14 +10,14 @@ AutoDrive::AutoDrive(double duration, double xtmp, double ytmp): Command("AutoDr
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
+  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9);
 }
 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);
 }
 void AutoDrive::Interrupted(){
   End();

Commands/Autonomous/Turn.cpp

@@ -7,15 +7,15 @@ Turn::Turn(double timeout): Command("Turn"){
 void Turn::Initialize(){
 }
 void Turn::Execute(){
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9, 0.0);
+  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, 0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9);
 }
 void Turn::Interrupted(){
   End();

Commands/Drivetrain/Drive.cpp

@@ -17,8 +17,8 @@ void Drive::Execute(){
   //if(DentRobot::oi->GetLeftStick()->GetRawButton(2)){
   //  y=0;
   //}
-  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9);
 }
 bool Drive::IsFinished(){
   return IsTimedOut();

RobotMap.h

@@ -28,6 +28,7 @@
 #define DRIVE_BACK_LEFT 3
 #define DRIVE_FRONT_RIGHT 4
 #define DRIVE_BACK_RIGHT 5
+#define DRIVE_GYRO_ANALOG 0
 
 // Collector
 #define COLLECTOR_RAMP 7

Subsystems/Drivetrain.cpp

@@ -7,14 +7,22 @@ Drivetrain::Drivetrain(): Subsystem("Drivetrain"){
   leftFront = new Victor(DRIVE_FRONT_LEFT);
   rightRear = new Victor(DRIVE_BACK_RIGHT);
   leftRear = new Victor(DRIVE_BACK_LEFT);
+  gyro = new Gyro(DRIVE_GYRO_ANALOG);
 }
 void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
-void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, double gyro){
+void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight){
+  //TODO: Find the correct value for kP
+  double kP=0.02;
   double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
   double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
-  double correctZ = -z * 0.5;
+  double correctZ;
+  if(driveStraight){
+    correctZ = -gyro->GetAngle()*kP;
+  }else{
+    correctZ = -z * 0.5;
+  }
   if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){
     correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
   }
@@ -23,7 +31,6 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity,
   rightRear->Set((correctX + correctY - correctZ));
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
-
 //Used in pretest
 void Drivetrain::TestMotor(e_motors motor, double power){
   switch(motor){
@@ -43,4 +50,7 @@ void Drivetrain::TestMotor(e_motors motor, double power){
       break;
   }
 }
+void Drivetrain::ResetGyro(){
+  gyro->Reset();
+}
 // vim: ts=2:sw=2:et

Subsystems/Drivetrain.h

@@ -13,6 +13,7 @@ class Drivetrain: public Subsystem{
              *leftFront, //<! Front left motor
              *rightRear, //<! Back right motor
              *leftRear; //<! Back left motor
+    Gyro *gyro; //<! Gyro
   public:
     /**
      * @brief Constructs Drivetrain
@@ -34,27 +35,24 @@ class Drivetrain: public Subsystem{
     /**
      * @brief Drives the robot with mecanum
      *
-     * @param double Joystick x value (-1.0 to 1.0)
+     * @param x Joystick x value (-1.0 to 1.0)
-     * @param double Joystick y value (-1.0 to 1.0)
+     * @param y Joystick y value (-1.0 to 1.0)
-     * @param double Joystick z value (-1.0 to 1.0)
+     * @param z Joystick z value (-1.0 to 1.0)
-     * @param double Sensitivity (0.0 to 1.0)
+     * @param sensitivity Sensitivity (0.0 to 1.0)
-     * @param double Gyro value (unused)
+     * @param driveStraight Overrides z value to correct for motor lag
      */
-    void DriveMecanum(double, double, double, double, double);
+    void DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight=false);
-    /**
-     * @brief Drives the robot with arcade drive
-     *
-     * @param double Joystick x value (-1.0 to 1.0)
-     * @param double Joystick y value (-1.0 to 1.0)
-     */
-    void DriveArcade(double, double);
     /**
      * @brief Tests one motor
      *
-     * @param e_motors Motor to test
+     * @param motor Motor to test
-     * @param double Power to set motor
+     * @param power Power to set motor
      */
-    void TestMotor(e_motors, double);
+    void TestMotor(e_motors motor, double power);
+    /**
+     * @brief Sets the gyro value to 0.0
+     */
+    void ResetGyro();
 };
 #endif
 // vim: ts=2:sw=2:et