aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFranklin Wei <me@fwei.tk>2018-12-26 20:43:34 -0500
committerFranklin Wei <me@fwei.tk>2018-12-26 20:43:34 -0500
commit3d2f07050511aa5458eb9574789665879625dea0 (patch)
tree2c597f6c0d83d89bd67850c85e715e838f9bc127
parent9606b23963a5dea4fbe4afde74cd9c2ff9060de4 (diff)
downloadftc-3d2f07050511aa5458eb9574789665879625dea0.zip
ftc-3d2f07050511aa5458eb9574789665879625dea0.tar.gz
ftc-3d2f07050511aa5458eb9574789665879625dea0.tar.bz2
ftc-3d2f07050511aa5458eb9574789665879625dea0.tar.xz
Begin autonomous mode; partially working
-rw-r--r--FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java2
-rw-r--r--FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java2
-rwxr-xr-xTeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java7
-rw-r--r--TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java504
-rwxr-xr-xTeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java (renamed from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java)179
5 files changed, 574 insertions, 120 deletions
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
index 9102a10..877eb3c 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
@@ -55,7 +55,7 @@ import java.util.Locale;
* @see <a href="http://www.adafruit.com/products/2472">Adafruit IMU</a>
*/
@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
-@Disabled // Comment this out to add to the opmode list
+//@Disabled // Comment this out to add to the opmode list
public class SensorBNO055IMU extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
index 064e8a4..900f487 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
@@ -99,7 +99,7 @@ import java.util.Locale;
* @see <a href="https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST_BNO055_DS000_14.pdf">BNO055 specification</a>
*/
@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
-@Disabled // Uncomment this to add to the opmode list
+//@Disabled // Uncomment this to add to the opmode list
public class SensorBNO055IMUCalibration extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
index 5d8902b..c0344d2 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
@@ -113,14 +113,13 @@ public class HardwarePushbot
// Set all motors to run without encoders.
// May want to use RUN_USING_ENCODERS if encoders are installed.
- DriveLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ DriveLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // autonomous will change these
DriveRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
IntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- LiftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
- /* stop these motors actively */
+ LiftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); // always, to maintain position
- //GlyphMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ /* stop these motors actively */
LiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
new file mode 100644
index 0000000..2fe7d0c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
@@ -0,0 +1,504 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
+import com.qualcomm.hardware.bosch.NaiveAccelerationIntegrator;
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+
+/**
+ * This file illustrates the concept of driving a path based on Gyro heading and encoder counts.
+ * It uses the common Pushbot hardware class to define the drive on the robot.
+ * The code is structured as a LinearOpMode
+ *
+ * The code REQUIRES that you DO have encoders on the wheels,
+ * otherwise you would use: PushbotAutoDriveByTime;
+ *
+ * This code ALSO requires that you have a Modern Robotics I2C imu with the name "imu"
+ * otherwise you would use: PushbotAutoDriveByEncoder;
+ *
+ * This code requires that the drive Motors have been configured such that a positive
+ * power command moves them forward, and causes the encoders to count UP.
+ *
+ * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
+ *
+ * In order to calibrate the Gyro correctly, the robot must remain stationary during calibration.
+ * This is performed when the INIT button is pressed on the Driver Station.
+ * This code assumes that the robot is stationary when the INIT button is pressed.
+ * If this is not the case, then the INIT should be performed again.
+ *
+ * Note: in this example, all angles are referenced to the initial coordinate frame set during the
+ * the Gyro Calibration process, or whenever the program issues a resetZAxisIntegrator() call on the Gyro.
+ *
+ * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis,
+ * which means that a Positive rotation is Counter Clock Wise, looking down on the field.
+ * This is consistent with the FTC field coordinate conventions set out in the document:
+ * ftc_app\doc\tutorial\FTC_FieldCoordinateSystemDefinition.pdf
+ *
+ * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="TankBot: Autonomous v1", group="Tankbot")
+
+public class TankbotAutonomousv1 extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
+ BNO055IMU imu = null; // Additional Gyro device
+
+ 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);
+
+ // 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 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
+
+ /* Which way the robot is pointing, and which way gravity pulls, as measured by the gyro.
+ Use gyroUpdate() to update
+ */
+ /* angles are:
+ Z: 1st: heading (CCW positive)
+ Y: 2nd pitch (nose down positive)
+ X: 3rd: roll (roll right positive)
+ */
+ Orientation angles;
+ Acceleration gravity;
+
+ private void gyroUpdate() {
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ gravity = imu.getGravity();
+ }
+
+ private float getHeading(boolean update)
+ {
+ if(update)
+ gyroUpdate();
+ return angles.firstAngle;
+ }
+
+ private void init_imu()
+ {
+ // Set up the parameters with which we will use our IMU. Note that integration
+ // algorithm here just reports accelerations to the logcat log; it doesn't actually
+ // provide positional information.
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+ parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+ parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+
+ // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
+ // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
+ // and named "imu".
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ imu.initialize(parameters);
+ }
+
+ int arm_up_pos, arm_folded_pos;
+
+ private void waitLift()
+ {
+ while(Math.abs(robot.LiftMotor.getTargetPosition() - robot.LiftMotor.getCurrentPosition()) > 100) {
+ telemetry.addData("waiting for arm", "Error: %d", robot.LiftMotor.getTargetPosition() - robot.LiftMotor.getCurrentPosition());
+ telemetry.update();
+ }
+
+ }
+
+ private void raiseArm(boolean wait)
+ {
+ robot.LiftMotor.setTargetPosition(arm_up_pos);
+ if(wait) waitLift();
+ }
+
+ private void retractArm(boolean wait)
+ {
+ robot.LiftMotor.setTargetPosition(arm_folded_pos);
+ if(wait) waitLift();
+ }
+
+ @Override
+ public void runOpMode() {
+
+ /*
+ * Initialize the standard drive system variables.
+ * The init() method of the hardware class does most of the work here
+ */
+ robot.init(hardwareMap);
+ init_imu();
+
+ robot.DriveLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ robot.DriveRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ robot.DriveLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ robot.DriveRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ telemetry.addData(">", "Robot Ready. Mount to lander."); //
+ telemetry.update();
+
+ /* Lock arm in place */
+ robot.LiftMotor.setTargetPosition(robot.LiftMotor.getCurrentPosition());
+ robot.LiftMotor.setPower(1);
+
+ // Wait for the game to start (Display Gyro value)
+ while (!isStarted()) {
+ telemetry.addData(">", "Robot Heading = %f", getHeading(true));
+ telemetry.update();
+ }
+
+ /* get arm positions */
+ arm_folded_pos = robot.LiftMotor.getCurrentPosition();
+ arm_up_pos = arm_folded_pos - 6000; // from experiment
+
+ robot.LiftMotor.setPower(1);
+
+ /* Descend from lander */
+ raiseArm(true);
+
+ sleep(500);
+
+ /* -1 = none, 0 = left, 1 = center, 2 = right, as viewed from robot */
+ int sample_pos = 2;
+
+ /* TODO */
+ // sample_pos = detect_sample();
+
+ // CCW is POSITIVE!!!
+ int sample_angles[] = { 30, 0, -30 };
+
+ gyroTurnRelative(TURN_SPEED, 87.0 + (sample_pos < 0 ? 0 : sample_angles[sample_pos]));
+
+ retractArm(false);
+
+ /* DON'T spin intake */
+ //robot.IntakeMotor.setPower(.5);
+ if(sample_pos >= 0) {
+ /* knock sample off */
+ double sample_distances[] = { 35, 30, 35 };
+ gyroDriveRelative(DRIVE_SPEED, sample_distances[sample_pos], 0);
+ gyroDriveRelative(DRIVE_SPEED, -sample_distances[sample_pos], 0);
+ }
+ //robot.IntakeMotor.setPower(0);
+
+ int robot_pos = 0; /* 0 = hook facing depot, 1 = hook 90deg CW from depot */
+
+ switch(robot_pos) {
+ 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, 20, 0);
+ gyroTurnRelative(TURN_SPEED, (45 + 10));
+ gyroDriveRelative(DRIVE_SPEED, 16, 0);
+ gyroTurnRelative(TURN_SPEED, 90);
+ gyroDriveRelative(DRIVE_SPEED, 35, 0);
+ break;
+ case 1:
+ /* TODO */
+ /* go back and drive to depot */
+ gyroDriveRelative(DRIVE_SPEED, -20, 0);
+ break;
+ }
+
+ /* Eject marker */
+ robot.IntakeMotor.setPower(-.5);
+ sleep(300);
+ gyroDriveRelative(DRIVE_SPEED * 2, -8, 0);
+
+ /* TODO: drive to crater */
+
+ // Step through each leg of the path,
+ // Note: Reverse movement is obtained by setting a negative distance (not speed)
+ // Put a hold after each turn
+ /*
+ gyroDrive(DRIVE_SPEED, 48.0, 0.0); // Drive FWD 48 inches
+ gyroTurn( TURN_SPEED, -45.0); // Turn CCW to -45 Degrees
+ gyroHold( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second
+ gyroDrive(DRIVE_SPEED, 12.0, -45.0); // Drive FWD 12 inches at 45 degrees
+ gyroTurn( TURN_SPEED, 45.0); // Turn CW to 45 Degrees
+ gyroHold( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
+ gyroTurn( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
+ gyroHold( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for a 1 second
+ gyroDrive(DRIVE_SPEED,-48.0, 0.0); // Drive REV 48 inches
+*/
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ }
+
+
+ /**
+ * Method to drive on a fixed compass bearing (angle), based on encoder counts.
+ * Move will stop if either of these conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Driver stops the opmode running.
+ *
+ * @param speed Target speed for forward motion. Should allow for _/- variance for adjusting heading
+ * @param distance Distance (in inches) to move from current position. Negative distance means move backwards.
+ * @param angle Absolute Angle (in Degrees) relative to last imu reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ */
+ public void gyroDrive ( double speed,
+ double distance,
+ double angle) {
+
+ int newLeftTarget;
+ int newRightTarget;
+ int moveCounts;
+ double max;
+ double error;
+ double steer;
+ double leftSpeed;
+ double rightSpeed;
+
+ // Ensure that the opmode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ moveCounts = (int)(distance * COUNTS_PER_INCH);
+ newLeftTarget = robot.DriveLeft.getCurrentPosition() + moveCounts;
+ newRightTarget = robot.DriveRight.getCurrentPosition() + moveCounts;
+
+ // Set Target and Turn On RUN_TO_POSITION
+ robot.DriveLeft.setTargetPosition(newLeftTarget);
+ robot.DriveRight.setTargetPosition(newRightTarget);
+
+ robot.DriveLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ robot.DriveRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // start motion.
+ speed = Range.clip(Math.abs(speed), 0.0, 1.0);
+ robot.DriveLeft.setPower(speed);
+ robot.DriveRight.setPower(speed);
+
+ // keep looping while we are still active, and BOTH motors are running.
+ while (opModeIsActive() &&
+ (robot.DriveLeft.isBusy() && robot.DriveRight.isBusy())) {
+
+ // adjust relative speed based on heading error.
+ error = getError(angle);
+ steer = getSteer(error, P_DRIVE_COEFF);
+
+ // if driving in reverse, the motor correction also needs to be reversed
+ if (distance < 0)
+ steer *= -1.0;
+
+ leftSpeed = speed + steer;
+ rightSpeed = speed - steer;
+
+ // Normalize speeds if either one exceeds +/- 1.0;
+ max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+ if (max > 1.0)
+ {
+ leftSpeed /= max;
+ rightSpeed /= max;
+ }
+
+ robot.DriveLeft.setPower(leftSpeed);
+ robot.DriveRight.setPower(rightSpeed);
+
+ // Display drive status for the driver.
+ telemetry.addData("Err/St", "%5.1f/%5.1f", error, steer);
+ telemetry.addData("Target", "%7d:%7d", newLeftTarget, newRightTarget);
+ telemetry.addData("Actual", "%7d:%7d", robot.DriveLeft.getCurrentPosition(),
+ robot.DriveRight.getCurrentPosition());
+ telemetry.addData("Speed", "%5.2f:%5.2f", leftSpeed, rightSpeed);
+ telemetry.update();
+ }
+
+ // Stop all motion;
+ robot.DriveLeft.setPower(0);
+ robot.DriveRight.setPower(0);
+
+ // Turn off RUN_TO_POSITION
+ robot.DriveLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ robot.DriveRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+ }
+
+
+ public void gyroDriveRelative ( double speed,
+ double distance,
+ double dtheta)
+ {
+ gyroDrive(speed, distance, getHeading(true) + dtheta);
+ }
+
+ /**
+ * Method to spin on central axis to point in a new direction.
+ * Move will stop if either of these conditions occur:
+ * 1) Move gets to the heading (angle)
+ * 2) Driver stops the opmode running.
+ *
+ * @param speed Desired speed of turn.
+ * @param angle Absolute Angle (in Degrees) relative to last imu reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ */
+ public void gyroTurn ( double speed, double angle) {
+
+ // keep looping while we are still active, and not on heading.
+ while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
+ // Update telemetry & Allow time for other processes to run.
+ telemetry.update();
+ }
+ }
+
+ public void gyroTurnRelative(double speed, double dtheta)
+ {
+ gyroTurn(speed, getHeading(true) + dtheta);
+ }
+
+ /**
+ * Method to obtain & hold a heading for a finite amount of time
+ * Move will stop once the requested time has elapsed
+ *
+ * @param speed Desired speed of turn.
+ * @param angle Absolute Angle (in Degrees) relative to last imu reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ * @param holdTime Length of time (in seconds) to hold the specified heading.
+ */
+ public void gyroHold( double speed, double angle, double holdTime) {
+
+ 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);
+ telemetry.update();
+ }
+
+ // Stop all motion;
+ robot.DriveLeft.setPower(0);
+ robot.DriveRight.setPower(0);
+ }
+
+ /**
+ * Perform one cycle of closed loop heading control.
+ *
+ * @param speed Desired speed of turn.
+ * @param angle Absolute Angle (in Degrees) relative to last imu reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ * @param PCoeff Proportional Gain coefficient
+ * @return
+ */
+
+ boolean onHeading(double speed, double angle, double PCoeff) {
+ double error ;
+ double steer ;
+ boolean onTarget = false ;
+ double leftSpeed;
+ double rightSpeed;
+
+ // determine turn power based on +/- error
+ error = getError(angle);
+
+ if (Math.abs(error) <= HEADING_THRESHOLD) {
+ steer = 0.0;
+ leftSpeed = 0.0;
+ rightSpeed = 0.0;
+ onTarget = true;
+ }
+ else {
+ steer = getSteer(error, PCoeff);
+ leftSpeed = speed * steer;
+ rightSpeed = -leftSpeed;
+ }
+
+ // Send desired speeds to motors.
+ robot.DriveLeft.setPower(leftSpeed);
+ robot.DriveRight.setPower(rightSpeed);
+
+ // Display it for the driver.
+ telemetry.addData("Target", "%5.2f", angle);
+ telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
+ telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
+
+ return onTarget;
+ }
+
+ /**
+ * getError determines the error between the target angle and the robot's current heading
+ * @param targetAngle Desired angle (relative to global reference established at last Gyro Reset).
+ * @return error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
+ * +ve error means the robot should turn LEFT (CCW) to reduce error.
+ */
+ public double getError(double targetAngle) {
+
+ double robotError;
+
+ // calculate error in -179 to +180 range (
+ robotError = targetAngle - getHeading(true);
+ while (robotError > 180) robotError -= 360;
+ while (robotError <= -180) robotError += 360;
+ return robotError;
+ }
+
+ /**
+ * 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);
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
index cd76df2..00af146 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
@@ -29,12 +29,20 @@
package org.firstinspires.ftc.teamcode;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+
/**
* This OpMode uses the common Pushbot hardware class to define the devices on the robot.
* All device access is managed through the HardwarePushbot class.
@@ -49,9 +57,9 @@ import com.qualcomm.robotcore.util.Range;
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@TeleOp(name="TankBot: Driver Mode", group="Pushbot")
+@TeleOp(name="TankBot: Driver Mode", group="Tankbot")
-public class PushbotTeleopPOV_Linear extends LinearOpMode {
+public class TankbotDriver extends LinearOpMode {
/* Declare OpMode members. */
HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
@@ -72,70 +80,49 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
DeviceInterfaceModule dim;
- ModernRoboticsI2cGyro modernRoboticsI2cGyro;
-
- /**
- * Perform one cycle of closed loop heading control.
- *
- * @param speed Desired speed of turn.
- * @param angle Absolute Angle (in Degrees) relative to last gyro reset.
- * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
- * If a relative angle is required, add/subtract from current heading.
- * @param PCoeff Proportional Gain coefficient
- * @return
- */
- double onHeading(double speed, double angle, double PCoeff) {
- double error ;
- double steer = 0 ;
- boolean onTarget = false ;
-
- // determine turn power based on +/- error
- error = getError(angle);
-
- double ret;
-
- if (Math.abs(error) <= HEADING_THRESHOLD) {
- ret = 0;
- }
- else {
- steer = getSteer(error, PCoeff);
- ret = steer;
- }
-
- // Display it for the driver.
- telemetry.addData("Target", "%5.2f", angle);
- telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
- telemetry.addData("Speed.", "%5.2f", ret);
+ BNO055IMU imu = null; // Additional Gyro device
- return ret;
- }
-
- /**
- * getError determines the error between the target angle and the robot's current heading
- * @param targetAngle Desired angle (relative to global reference established at last Gyro Reset).
- * @return error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
- * +ve error means the robot should turn LEFT (CCW) to reduce error.
+ /* Which way the robot is pointing, and which way gravity pulls, as measured by the gyro.
+ Use gyroUpdate() to update
*/
- public double getError(double targetAngle) {
-
- double robotError;
+ /* angles are:
+ Z: 1st: heading (CCW positive)
+ Y: 2nd pitch (nose down positive)
+ X: 3rd: roll (roll right positive)
+ */
+ Orientation angles;
+ Acceleration gravity;
- // calculate error in -179 to +180 range (
+ private void gyroUpdate() {
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ gravity = imu.getGravity();
+ }
- robotError = targetAngle - modernRoboticsI2cGyro.getIntegratedZValue();
- while (robotError > 180) robotError -= 360;
- while (robotError <= -180) robotError += 360;
- return robotError;
+ private float getHeading(boolean update)
+ {
+ if(update)
+ gyroUpdate();
+ return angles.firstAngle;
}
- /**
- * 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);
+ private void init_imu()
+ {
+ // Set up the parameters with which we will use our IMU. Note that integration
+ // algorithm here just reports accelerations to the logcat log; it doesn't actually
+ // provide positional information.
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+ parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+ parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+
+ // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
+ // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
+ // and named "imu".
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ imu.initialize(parameters);
}
@Override
@@ -148,29 +135,7 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
* The init() method of the hardware class does all the work here
*/
robot.init(hardwareMap);
-
- // Get a reference to a Modern Robotics gyro object. We use several interfaces
- // on this object to illustrate which interfaces support which functionality.
- //modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
- // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
- // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro");
- // A similar approach will work for the Gyroscope interface, if that's all you need.
-
- // Start calibrating the gyro. This takes a few seconds and is worth performing
- // during the initialization phase at the start of each opMode.
- //telemetry.log().add("Gyro Calibrating. Do Not Move!");
- //modernRoboticsI2cGyro.calibrate();
-
- // Wait until the gyro calibration is complete
- /*while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating()) {
- telemetry.addLine("calibrating...");
- telemetry.update();
- sleep(50);
- }
-
- telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
- telemetry.clear(); telemetry.update();
-*/
+ init_imu();
// Send telemetry message to signify robot waiting;
telemetry.addData("Say", "Hello Driver"); //
@@ -179,33 +144,12 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
// Wait for the game to start (driver presses PLAY)
waitForStart();
- /* indicate glyphmotor power */
- // dim.setLED(BLUE_LED, false);
-
- /* max power off */
- // dim.setLED(RED_LED, false);
-
- /*
- double fine_speed = FINE_SPEED, rotation_rate = ROT_SPEED;
- boolean max_power = false;
- boolean last_x = false, last_b = false, last_back = false;
- boolean glyphmotor_on = false;
- boolean correct_direction = false;
-
- double power[][] = new double[2][2];
-
- int heading = modernRoboticsI2cGyro.getIntegratedZValue();
-
- int zero_rotations = 0;
- */
-
double fine_speed = .5;
+ int liftpos = robot.LiftMotor.getCurrentPosition();
+
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
- //robot.Testmotor.setPower(gamepad1.left_stick_x * .5);
-
- /* y is horizontal, x is vertical... sorry */
double forward = -gamepad1.left_stick_y;
double rotate = gamepad1.right_stick_x;
final double DEAD_ZONE = .05;
@@ -246,18 +190,23 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
/* 2240 counts/rev */
int dlift = 0;
+ boolean no_lift = false;
if(gamepad1.a)
- dlift += 2240/5;
- if(gamepad1.y)
- dlift -= 2240/5;
-
- int liftpos = robot.LiftMotor.getCurrentPosition();
+ dlift += 2240/12;
+ else if(gamepad1.y)
+ dlift -= 2240/12;
+ else
+ no_lift = true;
boolean limit_reached = !robot.liftLimit.getState();
- if(limit_reached)
- dlift = 0;
- robot.LiftMotor.setTargetPosition(liftpos + dlift);
+ if(limit_reached || no_lift) {
+ robot.LiftMotor.setTargetPosition(robot.LiftMotor.getCurrentPosition());
+ }
+ else {
+ liftpos += dlift;
+ robot.LiftMotor.setTargetPosition(liftpos);
+ }
robot.IntakeMotor.setPower(intake);
robot.DriveLeft.setPower(powers[0]);
@@ -268,6 +217,8 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
telemetry.addData("rotation", "%.2f", rotate);
telemetry.addData("liftposition", "%d", robot.LiftMotor.getCurrentPosition());
telemetry.addData("forward", "%.3f", forward);
+ telemetry.addData("Drive positions", "%d %d", robot.DriveLeft.getCurrentPosition(), robot.DriveRight.getCurrentPosition());
+ telemetry.addData("heading", getHeading(true));
telemetry.update();
// Pace this loop so jaw action is reasonable speed.