SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

arm encoders made, tested, and implmented into main code

Browse Source
This commit is contained in:
robotics2
2023-10-12 17:13:35 -07:00
parent 69f9264228
commit 180f2bd873
1 changed files with 156 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

275
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java
View File

@ -68,20 +68,18 @@ import com.qualcomm.robotcore.util.ElapsedTime;
public class Autonomoustest extends LinearOpMode { public class Autonomoustest extends LinearOpMode {
/* Declare OpMode members. */ /* Declare OpMode members. */
private DcMotor leftDrive = null; private DcMotor leftDrive = null;
private DcMotor rightDrive = null; private DcMotor rightDrive = null;
private DcMotor backrightDrive = null; private DcMotor backrightDrive = null;
private DcMotor backleftDrive = null; private DcMotor backleftDrive = null;
private ColorSensor colorRight = null; private ColorSensor colorRight = null;
private ColorSensor colorLeft = null; private ColorSensor colorLeft = null;
private Servo wrist = null; private Servo wrist = null;
private Servo gripper = null; private Servo gripper = null;
private DcMotor arm = null; private DcMotor arm = null;
private ElapsedTime runtime = new ElapsedTime();
private ElapsedTime runtime = new ElapsedTime();
// Calculate the COUNTS_PER_INCH for your specific drive train. // Calculate the COUNTS_PER_INCH for your specific drive train.
// Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
@ -89,27 +87,32 @@ public class Autonomoustest extends LinearOpMode {
// For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
// This is gearing DOWN for less speed and more torque. // This is gearing DOWN for less speed and more torque.
// For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
static final double COUNTS_PER_MOTOR_REV = 537.6 ; // eg: TETRIX Motor Encoder static final double COUNTS_PER_MOTOR_REV = 537.6; // eg: TETRIX Motor Encoder
static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. static final double DRIVE_GEAR_REDUCTION = 1.0; // No External Gearing.
static final double WHEEL_DIAMETER_INCHES = 3.77953 ; // For figuring circumference static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
(WHEEL_DIAMETER_INCHES * Math.PI); (WHEEL_DIAMETER_INCHES * Math.PI);
static final double DRIVE_SPEED = 0.2; static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
static final double TURN_SPEED = 0.4; static final double DRIVE_SPEED = 0.2;
static final double TURN_SPEED = 0.4;
static final double LONG_TIMEOUT = 1000; static final double LONG_TIMEOUT = 1000;
static final double DEGREE_TOO_DISTANCE = 0.21944444444; @Override static final double DEGREE_TOO_DISTANCE = 0.21944444444;
static final double ARM_SPEED = .1;
static final double TICKS_TO_DEGREES = 0.07462686567;
@Override
public void runOpMode() { public void runOpMode() {
// Initialize the drive system variables // Initialize the drive system variables
leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt"); leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt"); rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
colorRight = hardwareMap.get(ColorSensor.class, "color right"); colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left"); colorLeft = hardwareMap.get(ColorSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper"); gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class , "arm raise"); arm = hardwareMap.get(DcMotor.class, "arm raise");
wrist = hardwareMap.get(Servo.class, "wrist"); wrist = hardwareMap.get(Servo.class, "wrist");
// To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
// When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
@ -118,7 +121,7 @@ public class Autonomoustest extends LinearOpMode {
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
arm.setDirection(DcMotor.Direction.REVERSE); arm.setDirection(DcMotor.Direction.FORWARD);
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@ -146,68 +149,57 @@ public class Autonomoustest extends LinearOpMode {
// Step through each leg of the path, // Step through each leg of the path,
// Note: Reverse movement is obtained by setting a negative distance (not speed) // Note: Reverse movement is obtained by setting a negative distance (not speed)
raisearm(1);
arm.setPower(0.001);
sleep(10000);
//driveForward(2);
// driveForward(28);
// {
// int blueleft = readColorLeft();
// int blueright = readColorRight();
// if (blueleft > 75)
// {
// //telemetry.addData("color sensor","left");
// if(blueleft > blueright)
// telemetry.addData("color sensor","left");
// turnLeft(90);
// straightLeft(2);
// driveForward(11);
// driveForward(-30);
// turnLeft(180);
// driveForward(19);
// straightLeft(7);
// //use arm to place pixel
// straightRight(34);
// driveForward(11);
// terminateOpModeNow();
//
//
//
//
// }
// if (blueright > 75)
// {
// //telemetry.addData("color sensor", "right");
// if(blueleft < blueright)
// telemetry.addData("color sensor","right");
// straightRight(11);
// driveForward(-17);
// turnRight(90);
// straightLeft(8);
// driveForward(28.5);
// //use arm to place pixel
// straightRight(20);
// driveForward(11);
// terminateOpModeNow();
//
//
// }
// else
// telemetry.addData("position","center");
// driveForward(11);
// driveForward(-17);
// turnRight(90);
// driveForward(39);
// straightRight(3.5);
// //use arm to place pixel
// straightRight(23.5);
// driveForward(11);
//
// telemetry.update();
// sleep(250);
//
// }
{
raisearm(100);
int blueleft = readColorLeft();
int blueright = readColorRight();
if (blueleft > 75)
{
//telemetry.addData("color sensor","left");
if(blueleft > blueright)
telemetry.addData("color sensor","left");
turnLeft(90);
straightLeft(2);
driveForward(11);
driveForward(-30);
terminateOpModeNow();
}
if (blueright > 75)
{
//telemetry.addData("color sensor", "right");
if(blueleft < blueright)
telemetry.addData("color sensor","right");
straightRight(11);
driveForward(-17);
turnRight(90);
straightLeft(8);
driveForward(-28.5);
raisearm(80);
straightRight(22);
driveForward(-11);
terminateOpModeNow();
}
else
telemetry.addData("position","center");
driveForward(7);
driveForward(-13);
turnRight(90);
driveForward(-35);
raisearm(80);
straightRight(22.5);
driveForward(-11);
telemetry.update();
sleep(250);
}
//Values were created from robot with wheel issues 9/28/23 //Values were created from robot with wheel issues 9/28/23
@ -217,50 +209,49 @@ public class Autonomoustest extends LinearOpMode {
sleep(1000); // pause to display final telemetry message. sleep(1000); // pause to display final telemetry message.
} }
public void driveForward(double distance) public void driveForward(double distance) {
{
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout
} }
public void straightLeft(double distance)
{ public void straightLeft(double distance) {
leftDrive.setDirection(DcMotor.Direction.FORWARD); leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
} }
public void straightRight(double distance)
{ public void straightRight(double distance) {
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE);
backrightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.FORWARD);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
} }
public void turnLeft(double degrees)
{ public void turnLeft(double degrees) {
leftDrive.setDirection(DcMotor.Direction.FORWARD); leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.FORWARD);
double turning_distance = degrees * DEGREE_TOO_DISTANCE; double turning_distance = degrees * DEGREE_TOO_DISTANCE;
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
} }
public void turnRight(double degrees)
{ public void turnRight(double degrees) {
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE);
backrightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
double turning_distance = degrees * DEGREE_TOO_DISTANCE; double turning_distance = degrees * DEGREE_TOO_DISTANCE;
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
} }
public int readColorRight()
{ public int readColorRight() {
telemetry.addData("Clear", colorRight.alpha()); telemetry.addData("Clear", colorRight.alpha());
telemetry.addData("Red ", colorRight.red()); telemetry.addData("Red ", colorRight.red());
telemetry.addData("Green", colorRight.green()); telemetry.addData("Green", colorRight.green());
@ -268,8 +259,8 @@ public class Autonomoustest extends LinearOpMode {
int bluenumber = colorRight.blue(); int bluenumber = colorRight.blue();
return bluenumber; return bluenumber;
} }
public int readColorLeft()
{ public int readColorLeft() {
telemetry.addData("Clear Left", colorLeft.alpha()); telemetry.addData("Clear Left", colorLeft.alpha());
telemetry.addData("Red left ", colorLeft.red()); telemetry.addData("Red left ", colorLeft.red());
telemetry.addData("Green left", colorLeft.green()); telemetry.addData("Green left", colorLeft.green());
@ -279,14 +270,14 @@ public class Autonomoustest extends LinearOpMode {
return bluenumber; return bluenumber;
}
public void raisearm(int degrees) {
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
} }
public void raisearm(long seconds)
{
arm.setPower(.1);
sleep(seconds * 1000);
}
@ -306,15 +297,16 @@ public class Autonomoustest extends LinearOpMode {
int newLeftTarget; int newLeftTarget;
int newRightTarget; int newRightTarget;
int newBackLeftTarget; int newBackLeftTarget;
int newbackRightTarget; int newbackRightTarget;
if (opModeIsActive()) { if (opModeIsActive()) {
// Determine new target position, and pass to motor controller // Determine new target position, and pass to motor controller
newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH); newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
newBackLeftTarget = backleftDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
newbackRightTarget = backrightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
leftDrive.setTargetPosition(newLeftTarget); leftDrive.setTargetPosition(newLeftTarget);
rightDrive.setTargetPosition(newRightTarget); rightDrive.setTargetPosition(newRightTarget);
backrightDrive.setTargetPosition(newbackRightTarget); backrightDrive.setTargetPosition(newbackRightTarget);
@ -340,22 +332,21 @@ public class Autonomoustest extends LinearOpMode {
// However, if you require that BOTH motors have finished their moves before the robot continues // However, if you require that BOTH motors have finished their moves before the robot continues
// onto the next step, use (isBusy() || isBusy()) in the loop test. // onto the next step, use (isBusy() || isBusy()) in the loop test.
while (opModeIsActive() && while (opModeIsActive() &&
(runtime.seconds() < timeoutS) && (runtime.seconds() < timeoutS) &&
(leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy())) { (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) {
// Display it for the driver. // Display it for the driver.
telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
telemetry.addData("Currently at", " at %7d :%7d", telemetry.addData("Currently at", " at %7d :%7d",
leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition()); leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition());
telemetry.update(); telemetry.update();
} }
leftDrive.setPower(0);
leftDrive.setPower(0); rightDrive.setPower(0);
rightDrive.setPower(0); backrightDrive.setPower(0);
backrightDrive.setPower(0); backleftDrive.setPower(0);
backleftDrive.setPower(0);
// Turn off RUN_TO_POSITION // Turn off RUN_TO_POSITION
@ -367,4 +358,48 @@ public class Autonomoustest extends LinearOpMode {
sleep(250); // optional pause after each move. sleep(250); // optional pause after each move.
} }
} }
public void armEncoder(double speed,
double Inches, double timeoutS) {
int newarmTarget;
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
arm.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// reset the timeout time and start motion.
runtime.reset();
arm.setPower(Math.abs(speed));
// keep looping while we are still active, and there is time left, and both motors are running.
// Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
// its target position, the motion will stop. This is "safer" in the event that the robot will
// always end the motion as soon as possible.
// However, if you require that BOTH motors have finished their moves before the robot continues
// onto the next step, use (isBusy() || isBusy()) in the loop test.
while (opModeIsActive() &&
(runtime.seconds() < timeoutS) &&
(arm.isBusy())) {
// Display it for the driver.
telemetry.addData("Running to", " %7d", newarmTarget);
telemetry.addData("Currently at", " at %7d",
arm.getCurrentPosition());
telemetry.update();
}
arm.setPower(0);
// Turn off RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}
} }