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

arm holds and autonmous set wtihout the arm implemented

Browse Source
This commit is contained in:
robotics2
2023-10-07 12:07:18 -07:00
parent 9aab8fadf1
commit 780f5fdde9
1 changed files with 91 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,6 +33,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
/** /**
@ -72,6 +74,10 @@ public class Autonomoustest extends LinearOpMode {
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 gripper = null;
private DcMotor arm = null;
@ -95,13 +101,16 @@ public class Autonomoustest extends LinearOpMode {
static final double DEGREE_TOO_DISTANCE = 0.21944444444; @Override static final double DEGREE_TOO_DISTANCE = 0.21944444444; @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");
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. // 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.
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
@ -109,17 +118,20 @@ 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);
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);
backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backrightDrive.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); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backleftDrive.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 // Send telemetry message to indicate successful Encoder reset
/* telemetry.addData("Starting at", "%7d :%7d", /* telemetry.addData("Starting at", "%7d :%7d",
leftDrive.getCurrentPosition(), leftDrive.getCurrentPosition(),
@ -134,32 +146,67 @@ 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(2);
loop(); // driveForward(28);
while(opModeIsActive()) // {
{ // int blueleft = readColorLeft();
int blueleft = readColorLeft(); // int blueright = readColorRight();
int blueright = readColorRight(); // if (blueleft > 75)
if (blueleft > 50) // {
{ // //telemetry.addData("color sensor","left");
//telemetry.addData("color sensor","left"); // if(blueleft > blueright)
if(blueleft > blueright) // telemetry.addData("color sensor","left");
telemetry.addData("color sensor","left"); // turnLeft(90);
// straightLeft(2);
// driveForward(11);
} // driveForward(-30);
if (blueright > 50) // turnLeft(180);
{ // driveForward(19);
//telemetry.addData("color sensor", "right"); // straightLeft(7);
if(blueleft < blueright) // //use arm to place pixel
telemetry.addData("color sensor","right"); // straightRight(34);
} // driveForward(11);
else // terminateOpModeNow();
telemetry.addData("position","center"); //
telemetry.update(); //
sleep(250); //
//
} // }
// 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);
//
// }
@ -235,6 +282,14 @@ public class Autonomoustest extends LinearOpMode {
} }
public void raisearm(long seconds)
{
arm.setPower(.1);
sleep(seconds * 1000);
}
/* /*
* Method to perform a relative move, based on encoder counts. * Method to perform a relative move, based on encoder counts.
* Encoders are not reset as the move is based on the current position. * Encoders are not reset as the move is based on the current position.
@ -242,7 +297,9 @@ public class Autonomoustest extends LinearOpMode {
* 1) Move gets to the desired position * 1) Move gets to the desired position
* 2) Move runs out of time * 2) Move runs out of time
* 3) Driver stops the opmode running. * 3) Driver stops the opmode running.
*/ */
public void encoderDrive(double speed, public void encoderDrive(double speed,
double leftInches, double rightInches, double leftInches, double rightInches,
double timeoutS) { double timeoutS) {
@ -251,7 +308,6 @@ public class Autonomoustest extends LinearOpMode {
int newBackLeftTarget; int newBackLeftTarget;
int newbackRightTarget; int newbackRightTarget;
// Ensure that the opmode is still active
if (opModeIsActive()) { if (opModeIsActive()) {
// Determine new target position, and pass to motor controller // Determine new target position, and pass to motor controller
@ -294,11 +350,13 @@ public class Autonomoustest extends LinearOpMode {
telemetry.update(); telemetry.update();
} }
// Stop all motion;
leftDrive.setPower(0);
rightDrive.setPower(0); leftDrive.setPower(0);
backrightDrive.setPower(0); rightDrive.setPower(0);
backleftDrive.setPower(0); backrightDrive.setPower(0);
backleftDrive.setPower(0);
// Turn off RUN_TO_POSITION // Turn off RUN_TO_POSITION
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);