aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFranklin Wei <me@fwei.tk>2018-12-27 13:30:52 -0500
committerFranklin Wei <me@fwei.tk>2018-12-27 13:30:52 -0500
commitbfae972036fbd728f2c5ba27bd24a6a39db317ed (patch)
tree5fca1e1f185bed32ecddb2c9569fd4f6cd582dd5
parent3d2f07050511aa5458eb9574789665879625dea0 (diff)
downloadftc-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.java75
-rwxr-xr-xTeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java4
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;