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-14 11:56:23 -07:00
parent 180f2bd873
commit eb81e1b4b6
1 changed files with 146 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,39 +102,10 @@ public class Autonomoustest extends LinearOpMode {
static final double TICKS_TO_DEGREES = 0.07462686567;
@Override
public void runOpMode() {
public void runOpMode()
{
hardwareinit();
// Initialize the drive system variables
leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class, "arm raise");
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.
// When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.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);
backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Send telemetry message to indicate successful Encoder reset
/* telemetry.addData("Starting at", "%7d :%7d",
leftDrive.getCurrentPosition(),
@ -146,70 +117,23 @@ public class Autonomoustest extends LinearOpMode {
// Wait for the game to start (driver presses PLAY)
waitForStart();
{
//executeAuto();
testGripper();
}
// Step through each leg of the path,
// Note: Reverse movement is obtained by setting a negative distance (not speed)
{
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
telemetry.addData("Path", "Complete");
telemetry.update();
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);
@ -217,7 +141,8 @@ public class Autonomoustest extends LinearOpMode {
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);
@ -225,7 +150,8 @@ public class Autonomoustest extends LinearOpMode {
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);
@ -233,7 +159,8 @@ public class Autonomoustest extends LinearOpMode {
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);
@ -276,6 +203,132 @@ public class Autonomoustest extends LinearOpMode {
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
}
public void hardwareinit()
{
leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class, "arm raise");
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.
// When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
arm.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
public void testWrist()
{
wrist.setPosition(0);
sleep(3000);
wrist.setPosition(1);
sleep(3000);
}
public void testGripper()
{
gripper.setPosition(0);
sleep(3000);
gripper.setPosition(1);
sleep(3000);
}
public void executeAuto()
{
raisearm(100);
wrist.setPosition(0);
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);
straightLeft(32);
turnLeft(10);
driveForward(18);
driveForward(-31);
double backboard = 34;
straightRight(backboard);
raisearm(80);
driveForward(-1);
gripper.setPosition(0);
terminateOpModeNow();
}
if (blueright > 75)
{
//telemetry.addData("color sensor", "right");
if(blueleft < blueright)
telemetry.addData("color sensor","right");
straightRight(11.5);
driveForward(-17);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-26);
double backboard = 22.5;
straightRight(backboard);
raisearm(80);
driveForward(-5);
gripper.setPosition(1);
terminateOpModeNow();
}
else
telemetry.addData("position","center");
driveForward(7);
driveForward(-7);
straightRight(11.5);
driveForward(-17);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-26);
double backboard = 29;
straightRight(backboard);
raisearm(80);
driveForward(-5);
gripper.setPosition(1);
telemetry.update();
sleep(250);
//Values were created from robot with wheel issues 9/28/23
telemetry.addData("Path", "Complete");
telemetry.update();
sleep(1000); // pause to display final telemetry message.
}
@ -400,6 +453,7 @@ public class Autonomoustest extends LinearOpMode {
// Turn off RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}
}