arm encoders made, tested, and implmented into main code
This commit is contained in:
robotics2
@ -79,8 +79,6 @@ public class Autonomoustest extends LinearOpMode {
|
||||
private DcMotor arm = null;
|
||||
|
||||
|
||||
|
||||
|
||||
private ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
// Calculate the COUNTS_PER_INCH for your specific drive train.
|
||||
@ -94,11 +92,16 @@ public class Autonomoustest extends LinearOpMode {
|
||||
static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference
|
||||
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
|
||||
(WHEEL_DIAMETER_INCHES * Math.PI);
|
||||
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
|
||||
static final double DRIVE_SPEED = 0.2;
|
||||
static final double TURN_SPEED = 0.4;
|
||||
|
||||
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() {
|
||||
|
||||
// Initialize the drive system variables
|
||||
@ -118,7 +121,7 @@ public class Autonomoustest extends LinearOpMode {
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.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);
|
||||
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
@ -146,68 +149,57 @@ public class Autonomoustest extends LinearOpMode {
|
||||
|
||||
// Step through each leg of the path,
|
||||
// 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
|
||||
@ -217,32 +209,31 @@ public class Autonomoustest extends LinearOpMode {
|
||||
sleep(1000); // pause to display final telemetry message.
|
||||
}
|
||||
|
||||
public void driveForward(double distance)
|
||||
{
|
||||
public void driveForward(double distance) {
|
||||
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
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);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
|
||||
}
|
||||
public void straightRight(double distance)
|
||||
{
|
||||
|
||||
public void straightRight(double distance) {
|
||||
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backleftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
|
||||
}
|
||||
public void turnLeft(double degrees)
|
||||
{
|
||||
|
||||
public void turnLeft(double degrees) {
|
||||
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
@ -250,8 +241,8 @@ public class Autonomoustest extends LinearOpMode {
|
||||
double turning_distance = degrees * DEGREE_TOO_DISTANCE;
|
||||
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
|
||||
}
|
||||
public void turnRight(double degrees)
|
||||
{
|
||||
|
||||
public void turnRight(double degrees) {
|
||||
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
@ -259,8 +250,8 @@ public class Autonomoustest extends LinearOpMode {
|
||||
double turning_distance = degrees * DEGREE_TOO_DISTANCE;
|
||||
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
|
||||
}
|
||||
public int readColorRight()
|
||||
{
|
||||
|
||||
public int readColorRight() {
|
||||
telemetry.addData("Clear", colorRight.alpha());
|
||||
telemetry.addData("Red ", colorRight.red());
|
||||
telemetry.addData("Green", colorRight.green());
|
||||
@ -268,8 +259,8 @@ public class Autonomoustest extends LinearOpMode {
|
||||
int bluenumber = colorRight.blue();
|
||||
return bluenumber;
|
||||
}
|
||||
public int readColorLeft()
|
||||
{
|
||||
|
||||
public int readColorLeft() {
|
||||
telemetry.addData("Clear Left", colorLeft.alpha());
|
||||
telemetry.addData("Red left ", colorLeft.red());
|
||||
telemetry.addData("Green left", colorLeft.green());
|
||||
@ -279,14 +270,14 @@ public class Autonomoustest extends LinearOpMode {
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -308,6 +299,7 @@ public class Autonomoustest extends LinearOpMode {
|
||||
int newBackLeftTarget;
|
||||
int newbackRightTarget;
|
||||
|
||||
|
||||
if (opModeIsActive()) {
|
||||
|
||||
// Determine new target position, and pass to motor controller
|
||||
@ -341,7 +333,7 @@ public class Autonomoustest extends LinearOpMode {
|
||||
// onto the next step, use (isBusy() || isBusy()) in the loop test.
|
||||
while (opModeIsActive() &&
|
||||
(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.
|
||||
telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
|
||||
@ -351,7 +343,6 @@ public class Autonomoustest extends LinearOpMode {
|
||||
}
|
||||
|
||||
|
||||
|
||||
leftDrive.setPower(0);
|
||||
rightDrive.setPower(0);
|
||||
backrightDrive.setPower(0);
|
||||
@ -367,4 +358,48 @@ public class Autonomoustest extends LinearOpMode {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user