karx/FreeOfCharge2022-23
1
1
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

MovementAPI in color sensor

Browse source
This commit is contained in:
Yash Karandikar 2022-11-08 12:53:17 -06:00
parent f26e7ef093
commit 4529cb03f9
2 changed files with 30 additions and 87 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java
View file

@ -1,35 +1,22 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Autonomous
public class AutoBlueColorSensor extends LinearOpMode {
SampleMecanumDrive mecanumDrive;
@Override
public void runOpMode() throws InterruptedException {
ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color");
mecanumDrive = new SampleMecanumDrive(hardwareMap);
API api = new API(this);
MovementAPI movementAPI = new MovementAPI(api);
waitForStart();
Trajectory firstMove = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, 60), Math.toRadians(270)),
new int[]{-34},
new int[]{40}
);
mecanumDrive.followTrajectory(firstMove);
movementAPI.move(0, 0.7);
api.pause(0.5);
movementAPI.stop();
api.pause(5);
int r = sensor.red();
@ -38,41 +25,26 @@ public class AutoBlueColorSensor extends LinearOpMode {
int largest = api.getLargest(r, g, b);
Trajectory endpos;
if (largest == g) {
// move to position 1
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -12, -12},
new int[]{60, 60, 12}
);
movementAPI.move(90, 0.7);
api.pause(0.25);
} else if (largest == r) {
// move to position 2
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -12, -12, -34},
new int[]{60, 60, 12, 12}
);
movementAPI.move(0, 0.7);
api.pause(0.25);
} else if (largest == b) {
// move to position 3
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -70, -60},
new int[]{60, 60, 30}
);
movementAPI.move(-90, 0.7);
api.pause(0.25);
} else {
// Color sensor broke, move to backup position
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -70},
new int[]{60, 60}
);
movementAPI.move(-180, 0.7);
api.pause(0.5);
movementAPI.move(90, 0.7);
api.pause(0.7);
}
mecanumDrive.followTrajectory(endpos);
movementAPI.stop();
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java
View file

@ -1,36 +1,22 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Autonomous
public class AutoRedColorSensor extends LinearOpMode {
SampleMecanumDrive mecanumDrive;
@Override
public void runOpMode() throws InterruptedException {
ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color");
mecanumDrive = new SampleMecanumDrive(hardwareMap);
API api = new API(this);
MovementAPI movementAPI = new MovementAPI(api);
waitForStart();
Trajectory firstMove = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, -60), Math.toRadians(90)),
new int[]{-34},
new int[]{-40}
);
mecanumDrive.followTrajectory(firstMove);
movementAPI.move(0, 0.7);
api.pause(0.5);
movementAPI.stop();
api.pause(5);
int r = sensor.red();
@ -39,41 +25,26 @@ public class AutoRedColorSensor extends LinearOpMode {
int largest = api.getLargest(r, g, b);
Trajectory endpos;
if (largest == g) {
// move to position 1
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -12, -12},
new int[]{-60, -60, -12}
);
movementAPI.move(-90, 0.7);
api.pause(0.25);
} else if (largest == r) {
// move to position 2
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -12, -12, -34},
new int[]{-60, -60, -12, -12}
);
movementAPI.move(0, 0.7);
api.pause(0.25);
} else if (largest == b) {
// move to position 3
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -70, -60},
new int[]{-60, -60, -30}
);
movementAPI.move(90, 0.7);
api.pause(0.25);
} else {
// Color sensor broke, move to backup position
endpos = api.makeTrajectories(
mecanumDrive,
firstMove.end(),
new int[]{-34, -70},
new int[]{-60, -60}
);
movementAPI.move(-180, 0.7);
api.pause(0.5);
movementAPI.move(-90, 0.7);
api.pause(0.7);
}
mecanumDrive.followTrajectory(endpos);
movementAPI.stop();
}
}