diff options
author | Franklin Wei <me@fwei.tk> | 2018-12-27 13:30:52 -0500 |
---|---|---|
committer | Franklin Wei <me@fwei.tk> | 2018-12-27 13:30:52 -0500 |
commit | bfae972036fbd728f2c5ba27bd24a6a39db317ed (patch) | |
tree | 5fca1e1f185bed32ecddb2c9569fd4f6cd582dd5 | |
parent | 3d2f07050511aa5458eb9574789665879625dea0 (diff) | |
download | ftc-bfae972036fbd728f2c5ba27bd24a6a39db317ed.zip ftc-bfae972036fbd728f2c5ba27bd24a6a39db317ed.tar.gz ftc-bfae972036fbd728f2c5ba27bd24a6a39db317ed.tar.bz2 ftc-bfae972036fbd728f2c5ba27bd24a6a39db317ed.tar.xz |
more stuff
-rw-r--r-- | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java | 75 | ||||
-rwxr-xr-x | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java | 4 |
2 files changed, 53 insertions, 26 deletions
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java index 2fe7d0c..c4b2160 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java @@ -88,19 +88,19 @@ public class TankbotAutonomousv1 extends LinearOpMode { static final double COUNTS_PER_MOTOR_REV = 560; // HD Hex 1:20 static final double DRIVE_GEAR_REDUCTION = 1.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 4.0; // For figuring circumference static final double INCH_PER_REV = 19.5 / 2.54; - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (INCH_PER_REV); + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / INCH_PER_REV; // These constants define the desired driving/control characteristics // The can/should be tweaked to suite the specific robot drive train. - static final double DRIVE_SPEED = 0.2; // Nominal speed for better accuracy. - static final double TURN_SPEED = 0.2; // Our motors are really fast + static final double DRIVE_SPEED = 0.40; // Nominal speed for better accuracy. + static final double TURN_SPEED = 0.20; // Our motors are really fast - static final double HEADING_THRESHOLD = 1.0; - static final double P_TURN_COEFF = 0.08; // Larger is more responsive, but also less stable - static final double P_DRIVE_COEFF = 0.03; // Larger is more responsive, but also less stable + static final double HEADING_THRESHOLD = 0.75; + static final double P_TURN_COEFF = 0.07; // Larger is more responsive, but also less stable + static final double P_DRIVE_COEFF = 0.01; // Larger is more responsive, but also less stable + static final double D_TURN_COEFF = .019; + static final double D_DRIVE_COEFF = .001; /* Which way the robot is pointing, and which way gravity pulls, as measured by the gyro. Use gyroUpdate() to update @@ -225,7 +225,7 @@ public class TankbotAutonomousv1 extends LinearOpMode { //robot.IntakeMotor.setPower(.5); if(sample_pos >= 0) { /* knock sample off */ - double sample_distances[] = { 35, 30, 35 }; + double sample_distances[] = { 32, 30, 32 }; gyroDriveRelative(DRIVE_SPEED, sample_distances[sample_pos], 0); gyroDriveRelative(DRIVE_SPEED, -sample_distances[sample_pos], 0); } @@ -237,11 +237,11 @@ public class TankbotAutonomousv1 extends LinearOpMode { case 0: /* reverse the previous turn (we will be facing depot directly) */ gyroTurnRelative(TURN_SPEED, (sample_pos < 0 ? 0 : -sample_angles[sample_pos])); - gyroDriveRelative(DRIVE_SPEED, 12, 0); - gyroTurnRelative(TURN_SPEED, -80); + gyroDriveRelative(DRIVE_SPEED * .75, 10, 0); + gyroTurnRelative(TURN_SPEED, -85); gyroDriveRelative(DRIVE_SPEED, 20, 0); - gyroTurnRelative(TURN_SPEED, (45 + 10)); - gyroDriveRelative(DRIVE_SPEED, 16, 0); + gyroTurnRelative(TURN_SPEED, (45 - 5)); + gyroDriveRelative(DRIVE_SPEED, 14, 0); gyroTurnRelative(TURN_SPEED, 90); gyroDriveRelative(DRIVE_SPEED, 35, 0); break; @@ -253,9 +253,9 @@ public class TankbotAutonomousv1 extends LinearOpMode { } /* Eject marker */ - robot.IntakeMotor.setPower(-.5); - sleep(300); - gyroDriveRelative(DRIVE_SPEED * 2, -8, 0); + robot.IntakeMotor.setPower(-.75); + sleep(2000); + gyroDriveRelative(DRIVE_SPEED * 2, -20, 0); /* TODO: drive to crater */ @@ -307,6 +307,8 @@ public class TankbotAutonomousv1 extends LinearOpMode { // Ensure that the opmode is still active if (opModeIsActive()) { + resetDerivative(); + // Determine new target position, and pass to motor controller moveCounts = (int)(distance * COUNTS_PER_INCH); newLeftTarget = robot.DriveLeft.getCurrentPosition() + moveCounts; @@ -330,7 +332,7 @@ public class TankbotAutonomousv1 extends LinearOpMode { // adjust relative speed based on heading error. error = getError(angle); - steer = getSteer(error, P_DRIVE_COEFF); + steer = getSteer(error, P_DRIVE_COEFF, D_DRIVE_COEFF); // if driving in reverse, the motor correction also needs to be reversed if (distance < 0) @@ -390,8 +392,10 @@ public class TankbotAutonomousv1 extends LinearOpMode { */ public void gyroTurn ( double speed, double angle) { + resetDerivative(); + // keep looping while we are still active, and not on heading. - while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) { + while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF, D_TURN_COEFF)) { // Update telemetry & Allow time for other processes to run. telemetry.update(); } @@ -399,7 +403,9 @@ public class TankbotAutonomousv1 extends LinearOpMode { public void gyroTurnRelative(double speed, double dtheta) { - gyroTurn(speed, getHeading(true) + dtheta); + double angle = getHeading(true) + dtheta; + gyroTurn(speed, angle); + gyroHold(speed, angle, .25); } /** @@ -414,13 +420,15 @@ public class TankbotAutonomousv1 extends LinearOpMode { */ public void gyroHold( double speed, double angle, double holdTime) { + resetDerivative(); + ElapsedTime holdTimer = new ElapsedTime(); // keep looping while we have time remaining. holdTimer.reset(); while (opModeIsActive() && (holdTimer.time() < holdTime)) { // Update telemetry & Allow time for other processes to run. - onHeading(speed, angle, P_TURN_COEFF); + onHeading(speed, angle, P_TURN_COEFF, D_TURN_COEFF); telemetry.update(); } @@ -440,7 +448,7 @@ public class TankbotAutonomousv1 extends LinearOpMode { * @return */ - boolean onHeading(double speed, double angle, double PCoeff) { + boolean onHeading(double speed, double angle, double PCoeff, double DCoeff) { double error ; double steer ; boolean onTarget = false ; @@ -457,7 +465,7 @@ public class TankbotAutonomousv1 extends LinearOpMode { onTarget = true; } else { - steer = getSteer(error, PCoeff); + steer = getSteer(error, PCoeff, DCoeff); leftSpeed = speed * steer; rightSpeed = -leftSpeed; } @@ -491,14 +499,33 @@ public class TankbotAutonomousv1 extends LinearOpMode { return robotError; } + boolean have_deriv = false; + double last_error; + long last_timestamp; /* nanoseconds */ + private void resetDerivative() + { + have_deriv = false; + } + /** * returns desired steering force. +/- 1 range. +ve = steer left * @param error Error angle in robot relative degrees * @param PCoeff Proportional Gain Coefficient * @return */ - public double getSteer(double error, double PCoeff) { - return Range.clip(error * PCoeff, -1, 1); + public double getSteer(double error, double PCoeff, double DCoeff) { + double steer = error * PCoeff; + long time_now = System.nanoTime(); + if(have_deriv && last_timestamp != time_now) + { + double dxdt = (error - last_error) / (time_now - last_timestamp); + steer += DCoeff * dxdt; + } + steer = Range.clip(steer, -1, 1); + have_deriv = true; + last_error = error; + last_timestamp = time_now; + return steer; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java index 00af146..fac3eb4 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java @@ -193,9 +193,9 @@ public class TankbotDriver extends LinearOpMode { boolean no_lift = false; if(gamepad1.a) - dlift += 2240/12; + dlift += 2240/5; else if(gamepad1.y) - dlift -= 2240/12; + dlift -= 2240/5; else no_lift = true; |