From 2afba316caca317928798a2bc24389311c2508c5 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Mon, 27 Nov 2017 15:32:04 -0500 Subject: [PATCH 1/9] Added mecanum auto --- .../ftc/teamcode/CompTeleop.java | 3 +- .../ftc/teamcode/Components/DriveTrain.java | 46 +++++++++++++++---- .../org/firstinspires/ftc/teamcode/Gyro.java | 26 ++++------- .../firstinspires/ftc/teamcode/GyroLock.java | 44 ++++++------------ .../org/firstinspires/ftc/teamcode/Robot.java | 6 +-- 5 files changed, 64 insertions(+), 61 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 2af92c0..5924dab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -21,7 +21,6 @@ public class CompTeleop extends OpMode { Gyro gyro; - GlyphIntake2 intake; DcMotor liftA, liftB; DigitalChannel limitSwitch; @@ -62,7 +61,7 @@ public void loop() { liftA.setPower(gamepad2.left_stick_y); liftB.setPower(-gamepad2.left_stick_y); - telemetry.addData("Orientation", gyro.x + "°"); + telemetry.addData("Orientation", gyro.getX() + "°"); telemetry.addData("Limit Switch", limitSwitch); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index f23a199..5d3d9b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -1,12 +1,12 @@ package org.firstinspires.ftc.teamcode.Components; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.Gyro; import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; -import org.firstinspires.ftc.teamcode.Gyro; /** * Created by Joshua Rapoport on 11/20/17. @@ -39,6 +39,10 @@ public void stop() { backRight.setPower(0); } + /** + * A single loop of teleop mode. + * @param g the controller for the drive train. + */ public void driveMechanum(Gamepad g) { double pow = 1.0; @@ -47,17 +51,41 @@ public void driveMechanum(Gamepad g) { } double x = g.left_stick_x * pow; - double y = -g.left_stick_y * pow; + double y = g.left_stick_y * pow; double turn = g.right_stick_x * pow; - if (turn < TURN_ERROR) { - lock.teleopLoop(g); + if (Math.abs(turn) < TURN_ERROR) { + lock.reactivate(); + lock.update(); + turn = lock.correction(); + } else { + lock.deactivate(); + } + + frontRight.setPower(x + y + turn); + backRight.setPower(-x + y + turn); + backLeft.setPower(-x - y + turn); + frontLeft.setPower(x - y + turn); + } + + /** + * A single loop of an auto mode + * @param x going along x-axis, from -1 to 1. + * @param y going along x-axis, from -1 to 1. + * @param turn rotating clockwise, from -1 to 1. + */ + public void driveMechanum(double x, double y, double turn) { + if (turn == 0) { + lock.reactivate(); + lock.update(); turn = lock.correction(); + } else { + lock.deactivate(); } - frontRight.setPower(x - y + turn); - backRight.setPower(-x - y + turn); - backLeft.setPower(-x + y + turn); - frontLeft.setPower(x + y + turn); + frontRight.setPower(x + y + turn); + backRight.setPower(-x + y + turn); + backLeft.setPower(-x - y + turn); + frontLeft.setPower(x - y + turn); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index 07c66d1..5538b59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -3,25 +3,16 @@ import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Input; - /** * Created by Montclair Robotics on 11/13/17. * @Author:Jack * */ -public class Gyro implements Input, Updatable { - double x, y, z; - +public class Gyro { private RRQuaternion quat; // An angle object to store the gyro angles private BNO055IMU imu; // Gyroscope public Gyro(HardwareMap map) { - x = y = z = 0; - imu = map.get(BNO055IMU.class, "gyro"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro @@ -31,22 +22,21 @@ public Gyro(HardwareMap map) { parameters.loggingTag = "IMU"; // set the logging tag imu.initialize(parameters); - imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); update(); - Updater.add(this, Priority.INPUT); } - @Override public void update() { quat = new RRQuaternion(imu.getQuaternionOrientation()); - x = (double) ((int) (10*quat.getX())) / 10; - y = (double) ((int) (10*quat.getY())) / 10; - z = (double) ((int) (10*quat.getZ())) / 10; } - @Override - public Double get() { + public Double getX() { return quat.getX(); } + public Double getY() { + return quat.getY(); + } + public Double getZ() { + return quat.getZ(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index 1d19d78..dc63fe8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -1,8 +1,5 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.firstinspires.ftc.teamcode.Components.DriveTrain; import org.montclairrobotics.sprocket.utils.Debug; /** @@ -27,36 +24,25 @@ public double correction() { return pid.getOutput(); } - public void teleopLoop(Gamepad g) { - if (Math.abs(g.right_stick_x) < DriveTrain.TURN_ERROR) { - // If the gyro-lock wasn't active before... - if (!active) { - // Reset the error record. - pid.error.reset(); - // Set the target to the current trajectory. - pid.setTarget(gyro.get()); - } - - // It sure is active now. - active = true; - // Run update loop. - loop(); - } else { - // DENIED!!! - active = false; - } - } - - public void autoLoop() { - //... - } - - public void loop() { + public void update() { gyro.update(); - pid.setInput(gyro.get()); + pid.setInput(gyro.getX()); pid.update(); Debug.msg("PID Input", pid.getInput().intValue() + "°"); Debug.msg("PID Output", (int) (100 * pid.getOutput()) + "%"); } + + public void reactivate() { + if (!active) { + pid.error.reset(); + pid.setTarget(gyro.getX()); + } + + active = true; + } + + public void deactivate() { + active = false; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 90ebf88..47e5d2a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -285,9 +285,9 @@ public void disable() { @Override public void update() { - Debug.msg("X-angle", gyro.x); - Debug.msg("Y-angle", gyro.y); - Debug.msg("Z-angle", gyro.z); + Debug.msg("X-angle", gyro.getX()); + Debug.msg("Y-angle", gyro.getY()); + Debug.msg("Z-angle", gyro.getZ()); //Debug.msg("Servo",intakeRightTop.getPosition()); Debug.msg("Switch Value:", limitSwitch.getState()); From 8aa117140a25128e5c2342c47ddb2ec22593e4da Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Mon, 27 Nov 2017 16:03:14 -0500 Subject: [PATCH 2/9] Created GyroBalance.java (not implemented yet). Probably fixed Gyro --- .../ftc/teamcode/CompTeleop.java | 11 ++-- .../ftc/teamcode/Components/DriveTrain.java | 8 +-- .../org/firstinspires/ftc/teamcode/Gyro.java | 7 +-- .../ftc/teamcode/GyroBalance.java | 52 +++++++++++++++++++ .../firstinspires/ftc/teamcode/GyroLock.java | 9 ++-- .../org/firstinspires/ftc/teamcode/Robot.java | 2 +- 6 files changed, 72 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 5924dab..7e6eb13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -27,8 +28,9 @@ public class CompTeleop extends OpMode { @Override public void init() { - gyro = new Gyro(hardwareMap); - this.driveTrain = new DriveTrain(hardwareMap, gyro); + gyro = new Gyro(hardwareMap.get(BNO055IMU.class, "gyro")); + + this.driveTrain = new DriveTrain(hardwareMap); liftA = hardwareMap.get(DcMotor.class,"lift_left"); liftB = hardwareMap.get(DcMotor.class,"lift_right"); @@ -39,13 +41,14 @@ public void init() { servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); - intake = new GlyphIntake2(servos); - gyro = new Gyro(hardwareMap); + intake = new GlyphIntake2(servos);; limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); } @Override public void loop() { + gyro.update(); + driveTrain.driveMechanum(gamepad1); if (gamepad2.a) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 5d3d9b3..1aea466 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.Components; +import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.Gyro; import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; @@ -18,8 +18,8 @@ public class DriveTrain { GyroLock lock; DcMotor frontLeft, frontRight, backLeft, backRight; - public DriveTrain(HardwareMap map, Gyro gyro) { - lock = new GyroLock(new PID(0.01, 0, 0.005), gyro); + public DriveTrain(HardwareMap map) { + lock = new GyroLock(new PID(0.01, 0, 0.005)); frontRight = map.get(DcMotor.class, "right_front"); backRight = map.get(DcMotor.class, "right_back"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index 5538b59..279c21b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.hardware.HardwareMap; /** * Created by Montclair Robotics on 11/13/17. @@ -11,9 +10,10 @@ public class Gyro { private RRQuaternion quat; // An angle object to store the gyro angles private BNO055IMU imu; // Gyroscope + public static Gyro current; - public Gyro(HardwareMap map) { - imu = map.get(BNO055IMU.class, "gyro"); + public Gyro(BNO055IMU imu) { + imu = imu; BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to @@ -24,6 +24,7 @@ public Gyro(HardwareMap map) { imu.initialize(parameters); update(); + current = this; } public void update() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java new file mode 100644 index 0000000..fd71dbc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode; + +import org.montclairrobotics.sprocket.utils.Debug; + +/** + * Created by Joshua Rapoport on 11/27/17. + */ + +public class GyroBalance { + private Gyro gyro; + private PID xPID, yPID; + private boolean active; + + public GyroBalance(PID x, PID y) { + this.gyro = Gyro.current; + this.xPID = x; + this.yPID = y; + + xPID.setInputRange(-45, 45); + yPID.setInputRange(-45, 45); + + xPID.setOutputRange(-1.0, 1.0); + yPID.setOutputRange(-1.0, 1.0); + } + + public double correctionX() { return xPID.getOutput(); } + public double correctionY() { return yPID.getOutput(); } + + public void update() { + xPID.setInput(gyro.getY()); // TODO: double check please + yPID.setInput(gyro.getZ()); // TODO: double check please + + xPID.update(); + yPID.update(); + + Debug.msg("GyroBalance: Input", xPID.getInput().intValue() + "°, " + xPID.getInput().intValue() + "°"); + Debug.msg("GyroBalance: Output", (int) (100 * xPID.getOutput()) + "%, " + (int) (100 * xPID.getOutput()) + "%"); + } + + public void reactivate() { + if (!active) { + xPID.error.reset(); + xPID.setTarget(gyro.getX()); + } + + active = true; + } + + public void deactivate() { + active = false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index dc63fe8..3faea73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -11,8 +11,8 @@ public class GyroLock { private PID pid; private boolean active; - public GyroLock(PID pid, Gyro gyro) { - this.gyro = gyro; + public GyroLock(PID pid) { + this.gyro = Gyro.current; this.pid = pid; this.active = false; @@ -25,12 +25,11 @@ public double correction() { } public void update() { - gyro.update(); pid.setInput(gyro.getX()); pid.update(); - Debug.msg("PID Input", pid.getInput().intValue() + "°"); - Debug.msg("PID Output", (int) (100 * pid.getOutput()) + "%"); + Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); + Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); } public void reactivate() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 47e5d2a..a965982 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -78,7 +78,7 @@ public void setup() { intake=new GlyphIntake2(servos); limitSwitch.setMode(DigitalChannel.Mode.INPUT); - gyro = new Gyro(hardwareMap); +// gyro = new Gyro(hardwareMap); final DriveModule[] modules = new DriveModule[4]; //Mecanum From 44a91a51d284491b18d1bac754b31b4f4d6ddb64 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Mon, 27 Nov 2017 16:09:43 -0500 Subject: [PATCH 3/9] Implemented GyroBalance in DriveTrain.autoBalance() --- .../ftc/teamcode/Components/DriveTrain.java | 12 ++++++++++-- .../firstinspires/ftc/teamcode/GyroBalance.java | 16 +++------------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 1aea466..9744338 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.Components; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.GyroBalance; import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; @@ -16,10 +16,13 @@ public class DriveTrain { public static final double TURN_ERROR = 0.1; GyroLock lock; + GyroBalance balance; + DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { - lock = new GyroLock(new PID(0.01, 0, 0.005)); + this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Test GyroLock + this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance frontRight = map.get(DcMotor.class, "right_front"); backRight = map.get(DcMotor.class, "right_back"); @@ -68,6 +71,11 @@ public void driveMechanum(Gamepad g) { frontLeft.setPower(x - y + turn); } + public void autoBalance() { + balance.update(); + driveMechanum(balance.correctionX(), balance.correctionY(), 0); + } + /** * A single loop of an auto mode * @param x going along x-axis, from -1 to 1. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java index fd71dbc..fdbba09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java @@ -16,6 +16,9 @@ public GyroBalance(PID x, PID y) { this.xPID = x; this.yPID = y; + xPID.setTarget(0.0); + yPID.setTarget(0.0); + xPID.setInputRange(-45, 45); yPID.setInputRange(-45, 45); @@ -36,17 +39,4 @@ public void update() { Debug.msg("GyroBalance: Input", xPID.getInput().intValue() + "°, " + xPID.getInput().intValue() + "°"); Debug.msg("GyroBalance: Output", (int) (100 * xPID.getOutput()) + "%, " + (int) (100 * xPID.getOutput()) + "%"); } - - public void reactivate() { - if (!active) { - xPID.error.reset(); - xPID.setTarget(gyro.getX()); - } - - active = true; - } - - public void deactivate() { - active = false; - } } From 5a3034a565e0f9f1bbcb49aee3c92877b90296a6 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Mon, 27 Nov 2017 19:14:41 -0500 Subject: [PATCH 4/9] Fixed fatal error in Gyro. CompTeleop.loop() is failing for some reason --- .../org/firstinspires/ftc/teamcode/CompTeleop.java | 1 - .../ftc/teamcode/Components/DriveTrain.java | 13 ++++++------- .../java/org/firstinspires/ftc/teamcode/Gyro.java | 5 ++--- .../org/firstinspires/ftc/teamcode/GyroBalance.java | 5 ----- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 7e6eb13..e6665d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -48,7 +48,6 @@ public void init() { @Override public void loop() { gyro.update(); - driveTrain.driveMechanum(gamepad1); if (gamepad2.a) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 9744338..938fb85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -4,7 +4,6 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.GyroBalance; import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; @@ -16,13 +15,13 @@ public class DriveTrain { public static final double TURN_ERROR = 0.1; GyroLock lock; - GyroBalance balance; +// GyroBalance balance; DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Test GyroLock - this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance +// this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance frontRight = map.get(DcMotor.class, "right_front"); backRight = map.get(DcMotor.class, "right_back"); @@ -71,10 +70,10 @@ public void driveMechanum(Gamepad g) { frontLeft.setPower(x - y + turn); } - public void autoBalance() { - balance.update(); - driveMechanum(balance.correctionX(), balance.correctionY(), 0); - } +// public void autoBalance() { +// balance.update(); +// driveMechanum(balance.correctionX(), balance.correctionY(), 0); +// } /** * A single loop of an auto mode diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index 279c21b..a212600 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -4,7 +4,7 @@ /** * Created by Montclair Robotics on 11/13/17. - * @Author:Jack + * @Author: Jack * */ public class Gyro { @@ -13,7 +13,7 @@ public class Gyro { public static Gyro current; public Gyro(BNO055IMU imu) { - imu = imu; + this.imu = imu; BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to @@ -23,7 +23,6 @@ public Gyro(BNO055IMU imu) { imu.initialize(parameters); - update(); current = this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java index fdbba09..0f0ffda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode; -import org.montclairrobotics.sprocket.utils.Debug; - /** * Created by Joshua Rapoport on 11/27/17. */ @@ -35,8 +33,5 @@ public void update() { xPID.update(); yPID.update(); - - Debug.msg("GyroBalance: Input", xPID.getInput().intValue() + "°, " + xPID.getInput().intValue() + "°"); - Debug.msg("GyroBalance: Output", (int) (100 * xPID.getOutput()) + "%, " + (int) (100 * xPID.getOutput()) + "%"); } } From abad63833a2af8cc8598dbb0593aceabaaa6b000 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Wed, 29 Nov 2017 18:28:36 -0500 Subject: [PATCH 5/9] Fixed fatal error in PID.java; Added Debug statements throughout gyro code --- .../org/firstinspires/ftc/teamcode/CompTeleop.java | 14 ++++++++++---- .../ftc/teamcode/Components/DriveTrain.java | 9 ++++++++- .../java/org/firstinspires/ftc/teamcode/Gyro.java | 7 ++++++- .../firstinspires/ftc/teamcode/GyroBalance.java | 4 ++-- .../org/firstinspires/ftc/teamcode/GyroLock.java | 9 +++++++-- .../java/org/firstinspires/ftc/teamcode/PID.java | 2 -- 6 files changed, 33 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index e6665d0..242927d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -16,8 +16,7 @@ * */ @TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL") public class CompTeleop extends OpMode { - public DriveTrain driveTrain; - DcMotor frontRight, backRight, frontLeft, backLeft; + DriveTrain driveTrain; Servo[] servos; Gyro gyro; @@ -28,7 +27,8 @@ public class CompTeleop extends OpMode { @Override public void init() { - gyro = new Gyro(hardwareMap.get(BNO055IMU.class, "gyro")); + BNO055IMU imu = hardwareMap.get(BNO055IMU.class, "gyro"); + this.gyro = new Gyro(imu); this.driveTrain = new DriveTrain(hardwareMap); @@ -41,7 +41,7 @@ public void init() { servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); - intake = new GlyphIntake2(servos);; + intake = new GlyphIntake2(servos); limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); } @@ -66,4 +66,10 @@ public void loop() { telemetry.addData("Orientation", gyro.getX() + "°"); telemetry.addData("Limit Switch", limitSwitch); } + + + @Override + public void stop() { + driveTrain.stop(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 938fb85..d12080f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -6,9 +6,11 @@ import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; +import org.montclairrobotics.sprocket.utils.Debug; /** - * Created by Joshua Rapoport on 11/20/17. + * @author Joshua Rapoport + * @version 11/27/17. */ public class DriveTrain { @@ -21,6 +23,7 @@ public class DriveTrain { public DriveTrain(HardwareMap map) { this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Test GyroLock + // this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance frontRight = map.get(DcMotor.class, "right_front"); @@ -46,6 +49,8 @@ public void stop() { * @param g the controller for the drive train. */ public void driveMechanum(Gamepad g) { + Debug.msg("DriveTrain", "update in progress..."); + double pow = 1.0; if (g.left_bumper) { @@ -68,6 +73,8 @@ public void driveMechanum(Gamepad g) { backRight.setPower(-x + y + turn); backLeft.setPower(-x - y + turn); frontLeft.setPower(x - y + turn); + + Debug.msg("DriveTrain", "update complete"); } // public void autoBalance() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index a212600..8bd348e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -2,15 +2,18 @@ import com.qualcomm.hardware.bosch.BNO055IMU; +import org.montclairrobotics.sprocket.utils.Debug; + /** * Created by Montclair Robotics on 11/13/17. * @Author: Jack * */ public class Gyro { + public static Gyro current; + private RRQuaternion quat; // An angle object to store the gyro angles private BNO055IMU imu; // Gyroscope - public static Gyro current; public Gyro(BNO055IMU imu) { this.imu = imu; @@ -27,7 +30,9 @@ public Gyro(BNO055IMU imu) { } public void update() { + Debug.msg("Gyro", "update in progress..."); quat = new RRQuaternion(imu.getQuaternionOrientation()); + Debug.msg("Gyro", "update complete"); } public Double getX() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java index 0f0ffda..87168ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java @@ -1,13 +1,13 @@ package org.firstinspires.ftc.teamcode; /** - * Created by Joshua Rapoport on 11/27/17. + * @author Joshua Rapoport + * @version 11/27/17. */ public class GyroBalance { private Gyro gyro; private PID xPID, yPID; - private boolean active; public GyroBalance(PID x, PID y) { this.gyro = Gyro.current; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index 3faea73..1490989 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -3,7 +3,8 @@ import org.montclairrobotics.sprocket.utils.Debug; /** - * Created by Joshua Rapoport on 11/16/17. + * @author Joshua Rapoport + * @version 11/27/17. */ public class GyroLock { @@ -25,9 +26,13 @@ public double correction() { } public void update() { + Debug.msg("GyroLock", "update in progress..."); + pid.setInput(gyro.getX()); pid.update(); + Debug.msg("GyroLock", "update complete"); + Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); } @@ -42,6 +47,6 @@ public void reactivate() { } public void deactivate() { - active = false; + if (active) active = false; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java index b9c7848..17c59b6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -106,8 +106,6 @@ else if (outRange.compareTo(potentialI) < 0) double out = (P * error.current * dTime()) + (I * error.total) + (D * -dInput / dTime()); - setInput(newInput); - if (outRange.compareTo(out) > 0) out = outRange.max; else if (outRange.compareTo(out) < 0) From d3d2de8948dcd41bf054c7eeea6300add586c152 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Thu, 30 Nov 2017 21:56:49 -0500 Subject: [PATCH 6/9] PID runs! Still needs callibration --- .../ftc/teamcode/Components/DriveTrain.java | 7 +------ .../java/org/firstinspires/ftc/teamcode/Gyro.java | 4 ---- .../org/firstinspires/ftc/teamcode/GyroLock.java | 12 ++++-------- .../java/org/firstinspires/ftc/teamcode/PID.java | 15 ++++++++++++--- 4 files changed, 17 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index d12080f..099902b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -6,7 +6,6 @@ import org.firstinspires.ftc.teamcode.GyroLock; import org.firstinspires.ftc.teamcode.PID; -import org.montclairrobotics.sprocket.utils.Debug; /** * @author Joshua Rapoport @@ -22,7 +21,7 @@ public class DriveTrain { DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { - this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Test GyroLock + this.lock = new GyroLock(new PID(0.01, 0, 0.01)); // TODO: Test GyroLock // this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance @@ -49,8 +48,6 @@ public void stop() { * @param g the controller for the drive train. */ public void driveMechanum(Gamepad g) { - Debug.msg("DriveTrain", "update in progress..."); - double pow = 1.0; if (g.left_bumper) { @@ -73,8 +70,6 @@ public void driveMechanum(Gamepad g) { backRight.setPower(-x + y + turn); backLeft.setPower(-x - y + turn); frontLeft.setPower(x - y + turn); - - Debug.msg("DriveTrain", "update complete"); } // public void autoBalance() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index 8bd348e..a556e76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -2,8 +2,6 @@ import com.qualcomm.hardware.bosch.BNO055IMU; -import org.montclairrobotics.sprocket.utils.Debug; - /** * Created by Montclair Robotics on 11/13/17. * @Author: Jack @@ -30,9 +28,7 @@ public Gyro(BNO055IMU imu) { } public void update() { - Debug.msg("Gyro", "update in progress..."); quat = new RRQuaternion(imu.getQuaternionOrientation()); - Debug.msg("Gyro", "update complete"); } public Double getX() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index 1490989..ff25809 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode; -import org.montclairrobotics.sprocket.utils.Debug; - /** * @author Joshua Rapoport * @version 11/27/17. @@ -17,6 +15,8 @@ public GyroLock(PID pid) { this.pid = pid; this.active = false; + pid.setTarget(gyro.getX()); + pid.setInputRange(-180, 180); pid.setOutputRange(-1.0, 1.0); } @@ -26,15 +26,11 @@ public double correction() { } public void update() { - Debug.msg("GyroLock", "update in progress..."); - pid.setInput(gyro.getX()); pid.update(); - Debug.msg("GyroLock", "update complete"); - - Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); - Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); +// Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); +// Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); } public void reactivate() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java index 17c59b6..dff7e03 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -37,7 +37,16 @@ public class Error { public Error() { reset(); } - public void update() { + public void updateCurrent() { + if (Math.abs(target - input) <= 180) { + current = target - input; + } else if (target > input) { + current = 360 - Math.abs(target) - Math.abs(input); + } else { + current = Math.abs(target) - Math.abs(input) - 360; + } + } + public void updateTotal() { total += current * dTime(); } public void reset() { current = total = 0; } @@ -83,7 +92,7 @@ public double getOutput() { return output; } private double setOutput(double newInput) { - error.current = target - newInput; + error.updateCurrent(); double dInput = newInput - input; double diff = inRange.distance(); @@ -92,7 +101,7 @@ private double setOutput(double newInput) { dInput = ((dInput - inRange.min) % diff + diff) % diff + inRange.min; } - error.update(); + error.updateTotal(); if (I != 0) { double potentialI = (error.current + error.total) * I; From 819a438021c9a922f1d3a2feb091ac4b354cd1f3 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Fri, 1 Dec 2017 22:51:30 -0500 Subject: [PATCH 7/9] Fixed two more Gyro-breaking bugs; reorganized PID --- .../ftc/teamcode/CompTeleop.java | 3 +- .../ftc/teamcode/Components/DriveTrain.java | 9 +- .../org/firstinspires/ftc/teamcode/Gyro.java | 9 +- .../ftc/teamcode/GyroBalance.java | 3 - .../firstinspires/ftc/teamcode/GyroLock.java | 1 - .../org/firstinspires/ftc/teamcode/PID.java | 126 +++++++++--------- 6 files changed, 76 insertions(+), 75 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 242927d..8f6febd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -64,7 +64,8 @@ public void loop() { liftB.setPower(-gamepad2.left_stick_y); telemetry.addData("Orientation", gyro.getX() + "°"); - telemetry.addData("Limit Switch", limitSwitch); +// telemetry.addData("Limit Switch", limitSwitch); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 099902b..99fc263 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -21,9 +21,9 @@ public class DriveTrain { DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { - this.lock = new GyroLock(new PID(0.01, 0, 0.01)); // TODO: Test GyroLock + this.lock = new GyroLock(new PID(0.02, 0, 0.01)); // TODO: Calibrate GyroLock -// this.balance = new GyroBalance(new PID(0,0,0), new PID(0,0,0)); // TODO: Test GyroBalance +// this.balance = new GyroBalance(new PID(0, 0, 0), new PID(0, 0, 0)); // TODO: Calibrate GyroBalance frontRight = map.get(DcMotor.class, "right_front"); backRight = map.get(DcMotor.class, "right_back"); @@ -58,10 +58,11 @@ public void driveMechanum(Gamepad g) { double y = g.left_stick_y * pow; double turn = g.right_stick_x * pow; - if (Math.abs(turn) < TURN_ERROR) { + //TODO: Press and hold `A` to use GyroLock + if (g.a && Math.abs(turn) < TURN_ERROR) { lock.reactivate(); lock.update(); - turn = lock.correction(); + turn = lock.correction() * pow; } else { lock.deactivate(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index a556e76..e766de0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -15,7 +15,7 @@ public class Gyro { public Gyro(BNO055IMU imu) { this.imu = imu; - + this.quat = new RRQuaternion(0, 0, 0, 0); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to parameters.calibrationDataFile = "gyroData.json"; // specify the gyro calibration file, see @GyroCalibration @@ -32,7 +32,12 @@ public void update() { } public Double getX() { - return quat.getX(); + double x = quat.getX(); + + if (x > 0) + return quat.getX() - 180; + + return quat.getX() + 180; } public Double getY() { return quat.getY(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java index 87168ac..6bd7c30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java @@ -30,8 +30,5 @@ public GyroBalance(PID x, PID y) { public void update() { xPID.setInput(gyro.getY()); // TODO: double check please yPID.setInput(gyro.getZ()); // TODO: double check please - - xPID.update(); - yPID.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index ff25809..0f393ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -27,7 +27,6 @@ public double correction() { public void update() { pid.setInput(gyro.getX()); - pid.update(); // Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); // Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java index dff7e03..0cab528 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -1,7 +1,8 @@ package org.firstinspires.ftc.teamcode; /** - * Created by Joshua Rapoport on 11/14/17. + * @author Joshua Rapoport + * @version 12/1/2017 */ public class PID { @@ -19,37 +20,34 @@ public Range(double a, double b) { public double distance() { return Math.abs(max - min); } - public boolean contains(double x) { - return x >= min && x <= max; - } public int compareTo(double x) { - if (contains(x)) - return 0; - else if (x < min) + if (x < min) return -1; - else + else if (x > max) return 1; + else + return 0; } } public class Error { - double current, total; + double current, rate, total; + public Error() { reset(); } - public void updateCurrent() { - if (Math.abs(target - input) <= 180) { - current = target - input; - } else if (target > input) { - current = 360 - Math.abs(target) - Math.abs(input); - } else { - current = Math.abs(target) - Math.abs(input) - 360; - } - } - public void updateTotal() { + + /** + * Update the current error, error rate, and total error. + * @param i new input with which to update. + */ + public void update(double i) { + rate = ((target - i) - current) / dTime(); + current = target - i; total += current * dTime(); } - public void reset() { current = total = 0; } + + public void reset() { current = rate = total = 0; } } private double P, I, D; @@ -57,6 +55,7 @@ public void updateTotal() { protected Range inRange; /** The acceptable range of output. */ protected Range outRange; + /** An object that handles error due to input-target difference. */ protected Error error; @@ -64,6 +63,7 @@ public void updateTotal() { private Double input; /** The robot's target, to be compared with the input. */ private double target; + /** The calculated output to correct the error. */ private double output; private long lastUpdateTime; @@ -85,52 +85,57 @@ public PID(double p, double i, double d) { this.input = this.target = this.output = 0.0; - update(); + this.lastUpdateTime = System.currentTimeMillis() / 1000; } - public double getOutput() { - return output; + /** Create a copy of a PID object with identical properties */ + public PID(PID pid) { + this.P = pid.P; + this.I = pid.I; + this.D = pid.D; + + this.inRange = pid.inRange; + this.outRange = pid.outRange; + + this.error = pid.error; + + this.input = pid.input; + this.target = pid.target; + this.output = pid.output; + + this.lastUpdateTime = pid.lastUpdateTime; } - private double setOutput(double newInput) { - error.updateCurrent(); - double dInput = newInput - input; - double diff = inRange.distance(); - if (diff != 0) { - error.current = ((error.current - inRange.min) % diff + diff) % diff + inRange.min; - dInput = ((dInput - inRange.min) % diff + diff) % diff + inRange.min; - } + /** Use this method to get a new output value. */ + public void setInput(double i) { + error.update(i); + this.updateOutput(); + this.input = i; + } - error.updateTotal(); + private void updateOutput() { + double d = inRange.distance(); + if (d != 0) { + error.current = ((error.current - inRange.min) % d + d) % d + inRange.min; + } if (I != 0) { - double potentialI = (error.current + error.total) * I; + double potentialI = I * error.total; + if (outRange.compareTo(potentialI) > 0) error.total = outRange.max / I; else if (outRange.compareTo(potentialI) < 0) error.total = outRange.min / I; - else - error.total += error.current; } - double out = (P * error.current * dTime()) + (I * error.total) + (D * -dInput / dTime()); - - if (outRange.compareTo(out) > 0) - out = outRange.max; - else if (outRange.compareTo(out) < 0) - out = outRange.min; + this.output = (P * error.current) + (I * error.total) + (D * error.rate); - return out; - } - - /** @return a reference to a different PID object with identical properties */ - public PID copy() { - PID c = new PID(P, I, D); - c.setInputRange(inRange.min, inRange.max); - c.setOutputRange(outRange.min, outRange.max); - c.setTarget(target); + if (outRange.compareTo(output) > 0) + this.output = outRange.max; + else if (outRange.compareTo(output) < 0) + this.output = outRange.min; - return c; + lastUpdateTime = System.currentTimeMillis() / 1000; } public void setPID(double p, double i, double d) { @@ -161,24 +166,17 @@ public void setOutputRange(double a, double b) { public Double getInput() { return input; } - - /** Use this method to get a new output value. */ - public void setInput(double i) { - this.output = setOutput(i); - this.input = i; - } - public double getTarget() { return target; } - public void setTarget(double t) { this.target = target; } + public double getOutput() { + return output; + } + + public void setTarget(double t) { this.target = t; } /** @return the time difference from the last update. */ public double dTime() { - return System.currentTimeMillis() - lastUpdateTime; - } - - public void update() { - lastUpdateTime = System.currentTimeMillis(); + return (System.currentTimeMillis() / 1000) - lastUpdateTime; } } From 4b9ee91e1fae9b32a2a54e177e7e7d811bf86969 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Sat, 2 Dec 2017 07:41:27 -0500 Subject: [PATCH 8/9] Changed parameters for DriveTrain.driveMecanum --- .../ftc/teamcode/CompTeleop.java | 3 +- .../ftc/teamcode/Components/DriveTrain.java | 60 +++++++------------ 2 files changed, 22 insertions(+), 41 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 8f6febd..f04b837 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -48,7 +48,8 @@ public void init() { @Override public void loop() { gyro.update(); - driveTrain.driveMechanum(gamepad1); + driveTrain.halfPower(gamepad1.left_bumper); + driveTrain.driveMechanum(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.right_stick_x); if (gamepad2.a) intake.openBottom(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 99fc263..1879dcb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.Components; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.GyroLock; @@ -13,18 +12,21 @@ */ public class DriveTrain { - public static final double TURN_ERROR = 0.1; + public static final double TURN_ERROR = 0.05; GyroLock lock; // GyroBalance balance; + private double power; DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { - this.lock = new GyroLock(new PID(0.02, 0, 0.01)); // TODO: Calibrate GyroLock + this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Calibrate GyroLock // this.balance = new GyroBalance(new PID(0, 0, 0), new PID(0, 0, 0)); // TODO: Calibrate GyroBalance + this.power = 0; + frontRight = map.get(DcMotor.class, "right_front"); backRight = map.get(DcMotor.class, "right_back"); backLeft = map.get(DcMotor.class, "left_back"); @@ -43,49 +45,22 @@ public void stop() { backRight.setPower(0); } - /** - * A single loop of teleop mode. - * @param g the controller for the drive train. - */ - public void driveMechanum(Gamepad g) { - double pow = 1.0; - - if (g.left_bumper) { - pow = 0.5; - } - - double x = g.left_stick_x * pow; - double y = g.left_stick_y * pow; - double turn = g.right_stick_x * pow; - - //TODO: Press and hold `A` to use GyroLock - if (g.a && Math.abs(turn) < TURN_ERROR) { - lock.reactivate(); - lock.update(); - turn = lock.correction() * pow; + public void halfPower(boolean h) { + if (h) { + power = 0.5; } else { - lock.deactivate(); + power = 1.0; } - - frontRight.setPower(x + y + turn); - backRight.setPower(-x + y + turn); - backLeft.setPower(-x - y + turn); - frontLeft.setPower(x - y + turn); } -// public void autoBalance() { -// balance.update(); -// driveMechanum(balance.correctionX(), balance.correctionY(), 0); -// } - /** - * A single loop of an auto mode + * A single loop. * @param x going along x-axis, from -1 to 1. * @param y going along x-axis, from -1 to 1. * @param turn rotating clockwise, from -1 to 1. */ public void driveMechanum(double x, double y, double turn) { - if (turn == 0) { + if (Math.abs(turn) < TURN_ERROR) { lock.reactivate(); lock.update(); turn = lock.correction(); @@ -93,9 +68,14 @@ public void driveMechanum(double x, double y, double turn) { lock.deactivate(); } - frontRight.setPower(x + y + turn); - backRight.setPower(-x + y + turn); - backLeft.setPower(-x - y + turn); - frontLeft.setPower(x - y + turn); + frontRight.setPower(power * (x + y + turn)); + backRight.setPower(power * (-x + y + turn)); + backLeft.setPower(power * (-x - y + turn)); + frontLeft.setPower(power * (x - y + turn)); } + +// public void autoBalance() { +// balance.update(); +// driveMechanum(balance.correctionX(), balance.correctionY(), 0); +// } } From 03cc9f8ad53a82780968fe7e0fa03188944b567d Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Sat, 2 Dec 2017 16:56:49 -0500 Subject: [PATCH 9/9] Stuff --- .../ftc/teamcode/Auto/DefaultHardwareMap.java | 2 +- .../ftc/teamcode/CompTeleop.java | 31 +++---- .../ftc/teamcode/Components/DriveTrain.java | 39 ++++---- .../ftc/teamcode/Components/GlyphIntake2.java | 14 ++- .../ftc/teamcode/Components/GlyphLift.java | 5 +- .../org/firstinspires/ftc/teamcode/Gyro.java | 16 ++-- .../firstinspires/ftc/teamcode/GyroLock.java | 29 ++++-- .../org/firstinspires/ftc/teamcode/Name.java | 24 +++++ .../org/firstinspires/ftc/teamcode/PID.java | 89 +++++-------------- .../ftc/teamcode/RRQuaternion.java | 4 + .../org/firstinspires/ftc/teamcode/Robot.java | 8 +- 11 files changed, 120 insertions(+), 141 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java index 5b4d80f..282c2d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java @@ -74,7 +74,7 @@ public void init(HardwareMap ahwMap){ servos[2] = ahwMap.get(Servo.class, "intake_right_bottom"); servos[3] = ahwMap.get(Servo.class, "intake_left_bottom"); - lift=new GlyphIntake2(servos); + lift=new GlyphIntake2(ahwMap); sensorColor = hwMap.get(ColorSensor.class, "colorSensor"); limitSwitch = hwMap.get(DigitalChannel.class, "limit_switch_1"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index f04b837..1951d27 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -5,7 +5,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DigitalChannel; -import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Components.DriveTrain; import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; @@ -17,8 +16,6 @@ @TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL") public class CompTeleop extends OpMode { DriveTrain driveTrain; - Servo[] servos; - Gyro gyro; GlyphIntake2 intake; @@ -27,21 +24,15 @@ public class CompTeleop extends OpMode { @Override public void init() { - BNO055IMU imu = hardwareMap.get(BNO055IMU.class, "gyro"); + BNO055IMU imu = hardwareMap.get(BNO055IMU.class, Name.SENSOR_GYRO); this.gyro = new Gyro(imu); this.driveTrain = new DriveTrain(hardwareMap); - liftA = hardwareMap.get(DcMotor.class,"lift_left"); - liftB = hardwareMap.get(DcMotor.class,"lift_right"); - - servos = new Servo[4]; - servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); - servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); - servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); - servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); + liftA = hardwareMap.get(DcMotor.class, Name.LIFT_L); + liftB = hardwareMap.get(DcMotor.class, Name.LIFT_R); - intake = new GlyphIntake2(servos); + intake = new GlyphIntake2(hardwareMap); limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); } @@ -51,21 +42,23 @@ public void loop() { driveTrain.halfPower(gamepad1.left_bumper); driveTrain.driveMechanum(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.right_stick_x); - if (gamepad2.a) + if (gamepad2.a) { intake.openBottom(); - else + } else { intake.closeBottom(); + } - if (gamepad2.b) + if (gamepad2.b) { intake.openTop(); - else + } else { intake.closeTop(); + } liftA.setPower(gamepad2.left_stick_y); liftB.setPower(-gamepad2.left_stick_y); - telemetry.addData("Orientation", gyro.getX() + "°"); -// telemetry.addData("Limit Switch", limitSwitch); + telemetry.addData("Gyroscope", gyro); + telemetry.addData("Limit Switch", limitSwitch); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index 1879dcb..962e698 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -4,11 +4,13 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.GyroLock; +import org.firstinspires.ftc.teamcode.Name; import org.firstinspires.ftc.teamcode.PID; /** + * Created by Montclair Robotics on 11/27/17 * @author Joshua Rapoport - * @version 11/27/17. + * @version 12/2/17. */ public class DriveTrain { @@ -21,16 +23,16 @@ public class DriveTrain { DcMotor frontLeft, frontRight, backLeft, backRight; public DriveTrain(HardwareMap map) { - this.lock = new GyroLock(new PID(0.01, 0, 0.005)); // TODO: Calibrate GyroLock + this.lock = new GyroLock(new PID(0.04, 0, 0.02)); // TODO: Calibrate GyroLock // this.balance = new GyroBalance(new PID(0, 0, 0), new PID(0, 0, 0)); // TODO: Calibrate GyroBalance this.power = 0; - frontRight = map.get(DcMotor.class, "right_front"); - backRight = map.get(DcMotor.class, "right_back"); - backLeft = map.get(DcMotor.class, "left_back"); - frontLeft = map.get(DcMotor.class, "left_front"); + frontRight = map.get(DcMotor.class, Name.DRIVETRAIN_FR); + backRight = map.get(DcMotor.class, Name.DRIVETRAIN_BR); + backLeft = map.get(DcMotor.class, Name.DRIVETRAIN_BL); + frontLeft = map.get(DcMotor.class, Name.DRIVETRAIN_FL); frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); @@ -39,17 +41,14 @@ public DriveTrain(HardwareMap map) { } public void stop() { - frontLeft.setPower(0); - frontRight.setPower(0); - backLeft.setPower(0); - backRight.setPower(0); + driveMechanum(0, 0, 0); } public void halfPower(boolean h) { + power = 0.8; + if (h) { - power = 0.5; - } else { - power = 1.0; + power = 0.4; } } @@ -61,11 +60,14 @@ public void halfPower(boolean h) { */ public void driveMechanum(double x, double y, double turn) { if (Math.abs(turn) < TURN_ERROR) { - lock.reactivate(); - lock.update(); + if (lock.init()) { + lock.setTarget(); + } + + lock.setInput(); turn = lock.correction(); } else { - lock.deactivate(); + lock.stop(); } frontRight.setPower(power * (x + y + turn)); @@ -73,9 +75,4 @@ public void driveMechanum(double x, double y, double turn) { backLeft.setPower(power * (-x - y + turn)); frontLeft.setPower(power * (x - y + turn)); } - -// public void autoBalance() { -// balance.update(); -// driveMechanum(balance.correctionX(), balance.correctionY(), 0); -// } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java index 0b9cb8a..5280e27 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java @@ -1,12 +1,14 @@ package org.firstinspires.ftc.teamcode.Components; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.Name; import org.firstinspires.ftc.teamcode.Robot; /** * Created by Montclair Robotics on 11/13/17. - * @Author:Rafi + * @author Rafi * */ public class GlyphIntake2 { @@ -19,8 +21,14 @@ public class GlyphIntake2 { private Servo[] servos; - public GlyphIntake2(Servo... servos) { - this.servos = servos; + public GlyphIntake2(HardwareMap map) { + this.servos = new Servo[] { + map.get(Servo.class, Name.INTAKE_TR), + map.get(Servo.class, Name.INTAKE_TL), + map.get(Servo.class, Name.INTAKE_BR), + map.get(Servo.class, Name.INTAKE_BL) + }; + servoOpen=new double[4]; servoClose=new double[4]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java index 53f6ca5..34d98e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java @@ -2,15 +2,12 @@ import com.qualcomm.robotcore.hardware.DcMotor; -import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.ftc.FTCMotor; import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.SEncoder; -import org.montclairrobotics.sprocket.utils.PID; /** * Created by Montclair Robotics on 11/13/17. - * @Author:Garrett + * @author Garrett * */ public class GlyphLift extends Module{ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index e766de0..9331e8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -4,7 +4,7 @@ /** * Created by Montclair Robotics on 11/13/17. - * @Author: Jack + * @author Jack * */ public class Gyro { @@ -15,7 +15,8 @@ public class Gyro { public Gyro(BNO055IMU imu) { this.imu = imu; - this.quat = new RRQuaternion(0, 0, 0, 0); + this.quat = new RRQuaternion(); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to parameters.calibrationDataFile = "gyroData.json"; // specify the gyro calibration file, see @GyroCalibration @@ -32,12 +33,7 @@ public void update() { } public Double getX() { - double x = quat.getX(); - - if (x > 0) - return quat.getX() - 180; - - return quat.getX() + 180; + return -quat.getX(); } public Double getY() { return quat.getY(); @@ -45,4 +41,8 @@ public Double getY() { public Double getZ() { return quat.getZ(); } + + public String toString() { + return "{x: " + getX().intValue() + "°,\ty: " + getY().intValue() + "°,\tz: " + getZ().intValue() + "°}"; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index 0f393ea..fda5788 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -25,23 +25,34 @@ public double correction() { return pid.getOutput(); } - public void update() { - pid.setInput(gyro.getX()); + public void setTarget() { + setTarget(gyro.getX()); + } -// Debug.msg("GyroLock: Input", pid.getInput().intValue() + "°"); -// Debug.msg("GyroLock: Output", (int) (100 * pid.getOutput()) + "%"); + public void setTarget(double t) { + pid.setTarget(t); } - public void reactivate() { + public void setInput() { + pid.setInput(gyro.getX()); + } + + public boolean init() { if (!active) { pid.error.reset(); - pid.setTarget(gyro.getX()); + active = true; + return true; } - active = true; + return false; } - public void deactivate() { - if (active) active = false; + public boolean stop() { + if (active) { + active = false; + return true; + } + + return false; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java new file mode 100644 index 0000000..30e60fd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Created by Montclair Robotics on 12/2/17. + * @author Joshua Rapoport + */ + +public final class Name { + public static final String DRIVETRAIN_FL = "left_front"; + public static final String DRIVETRAIN_FR = "right_front"; + public static final String DRIVETRAIN_BL = "left_back"; + public static final String DRIVETRAIN_BR = "right_back"; + + public static final String LIFT_L = "lift_left"; + public static final String LIFT_R = "lift_right"; + + public static final String INTAKE_TL = "intake_left_top"; + public static final String INTAKE_TR = "intake_right_top"; + public static final String INTAKE_BL = "intake_left_bottom"; + public static final String INTAKE_BR = "intake_right_bottom"; + + public static final String SENSOR_COLOR = "sensor_color"; + public static final String SENSOR_GYRO = "gyro"; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java index 0cab528..8a30a80 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -1,26 +1,27 @@ package org.firstinspires.ftc.teamcode; /** + * Created by Montclair Robotics on 11/10/2017 * @author Joshua Rapoport - * @version 12/1/2017 + * @version 12/2/2017 */ public class PID { - public class Range { + class Range { double min, max; - public Range() { + Range() { this.min = this.max = 0; } - public Range(double a, double b) { + Range(double a, double b) { this.min = a; this.max = b; } - public double distance() { + double distance() { return Math.abs(max - min); } - public int compareTo(double x) { + int compareTo(double x) { if (x < min) return -1; else if (x > max) @@ -30,24 +31,13 @@ else if (x > max) } } - public class Error { + class Error { double current, rate, total; - public Error() { + Error() { reset(); } - - /** - * Update the current error, error rate, and total error. - * @param i new input with which to update. - */ - public void update(double i) { - rate = ((target - i) - current) / dTime(); - current = target - i; - total += current * dTime(); - } - - public void reset() { current = rate = total = 0; } + void reset() { current = rate = total = 0; } } private double P, I, D; @@ -88,36 +78,16 @@ public PID(double p, double i, double d) { this.lastUpdateTime = System.currentTimeMillis() / 1000; } - /** Create a copy of a PID object with identical properties */ - public PID(PID pid) { - this.P = pid.P; - this.I = pid.I; - this.D = pid.D; - - this.inRange = pid.inRange; - this.outRange = pid.outRange; - - this.error = pid.error; - - this.input = pid.input; - this.target = pid.target; - this.output = pid.output; - - this.lastUpdateTime = pid.lastUpdateTime; - } - /** Use this method to get a new output value. */ public void setInput(double i) { - error.update(i); - this.updateOutput(); + this.updateOutput(i); this.input = i; } - private void updateOutput() { + private void updateOutput(double i) { double d = inRange.distance(); - if (d != 0) { - error.current = ((error.current - inRange.min) % d + d) % d + inRange.min; - } + error.current = ((target - i - d / 2) % d + d) % d + d / 2; + error.rate = (dTime() > 0) ? ((target - i) - error.current) / dTime() : 0; if (I != 0) { double potentialI = I * error.total; @@ -126,34 +96,18 @@ private void updateOutput() { error.total = outRange.max / I; else if (outRange.compareTo(potentialI) < 0) error.total = outRange.min / I; + else + error.total += error.current * dTime(); } - this.output = (P * error.current) + (I * error.total) + (D * error.rate); + lastUpdateTime = System.currentTimeMillis(); if (outRange.compareTo(output) > 0) this.output = outRange.max; else if (outRange.compareTo(output) < 0) this.output = outRange.min; - - lastUpdateTime = System.currentTimeMillis() / 1000; - } - - public void setPID(double p, double i, double d) { - P = p; - I = i; - D = d; - } - /** @return P, the proportional constant. */ - public double getP() { - return P; - } - /** @return I, the integral constant. */ - public double getI() { - return I; - } - /** @return D, the derivative constant. */ - public double getD() { - return D; + else + this.output = (P * error.current) + (I * error.total) + (D * error.rate); } public void setInputRange(double a, double b) { @@ -166,9 +120,6 @@ public void setOutputRange(double a, double b) { public Double getInput() { return input; } - public double getTarget() { - return target; - } public double getOutput() { return output; } @@ -177,6 +128,6 @@ public double getOutput() { /** @return the time difference from the last update. */ public double dTime() { - return (System.currentTimeMillis() / 1000) - lastUpdateTime; + return (System.currentTimeMillis()) - lastUpdateTime; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java index 6ffcdd6..ff3fb4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java @@ -26,6 +26,10 @@ public class RRQuaternion { public double w, x, y, z; + public RRQuaternion() { + w = x = y = z = 0.0; + } + public RRQuaternion(double w, double x, double y, double z) { this.w = w; this.x = x; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index a965982..c4a89ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -66,16 +66,10 @@ public void setup() { frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); - servos = new Servo[4]; - servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); - servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); - servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); - servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); - liftLeft = new FTCMotor("lift_left"); liftRight = new FTCMotor("lift_right"); lift = new GlyphLift(liftLeft, liftRight); - intake=new GlyphIntake2(servos); + intake=new GlyphIntake2(hardwareMap); limitSwitch.setMode(DigitalChannel.Mode.INPUT); // gyro = new Gyro(hardwareMap);