@@ -18,11 +18,11 @@ public class DriveDistance extends Command {
18
18
private double D = 0.0055 ;
19
19
private PIDController pidDriveController = new PIDController (P , I , D );
20
20
private PIDController pidTurnController = new PIDController (P , I , D );
21
-
21
+
22
22
private double rotationsToTarget ;
23
23
private double ticksTarget ;
24
24
private int ticksPerRotation = 42 ;
25
- private double wheelDiameter = 0.5 ; //ft
25
+ private double wheelDiameter = 0.5 ; // ft
26
26
27
27
/**
28
28
* Drives robot a set distance w/ PID loop to keep it straight
@@ -32,7 +32,7 @@ public class DriveDistance extends Command {
32
32
public DriveDistance (double distance ) {
33
33
requires (Robot .drivetrain );
34
34
// calculate # of ticks based off distance
35
- rotationsToTarget = distance / (wheelDiameter * Math .PI ) * 10.75 ;
35
+ rotationsToTarget = distance / (wheelDiameter * Math .PI ) * 10.75 ;
36
36
pidDriveController .setTolerance (1 );
37
37
pidTurnController .setTolerance (0.05 );
38
38
}
@@ -43,19 +43,17 @@ protected void initialize() {
43
43
Robot .drivetrain .calibrate ();
44
44
SmartDashboard .putNumber ("Ticks To Target" , rotationsToTarget );
45
45
SmartDashboard .putString ("Command" , "DriveDistance" );
46
-
47
46
}
48
47
49
48
// Called repeatedly when this Command is scheduled to run
50
49
@ Override
51
50
protected void execute () {
52
- double forwardsSpeed = pidDriveController .calculate (Robot .drivetrain .getPosition (), rotationsToTarget );
51
+ double forwardsSpeed =
52
+ pidDriveController .calculate (Robot .drivetrain .getPosition (), rotationsToTarget );
53
53
double turnSpeed = pidTurnController .calculate (Robot .drivetrain .gyro .getAngle (), 0 );
54
54
Robot .drivetrain .drive (forwardsSpeed , turnSpeed );
55
55
}
56
56
57
-
58
-
59
57
// Make this return true when this Command no longer needs to run execute()
60
58
@ Override
61
59
protected boolean isFinished () {
0 commit comments