Made DriveStraight a method correctStraightness() in drive train. NOTE: Tends to...

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index c2e4f996d49458d36e9e300d101888b341214047..fc31fb4886df03ab86ccfaed3805f832c2b67f99 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -40,13 +40,14 @@ public class Robot extends IterativeRobot {
 
   @Override
   public void autonomousInit() {
-    Scheduler.getInstance().add(new TimeDrive(1.5, 0.4));
+    TimeDrive timedrive = new TimeDrive(0.5, 0.2);
+    Scheduler.getInstance().add(timedrive);
   }
 
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
-
+    Robot.getDriveTrain().correctStraightness();
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
index 4d55689148e04f6d395927aea60a525772cd4ebe..14c36197aad77d2c36a35324c1364883cb20fe4e 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
@@ -7,10 +7,21 @@ import edu.wpi.first.wpilibj.command.Command;
 public class DriveStraight extends Command {
 
   double speedL, speedR, tempSpeedR, seconds;
-  // double moveR = Robot.getDriveTrain().getLeftSpeed();
-  // double moveL = Robot.getDriveTrain().getRightSpeed();
 
-  public DriveStraight(double seconds) {
+  // Max speed at mv of 0.2 (RIGHT)
+  final double maxSpeedOneR = 16.1;
+  // Max speed at mv of 0.4 (RIGHT)
+  final double maxSpeedTwoR = 49;
+  // Max speed at mv of 0.6 (RIGHT)
+  final double maxSpeedThreeR = 75.2;
+  // Max speed at mv of 0.2 (LEFT)
+  final double maxSpeedOneL = 13.8;
+  // Max speed at mv of 0.4 (LEFT)
+  final double maxSpeedTwoL = 46.9;
+  // Max speed at mv of 0.6 (LEFT)
+  final double maxSpeedThreeL = 74;
+
+  public DriveStraight(double seconds, double mv) {
     this.seconds = seconds;
     requires(Robot.getDriveTrain());
     speedL = Robot.getDriveTrain().getLeftSpeed();
@@ -24,18 +35,17 @@ public class DriveStraight extends Command {
 
   @Override
   public void execute() {
+    // Motor values for both motors
     Double mvR = null, mvL = null;
-    final double maxSpeedOneR = 16.1;
-    final double maxSpeedTwoR = 49;
-    final double maxSpeedThreeR = 75.2;
-    final double maxSpeedOneL = 13.8;
-    final double maxSpeedTwoL = 46.9;
-    final double maxSpeedThreeL = 74;
+
+    // getting speed for both motors and exchanging them
     speedL = Robot.getDriveTrain().getLeftSpeed();
     speedR = Robot.getDriveTrain().getRightSpeed();
     tempSpeedR = speedR;
     speedR = speedL;
     speedL = tempSpeedR;
+
+    // converting right motor speed to motor value
     if (speedR <= maxSpeedOneR) {
       mvR = speedR / maxSpeedOneR * 5;
     } else if (speedR <= maxSpeedTwoR) {
@@ -43,6 +53,8 @@ public class DriveStraight extends Command {
     } else if (speedR <= maxSpeedThreeR) {
       mvR = speedR / maxSpeedThreeR * 1.66;
     }
+
+    // converting left motor speed to motor value
     if (speedL <= maxSpeedOneL) {
       mvL = speedL / maxSpeedOneL * 5;
     } else if (speedL <= maxSpeedTwoL) {
@@ -50,6 +62,8 @@ public class DriveStraight extends Command {
     } else if (speedL <= maxSpeedThreeL) {
       mvL = speedL / maxSpeedThreeL * 1.66;
     }
+
+    // Setting motors at new motor values
     if (mvR != null && mvL != null) {
       Robot.getDriveTrain().setMotorValues(mvR, mvL); // we changed move to //
                                                       // speed

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 364340e106f2901ab65720f71d0f402df0aa950d..9dd8d8c9c6a708f828707a8262bf70afa56e8031 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
 import com.ctre.CANTalon;
@@ -64,6 +65,62 @@ public class DriveTrain extends Subsystem {
     setMotorValues(0, 0);
   }
 
+  public void correctStraightness() {
+
+    double speedL, speedR, tempSpeedR;
+
+    // Max speed at mv of 0.2 (RIGHT)
+    final double maxSpeedOneR = 16.1;
+    // Max speed at mv of 0.4 (RIGHT)
+    final double maxSpeedTwoR = 49;
+    // Max speed at mv of 0.6 (RIGHT)
+    final double maxSpeedThreeR = 75.2;
+    // Max speed at mv of 0.2 (LEFT)
+    final double maxSpeedOneL = 13.8;
+    // Max speed at mv of 0.4 (LEFT)
+    final double maxSpeedTwoL = 46.9;
+    // Max speed at mv of 0.6 (LEFT)
+    final double maxSpeedThreeL = 74;
+
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+
+    // Motor values for both motors
+    Double mvR = null, mvL = null;
+
+    // NOTE: Make threshold. Robot tends to go haywire.
+    // getting speed for both motors and exchanging them
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+    tempSpeedR = speedR;
+    speedR = speedL;
+    speedL = tempSpeedR;
+
+    // converting right motor speed to motor value
+    if (speedR <= maxSpeedOneR) {
+      mvR = speedR / maxSpeedOneR * 5;
+    } else if (speedR <= maxSpeedTwoR) {
+      mvR = speedR / maxSpeedTwoR * 2.5;
+    } else if (speedR <= maxSpeedThreeR) {
+      mvR = speedR / maxSpeedThreeR * 1.66;
+    }
+
+    // converting left motor speed to motor value
+    if (speedL <= maxSpeedOneL) {
+      mvL = speedL / maxSpeedOneL * 5;
+    } else if (speedL <= maxSpeedTwoL) {
+      mvL = speedL / maxSpeedTwoL * 2.5;
+    } else if (speedL <= maxSpeedThreeL) {
+      mvL = speedL / maxSpeedThreeL * 1.66;
+    }
+
+    // Setting motors at new motor values
+    if (mvR != null && mvL != null) {
+      Robot.getDriveTrain().setMotorValues(mvR, mvL); // we changed move to //
+    } // speed
+
+  }
+
   public double getFrontLeftMotorVal() {
     return frontLeft.get();
   }