autos that can park
Former-commit-id: d969dffebee5e97ae66bbfc46f84f2d6b661b2db
This commit is contained in:
nn2wg
parent
d7906d5634
commit
1c819d1015
|
|
@ -1,34 +1,22 @@
|
|||
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
||||
|
||||
import java.util.Locale;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
|
||||
|
||||
@Autonomous
|
||||
public class AbhinavAutoTest extends LinearOpMode{
|
||||
public class AutoTester extends LinearOpMode{
|
||||
//change values
|
||||
public static final double REV_COUNT= 1120;//counts per revolution
|
||||
public static final double DIAMETER = 4;
|
||||
public static final double GEAR_RATIO = 5;
|
||||
private static final double INCHES_ROTATION = (REV_COUNT * GEAR_RATIO) / (DIAMETER * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor;
|
||||
Servo rotateClawServo , clawServo;
|
||||
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
|
||||
Servo bucketServo;
|
||||
|
||||
Orientation angles = new Orientation();
|
||||
double finalAngle;
|
||||
Acceleration gravity;
|
||||
|
||||
public void runOpMode() throws InterruptedException{
|
||||
|
||||
|
|
@ -39,6 +27,7 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
|
||||
intakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
|
@ -48,8 +37,7 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
rearLeft.setTargetPosition(0);
|
||||
rearRight.setTargetPosition(0);
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
resetMotorDirections();
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
|
@ -58,58 +46,77 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
|
||||
waitForStart();
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+350);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+350);
|
||||
setSpeed(0.2);
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
driveForward(-10, 0.2);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
sleep(4000);
|
||||
|
||||
driveForward(1000,.5);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(5,0.5);
|
||||
|
||||
//sleep(1000);
|
||||
|
||||
/*
|
||||
setSpeed(0);
|
||||
|
||||
driveBackward(5,0.5);
|
||||
|
||||
sleep(1000);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
strafeLeft(5,0.5);
|
||||
|
||||
sleep(1000);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
strafeRight(5,0.5);
|
||||
|
||||
sleep(1000);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
while(opModeIsActive()){
|
||||
|
||||
// carouselPosition();
|
||||
|
||||
//sleep(1000);
|
||||
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+250);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+250);
|
||||
setSpeed(0.2);
|
||||
while (frontRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
*/
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(100,0.2);
|
||||
setSpeed(0);
|
||||
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
public void driveForward(int distance, double speed){
|
||||
distance = (int) Math.round(distance * INCHES_ROTATION);
|
||||
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveBackward(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * INCHES_ROTATION * 0.7));
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
|
|
@ -119,7 +126,7 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
setSpeed(speed);
|
||||
}
|
||||
public void strafeLeft(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * INCHES_ROTATION * 0.7));
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
|
|
@ -128,7 +135,7 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
setSpeed(speed);
|
||||
}
|
||||
public void strafeRight(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * INCHES_ROTATION * 0.7));
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
|
|
@ -148,30 +155,8 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
}
|
||||
*/
|
||||
|
||||
public void carouselPosition(){
|
||||
turn(180,1);
|
||||
//sleep(1000);
|
||||
spool.setPower(1);
|
||||
//sleep(1000);
|
||||
rotateClawServo.setPosition(0.0);
|
||||
//sleep(1000);
|
||||
clawServo.setPosition(1.0);
|
||||
//sleep(1000);
|
||||
turn(-90,1);
|
||||
//sleep(1000);
|
||||
driveForward(15,1);
|
||||
//sleep(1000);
|
||||
intakeMotor.setPower(1);
|
||||
turn(-90,1000);
|
||||
driveForward(2,1);
|
||||
//sleep(1000);
|
||||
turn(-90,1000);
|
||||
//sleep(1000);
|
||||
driveForward(30,1);
|
||||
///sleep(1000);
|
||||
setSpeed(0);
|
||||
rotate90();
|
||||
setSpeed(0);
|
||||
public void turnCarousel(){
|
||||
|
||||
|
||||
}
|
||||
public void setSpeed(double speed){
|
||||
|
|
@ -179,34 +164,17 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
public double currAngle(){
|
||||
|
||||
// Orientation angle = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX,AngleUnit.DEGREES);
|
||||
double deltaAngle = angles.thirdAngle - angles.firstAngle;
|
||||
|
||||
if (deltaAngle < -180)
|
||||
deltaAngle += 360;
|
||||
else if (deltaAngle > 180)
|
||||
deltaAngle -= 360;
|
||||
|
||||
finalAngle += deltaAngle;
|
||||
//angles = angle;
|
||||
return finalAngle;
|
||||
|
||||
}
|
||||
String formatAngle(AngleUnit angleUnit, double angle) {
|
||||
return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
|
||||
}
|
||||
String formatDegrees(double degrees){
|
||||
return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
|
||||
}
|
||||
public void rotate90(){
|
||||
frontLeft.setPower(-1);
|
||||
rearLeft.setPower(-1);
|
||||
|
|
@ -215,19 +183,37 @@ public class AbhinavAutoTest extends LinearOpMode{
|
|||
|
||||
setSpeed(0);
|
||||
}
|
||||
public void turn(int degrees, double speed){
|
||||
double angleTurn = currAngle();
|
||||
if(angleTurn!=degrees && angleTurn>0 ){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
}else if(angleTurn!=degrees && angleTurn<0){
|
||||
frontLeft.setPower(-speed);
|
||||
rearLeft.setPower(-speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
}
|
||||
public void turnRight(int degrees, double speed){
|
||||
double turnDist = 10;
|
||||
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void turnLeft(int degrees, double speed){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
public void resetMotorDirections(){
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,224 @@
|
|||
|
||||
package org.firstinspires.ftc.teamcode.autoparking;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@Autonomous (group = "parking")
|
||||
public class BlueSideParkingAuto extends LinearOpMode{
|
||||
//change values
|
||||
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
|
||||
Servo bucketServo;
|
||||
|
||||
|
||||
public void runOpMode() throws InterruptedException{
|
||||
|
||||
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
||||
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
|
||||
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
|
||||
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
|
||||
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
|
||||
intakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontLeft.setTargetPosition(0);
|
||||
frontRight.setTargetPosition(0);
|
||||
rearLeft.setTargetPosition(0);
|
||||
rearRight.setTargetPosition(0);
|
||||
|
||||
resetMotorDirections();
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
waitForStart();
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+500);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+500);
|
||||
setSpeed(0.2);
|
||||
while (rearRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(-16, 0.2);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
sleep(4000);
|
||||
|
||||
driveForward(200,.8);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+250);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+250);
|
||||
setSpeed(0.2);
|
||||
while (frontRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(100,0.2);
|
||||
setSpeed(0);
|
||||
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
public void driveForward(int distance, double speed){
|
||||
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveBackward(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeLeft(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeRight(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
/*
|
||||
public void linearSlideExtendLevel1(){
|
||||
motorLinearSlide.setPower(-1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(0.0);
|
||||
rotateClawServo.setPosition(1);
|
||||
clawServo.setPosition(1.0);
|
||||
}
|
||||
*/
|
||||
|
||||
public void turnCarousel(){
|
||||
|
||||
|
||||
}
|
||||
public void setSpeed(double speed){
|
||||
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontLeft.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void rotate90(){
|
||||
frontLeft.setPower(-1);
|
||||
rearLeft.setPower(-1);
|
||||
frontRight.setPower(1);
|
||||
rearRight.setPower(1);
|
||||
|
||||
setSpeed(0);
|
||||
}
|
||||
public void turnRight(int degrees, double speed){
|
||||
double turnDist = 10;
|
||||
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void turnLeft(int degrees, double speed){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
public void resetMotorDirections(){
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,226 @@
|
|||
|
||||
package org.firstinspires.ftc.teamcode.autoparking;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@Autonomous (group = "parking")
|
||||
public class BlueSideParkingAutoDelayed extends LinearOpMode{
|
||||
//change values
|
||||
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
|
||||
Servo bucketServo;
|
||||
|
||||
|
||||
public void runOpMode() throws InterruptedException{
|
||||
|
||||
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
||||
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
|
||||
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
|
||||
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
|
||||
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
|
||||
intakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontLeft.setTargetPosition(0);
|
||||
frontRight.setTargetPosition(0);
|
||||
rearLeft.setTargetPosition(0);
|
||||
rearRight.setTargetPosition(0);
|
||||
|
||||
resetMotorDirections();
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
waitForStart();
|
||||
|
||||
sleep(4000);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+530);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+530);
|
||||
setSpeed(0.2);
|
||||
while (rearRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(-16, 0.2);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
sleep(4000);
|
||||
|
||||
driveForward(200,.5);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+250);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+250);
|
||||
setSpeed(0.2);
|
||||
while (frontRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(100,0.2);
|
||||
setSpeed(0);
|
||||
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
public void driveForward(int distance, double speed){
|
||||
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveBackward(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeLeft(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeRight(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
/*
|
||||
public void linearSlideExtendLevel1(){
|
||||
motorLinearSlide.setPower(-1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(0.0);
|
||||
rotateClawServo.setPosition(1);
|
||||
clawServo.setPosition(1.0);
|
||||
}
|
||||
*/
|
||||
|
||||
public void turnCarousel(){
|
||||
|
||||
|
||||
}
|
||||
public void setSpeed(double speed){
|
||||
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontLeft.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void rotate90(){
|
||||
frontLeft.setPower(-1);
|
||||
rearLeft.setPower(-1);
|
||||
frontRight.setPower(1);
|
||||
rearRight.setPower(1);
|
||||
|
||||
setSpeed(0);
|
||||
}
|
||||
public void turnRight(int degrees, double speed){
|
||||
double turnDist = 10;
|
||||
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void turnLeft(int degrees, double speed){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
public void resetMotorDirections(){
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,224 @@
|
|||
|
||||
package org.firstinspires.ftc.teamcode.autoparking;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
|
||||
|
||||
@Autonomous (group = "parking")
|
||||
public class RedSideParkingAuto extends LinearOpMode{
|
||||
//change values
|
||||
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
|
||||
Servo bucketServo;
|
||||
|
||||
|
||||
public void runOpMode() throws InterruptedException{
|
||||
|
||||
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
||||
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
|
||||
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
|
||||
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
|
||||
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
|
||||
intakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontLeft.setTargetPosition(0);
|
||||
frontRight.setTargetPosition(0);
|
||||
rearLeft.setTargetPosition(0);
|
||||
rearRight.setTargetPosition(0);
|
||||
|
||||
resetMotorDirections();
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
waitForStart();
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+350);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+350);
|
||||
setSpeed(0.2);
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
driveForward(-10, 0.2);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
sleep(4000);
|
||||
|
||||
driveForward(1000,.5);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+250);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+250);
|
||||
setSpeed(0.2);
|
||||
while (frontRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(100,0.2);
|
||||
setSpeed(0);
|
||||
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
public void driveForward(int distance, double speed){
|
||||
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveBackward(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeLeft(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeRight(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
/*
|
||||
public void linearSlideExtendLevel1(){
|
||||
motorLinearSlide.setPower(-1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(0.0);
|
||||
rotateClawServo.setPosition(1);
|
||||
clawServo.setPosition(1.0);
|
||||
}
|
||||
*/
|
||||
|
||||
public void turnCarousel(){
|
||||
|
||||
|
||||
}
|
||||
public void setSpeed(double speed){
|
||||
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontLeft.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void rotate90(){
|
||||
frontLeft.setPower(-1);
|
||||
rearLeft.setPower(-1);
|
||||
frontRight.setPower(1);
|
||||
rearRight.setPower(1);
|
||||
|
||||
setSpeed(0);
|
||||
}
|
||||
public void turnRight(int degrees, double speed){
|
||||
double turnDist = 10;
|
||||
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void turnLeft(int degrees, double speed){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
public void resetMotorDirections(){
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,226 @@
|
|||
|
||||
package org.firstinspires.ftc.teamcode.autoparking;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@Autonomous (group = "parking")
|
||||
public class RedSideParkingAutoDelayed extends LinearOpMode{
|
||||
//change values
|
||||
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
|
||||
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
|
||||
Servo bucketServo;
|
||||
|
||||
|
||||
public void runOpMode() throws InterruptedException{
|
||||
|
||||
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
||||
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
|
||||
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
|
||||
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
|
||||
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
|
||||
intakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontLeft.setTargetPosition(0);
|
||||
frontRight.setTargetPosition(0);
|
||||
rearLeft.setTargetPosition(0);
|
||||
rearRight.setTargetPosition(0);
|
||||
|
||||
resetMotorDirections();
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
waitForStart();
|
||||
sleep (4000);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+350);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+350);
|
||||
setSpeed(0.2);
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
driveForward(-10, 0.2);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
sleep(4000);
|
||||
|
||||
driveForward(1000,.5);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+250);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+250);
|
||||
setSpeed(0.2);
|
||||
while (frontRight.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
setSpeed(0);
|
||||
|
||||
driveForward(100,0.2);
|
||||
setSpeed(0);
|
||||
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
public void driveForward(int distance, double speed){
|
||||
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
while (frontLeft.isBusy()){
|
||||
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
|
||||
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
|
||||
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
|
||||
telemetry.addData("rearLeftEncoder", rearLeft.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveBackward(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeLeft(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
public void strafeRight(int distance, double speed){
|
||||
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
|
||||
|
||||
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
|
||||
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
|
||||
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
|
||||
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
|
||||
setSpeed(speed);
|
||||
}
|
||||
/*
|
||||
public void linearSlideExtendLevel1(){
|
||||
motorLinearSlide.setPower(-1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(1.0);
|
||||
sleep(1000);
|
||||
motorLinearSlide.setPower(0.0);
|
||||
rotateClawServo.setPosition(1);
|
||||
clawServo.setPosition(1.0);
|
||||
}
|
||||
*/
|
||||
|
||||
public void turnCarousel(){
|
||||
|
||||
|
||||
}
|
||||
public void setSpeed(double speed){
|
||||
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontLeft.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void rotate90(){
|
||||
frontLeft.setPower(-1);
|
||||
rearLeft.setPower(-1);
|
||||
frontRight.setPower(1);
|
||||
rearRight.setPower(1);
|
||||
|
||||
setSpeed(0);
|
||||
}
|
||||
public void turnRight(int degrees, double speed){
|
||||
double turnDist = 10;
|
||||
|
||||
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
setSpeed(speed);
|
||||
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void turnLeft(int degrees, double speed){
|
||||
frontLeft.setPower(speed);
|
||||
rearLeft.setPower(speed);
|
||||
frontRight.setPower(speed);
|
||||
rearRight.setPower(speed);
|
||||
|
||||
setSpeed(0);
|
||||
|
||||
}
|
||||
|
||||
public void resetMotorDirections(){
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rearRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
}
|
||||
}
|
||||
|
|
@ -3,15 +3,17 @@ package org.firstinspires.ftc.teamcode;
|
|||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@TeleOp
|
||||
public class controllerOpMode extends OpMode {
|
||||
private DcMotor frontRight, frontLeft, rearRight, rearLeft, spool, spinnyBoy, intakeMotor;
|
||||
private DcMotor spool, spinnyBoy, intakeMotor;
|
||||
private Servo bucketServo1, bucketServo2;
|
||||
private double drive,strafe, turn, linearSpeed;
|
||||
private double speed;
|
||||
MotorBox driveMotors;
|
||||
|
||||
double bucketServoPos;
|
||||
|
||||
|
||||
// part that actually runs
|
||||
|
|
@ -22,6 +24,7 @@ public class controllerOpMode extends OpMode {
|
|||
detectSpeedChange();
|
||||
detectSlideSpeedChange();
|
||||
|
||||
|
||||
// sets the driver strafe and turn values
|
||||
// drive is negated so 1 is forward
|
||||
drive = -1*gamepad1.left_stick_y;
|
||||
|
|
@ -35,19 +38,40 @@ public class controllerOpMode extends OpMode {
|
|||
// makes it drive
|
||||
// result is just debug values
|
||||
String result = driveMotors.drivePower(drive, strafe, turn, speed);
|
||||
spool.setPower(gamepad2.left_stick_y*linearSpeed);
|
||||
spool.setPower(-1*gamepad2.left_stick_y*linearSpeed);
|
||||
|
||||
// carousel spinner
|
||||
// spins for red on gamepad2's b
|
||||
if(gamepad2.b){
|
||||
spinnyBoy.setPower(0.5);
|
||||
}
|
||||
else {
|
||||
spinnyBoy.setPower(0);
|
||||
}
|
||||
// spins for blue on gamepad2's x
|
||||
if(gamepad2.x){
|
||||
spinnyBoy.setPower(-0.5);
|
||||
}
|
||||
else {
|
||||
spinnyBoy.setPower(0);
|
||||
}
|
||||
|
||||
// intakeMotor spinner
|
||||
// spins on gamepad2's a
|
||||
if(gamepad2.a){
|
||||
intakeMotor.setPower(0.5);
|
||||
}
|
||||
else {
|
||||
intakeMotor.setPower(0);
|
||||
}
|
||||
if(gamepad2.y){
|
||||
intakeMotor.setPower(-0.5);
|
||||
}
|
||||
else {
|
||||
intakeMotor.setPower(0);
|
||||
}
|
||||
// changes the bucketServo's position using the gamepad2's right stick y
|
||||
bucketServoPositionChange();
|
||||
|
||||
|
||||
//debugging
|
||||
|
|
@ -72,9 +96,19 @@ public class controllerOpMode extends OpMode {
|
|||
);
|
||||
speed = 0.25;
|
||||
linearSpeed = 0.25;
|
||||
bucketServoPos = 1;
|
||||
|
||||
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||
intakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
bucketServo1 = hardwareMap.get(Servo.class, "bucketServo1");
|
||||
bucketServo2 = hardwareMap.get(Servo.class, "bucketServo2");
|
||||
bucketServo1.setDirection(Servo.Direction.REVERSE);
|
||||
|
||||
bucketServo1.setPosition(bucketServoPos);
|
||||
bucketServo2.setPosition(bucketServoPos);
|
||||
|
||||
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
}
|
||||
|
||||
|
|
@ -103,4 +137,16 @@ public class controllerOpMode extends OpMode {
|
|||
}
|
||||
}
|
||||
}
|
||||
public void bucketServoPositionChange(){
|
||||
bucketServoPos += 0.001 * gamepad2.right_stick_y;
|
||||
if (bucketServoPos > 1){
|
||||
bucketServoPos = 1;
|
||||
}
|
||||
if (bucketServoPos < 0){
|
||||
bucketServoPos = 0;
|
||||
}
|
||||
bucketServo1.setPosition(bucketServoPos);
|
||||
bucketServo2.setPosition(bucketServoPos);
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@ public class DriveConstants {
|
|||
/*
|
||||
* These are motor constants that should be listed online for your motors.
|
||||
*/
|
||||
public static final double TICKS_PER_REV = 1120;
|
||||
public static final double MAX_RPM = 150;
|
||||
public static final double TICKS_PER_REV = 537.6;
|
||||
public static final double MAX_RPM = 312.5;
|
||||
|
||||
/*
|
||||
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,16 @@
|
|||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
@Autonomous
|
||||
public class getServoPosition extends LinearOpMode {
|
||||
private Servo bucketServo;
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
waitForStart();
|
||||
bucketServo = hardwareMap.get(Servo.class, "bucketServo");
|
||||
telemetry.addData("Position is", bucketServo.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue