Difference between revisions of "FTC Using methods 20203018"
From wikidb
(→Full Code One Method) |
(→Code Outline) |
||
Line 8: | Line 8: | ||
* Code based on [[FTC_Motor_Encoders_20200304]] | * Code based on [[FTC_Motor_Encoders_20200304]] | ||
* Enhance the drive forward with encoders using the setVelocity | * Enhance the drive forward with encoders using the setVelocity | ||
+ | |||
+ | <pre> | ||
+ | // autonomous program that drives bot in the shape of a square. | ||
+ | // | ||
+ | // Ed C. Epp | ||
+ | // March 18, 2022 | ||
+ | |||
+ | package org.firstinspires.ftc.teamcode; | ||
+ | |||
+ | import com.qualcomm.robotcore.eventloop.opmode.Autonomous; | ||
+ | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
+ | import com.qualcomm.robotcore.hardware.DcMotor; | ||
+ | import com.qualcomm.robotcore.hardware.DcMotorEx; | ||
+ | |||
+ | @Autonomous(name="Drive in a square v05", group="Concept") | ||
+ | //@Disabled | ||
+ | |||
+ | // --------------------------- DriveInASquare class ----------------------------- | ||
+ | // ------------------------------------------------------------------------------ | ||
+ | public class DriveInASquare extends LinearOpMode | ||
+ | { | ||
+ | double TARGET_DISTANCE = 300; // mm (about 12 inches) | ||
+ | double MAX_MOTOR_VELOCITY = 600; // mm / second | ||
+ | double TARGET_VELOCITY = MAX_MOTOR_VELOCITY / 4; | ||
+ | double TARGET_TURN = 90; // degrees | ||
+ | int SQUARE_SIDES = 2; | ||
+ | |||
+ | DcMotorEx leftMotor; | ||
+ | DcMotorEx rightMotor; | ||
+ | |||
+ | @Override | ||
+ | public void runOpMode() throws InterruptedException | ||
+ | { | ||
+ | // Configure the motors | ||
+ | leftMotor = hardwareMap.get(DcMotorEx.class,"myLeftMotor"); | ||
+ | leftMotor.setDirection(DcMotor.Direction.REVERSE); | ||
+ | rightMotor = hardwareMap.get(DcMotorEx.class,"myRightMotor"); | ||
+ | |||
+ | telemetry.addData("Mode", "waiting"); | ||
+ | telemetry.update(); | ||
+ | |||
+ | // wait for start button. | ||
+ | waitForStart(); | ||
+ | |||
+ | // wait while opmode is active and | ||
+ | // left or right motor is busy running to position. | ||
+ | int count = 0; | ||
+ | while (opModeIsActive() && count < SQUARE_SIDES) | ||
+ | { | ||
+ | driveForMmAt (TARGET_DISTANCE, TARGET_VELOCITY); | ||
+ | turnForDegrees (TARGET_TURN); | ||
+ | count++; | ||
+ | } | ||
+ | |||
+ | |||
+ | // wait 5 sec to you can observe the final encoder position. | ||
+ | Thread.sleep(5000); | ||
+ | } | ||
+ | |||
+ | // ----------- driveForMmAt ---------------------------------------- | ||
+ | // Drive for a specific distance and velocity | ||
+ | // distanceMm: target distance in mm | ||
+ | // velocityMm: target velocity in mm per second | ||
+ | void driveForMmAt (double distanceMm, double velocityMmPerSec) | ||
+ | { | ||
+ | // REV-41-1300 Core Hex Motor | ||
+ | // 4 Ticks per revolution at the motor | ||
+ | // Gear ration: 72:1 motor revolutions per output revolution | ||
+ | // 72 motor rev per output rev * 4 ticks per rev => 288 ticks per output rev | ||
+ | |||
+ | // REV-41-1354 90 mm traction wheel | ||
+ | // 90 mm Wheel diameter | ||
+ | // 90 mm * pi => 283 mm / rev | ||
+ | |||
+ | double mMPerTick = 283.0 / 288.0; | ||
+ | double ticksToMove = distanceMm / mMPerTick; | ||
+ | // telemetry.addData(" mMPerTick ticksToMove ", mMPerTick + " " + ticksToMove); | ||
+ | |||
+ | double ticksPerSecond = velocityMmPerSec / mMPerTick; | ||
+ | |||
+ | // reset the motors | ||
+ | leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); | ||
+ | rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); | ||
+ | leftMotor.setTargetPosition((int)ticksToMove); | ||
+ | rightMotor.setTargetPosition((int)ticksToMove); | ||
+ | leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); | ||
+ | rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); | ||
+ | leftMotor.setVelocity(ticksPerSecond); | ||
+ | rightMotor.setVelocity(ticksPerSecond); | ||
+ | |||
+ | // telemetry.addData("ticksPerSecond ", ticksPerSecond); | ||
+ | // telemetry.update(); | ||
+ | while (leftMotor.isBusy() || leftMotor.isBusy()) | ||
+ | { | ||
+ | telemetry.addData("position velocity ", leftMotor.getCurrentPosition() + | ||
+ | " " + leftMotor.getVelocity()); | ||
+ | telemetry.update(); | ||
+ | idle(); | ||
+ | } | ||
+ | |||
+ | // set motor power to zero to turn off motors. The motors stop on their own but | ||
+ | // power is still applied so we turn off the power. | ||
+ | leftMotor.setVelocity(0); | ||
+ | rightMotor.setVelocity(0); | ||
+ | } | ||
+ | |||
+ | // ---------- turnForDegrees ----------------------------------- | ||
+ | // turnDegrees: target turn in degrees | ||
+ | // To be completed by the student | ||
+ | void turnForDegrees (double turnDegrees) | ||
+ | { | ||
+ | |||
+ | } | ||
+ | } | ||
+ | </pre> | ||
= Full Code One Method = | = Full Code One Method = | ||
* [[DriveInASquare.java]] | * [[DriveInASquare.java]] |
Revision as of 12:07, 18 March 2022
Why Use Abstraction
- Makes it easier to reason about complex programs
- Easier to test programs - can test small part individually
- Code is reused making it more reliable
Code Outline
- Code based on FTC_Motor_Encoders_20200304
- Enhance the drive forward with encoders using the setVelocity
// autonomous program that drives bot in the shape of a square. // // Ed C. Epp // March 18, 2022 package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @Autonomous(name="Drive in a square v05", group="Concept") //@Disabled // --------------------------- DriveInASquare class ----------------------------- // ------------------------------------------------------------------------------ public class DriveInASquare extends LinearOpMode { double TARGET_DISTANCE = 300; // mm (about 12 inches) double MAX_MOTOR_VELOCITY = 600; // mm / second double TARGET_VELOCITY = MAX_MOTOR_VELOCITY / 4; double TARGET_TURN = 90; // degrees int SQUARE_SIDES = 2; DcMotorEx leftMotor; DcMotorEx rightMotor; @Override public void runOpMode() throws InterruptedException { // Configure the motors leftMotor = hardwareMap.get(DcMotorEx.class,"myLeftMotor"); leftMotor.setDirection(DcMotor.Direction.REVERSE); rightMotor = hardwareMap.get(DcMotorEx.class,"myRightMotor"); telemetry.addData("Mode", "waiting"); telemetry.update(); // wait for start button. waitForStart(); // wait while opmode is active and // left or right motor is busy running to position. int count = 0; while (opModeIsActive() && count < SQUARE_SIDES) { driveForMmAt (TARGET_DISTANCE, TARGET_VELOCITY); turnForDegrees (TARGET_TURN); count++; } // wait 5 sec to you can observe the final encoder position. Thread.sleep(5000); } // ----------- driveForMmAt ---------------------------------------- // Drive for a specific distance and velocity // distanceMm: target distance in mm // velocityMm: target velocity in mm per second void driveForMmAt (double distanceMm, double velocityMmPerSec) { // REV-41-1300 Core Hex Motor // 4 Ticks per revolution at the motor // Gear ration: 72:1 motor revolutions per output revolution // 72 motor rev per output rev * 4 ticks per rev => 288 ticks per output rev // REV-41-1354 90 mm traction wheel // 90 mm Wheel diameter // 90 mm * pi => 283 mm / rev double mMPerTick = 283.0 / 288.0; double ticksToMove = distanceMm / mMPerTick; // telemetry.addData(" mMPerTick ticksToMove ", mMPerTick + " " + ticksToMove); double ticksPerSecond = velocityMmPerSec / mMPerTick; // reset the motors leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftMotor.setTargetPosition((int)ticksToMove); rightMotor.setTargetPosition((int)ticksToMove); leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); leftMotor.setVelocity(ticksPerSecond); rightMotor.setVelocity(ticksPerSecond); // telemetry.addData("ticksPerSecond ", ticksPerSecond); // telemetry.update(); while (leftMotor.isBusy() || leftMotor.isBusy()) { telemetry.addData("position velocity ", leftMotor.getCurrentPosition() + " " + leftMotor.getVelocity()); telemetry.update(); idle(); } // set motor power to zero to turn off motors. The motors stop on their own but // power is still applied so we turn off the power. leftMotor.setVelocity(0); rightMotor.setVelocity(0); } // ---------- turnForDegrees ----------------------------------- // turnDegrees: target turn in degrees // To be completed by the student void turnForDegrees (double turnDegrees) { } }