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.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
/**
@ -72,6 +74,10 @@ public class Autonomoustest extends LinearOpMode {
private DcMotor backleftDrive = null;
private ColorSensor colorRight = 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
public void runOpMode() {
// Initialize the drive system variables.
// 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
@ -109,17 +118,20 @@ 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);
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(),
@ -134,32 +146,67 @@ 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);
loop();
while(opModeIsActive())
{
int blueleft = readColorLeft();
int blueright = readColorRight();
if (blueleft > 50)
{
//telemetry.addData("color sensor","left");
if(blueleft > blueright)
telemetry.addData("color sensor","left");
}
if (blueright > 50)
{
//telemetry.addData("color sensor", "right");
if(blueleft < blueright)
telemetry.addData("color sensor","right");
}
else
telemetry.addData("position","center");
telemetry.update();
sleep(250);
}
// 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);
//
// }
@ -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.
* 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
* 2) Move runs out of time
* 3) Driver stops the opmode running.
*/
public void encoderDrive(double speed,
double leftInches, double rightInches,
double timeoutS) {
@ -251,7 +308,6 @@ public class Autonomoustest extends LinearOpMode {
int newBackLeftTarget;
int newbackRightTarget;
// Ensure that the opmode is still active
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller
@ -294,11 +350,13 @@ public class Autonomoustest extends LinearOpMode {
telemetry.update();
}
// Stop all motion;
leftDrive.setPower(0);
rightDrive.setPower(0);
backrightDrive.setPower(0);
backleftDrive.setPower(0);
leftDrive.setPower(0);
rightDrive.setPower(0);
backrightDrive.setPower(0);
backleftDrive.setPower(0);
// Turn off RUN_TO_POSITION
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);