| 7 | | |
| 8 | | * Add the following imports: |
| 9 | | {{{ |
| 10 | | import edu.wpi.first.wpilibj.Timer; |
| 11 | | }}} |
| 12 | | |
| 13 | | * Add variables for closed loop control in the Robot class |
| 14 | | {{{ |
| 15 | | private double leftPower, rightPower; |
| 16 | | private Timer autoTimer; |
| 17 | | private boolean autoDone; |
| 18 | | }}} |
| 19 | | |
| 20 | | * In robotInit() instantiate the new Timer object |
| 21 | | {{{ |
| 22 | | autoTimer = new Timer(); |
| 23 | | }}} |
| 24 | | |
| 25 | | * In robotPeriodic() add the motor power settings to the smart dashboard |
| 26 | | {{{ |
| 27 | | SmartDashboard.putNumber("Left power", leftPower); |
| 28 | | SmartDashboard.putNumber("Right power", rightPower); |
| 29 | | }}} |
| 30 | | |
| 31 | | * In autonomousInit() initialize the motor powers and start the timer we created |
| 32 | | so we can periodically adjust the motor power to keep the robot going straight: |
| 33 | | {{{ |
| 34 | | // start motors turning forward at 20% power |
| 35 | | leftPower = 0.20; |
| 36 | | leftMotor.set(leftPower); |
| 37 | | // to go forward, left motor turns clockwise, right motor counter-clockwise |
| 38 | | rightPower = -0.20; |
| 39 | | rightMotor.set(rightPower); |
| 40 | | |
| 41 | | autoTimer.start(); |
| 42 | | autoDone = false; |
| 43 | | }}} |
| 44 | | |
| 45 | | * In the default section of autonomousPeriodic() periodically adjust the motor powers |
| 46 | | and stop if we've gone the desired distance. |
| 47 | | {{{ |
| 48 | | if (!autoDone) { |
| 49 | | // adjust motor power every 1/4 second to keep us going straight |
| 50 | | if (autoTimer.hasPeriodPassed(0.25)) { |
| 51 | | // if we're going straight, leftDistance == rightDistance |
| 52 | | // otherwise, compute the error and adjust the motor powers to minimize it |
| 53 | | double error = leftEncoder.getDistance() - rightEncoder.getDistance(); |
| 54 | | double kP = 0.01; // proportion of error used to correct motor power |
| 55 | | // adjust motor power to try to keep left/right distances same |
| 56 | | leftPower -= kP*error; // positive error means left is going too fast |
| 57 | | rightPower -= kP*error; // right motor spins opposite direction of left |
| 58 | | leftMotor.set(leftPower); |
| 59 | | rightMotor.set(rightPower); |
| 60 | | } |
| 61 | | |
| 62 | | // For PWM Motor Controllers uncomment the following 2 lines |
| 63 | | //if ((leftEncoder.getDistance() > 36.0) || |
| 64 | | // (rightEncoder.getDistance() > 36.0)) { |
| 65 | | |
| 66 | | // For CAN motor controllers uncomment the following 4 lines |
| 67 | | //int l_raw = leftMotor.getSelectedSensorPosition(); |
| 68 | | //int r_raw = rightMotor.getSelectedSensorPosition(); |
| 69 | | //if ((raw2inches(l_raw, 360*2, 7.5) > 36.0) || |
| 70 | | // (raw2inches(r_raw, 360*2, 7.5) > 36.0)) { |
| 71 | | |
| 72 | | leftMotor.stopMotor(); |
| 73 | | rightMotor.stopMotor(); |
| 74 | | autoDone = true; |
| 75 | | } |
| 76 | | } |
| 77 | | }}} |
