TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

An Autonomous implemented for Pizza on RED RIGHT and BLUE LEFT

Browse Source
This commit is contained in:
Cyberarm
2023-11-13 20:19:16 -06:00
parent ef919b821b
commit df86f9c11d
7 changed files with 695 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java
View File

@@ -1,5 +1,6 @@
package dev.cyberarm.minibots.pizza;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
@@ -7,7 +8,9 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
@@ -29,11 +32,18 @@ public class PizzaMinibot {
public IMU imu;
public Servo gripper, arm;
public int armStackPosition = -1;
final public TimeCraftersConfiguration config;
final public double wheelCircumference, distancePerTick;
public final double imuAngleOffset, initialFacing;
private CyberarmEngine engine;
public PizzaMinibot(CyberarmEngine engine) {
this.engine = engine;
this.config = new TimeCraftersConfiguration("Pizza_2023");
leftFront = new MotorEx(engine.hardwareMap, "leftFront");
rightFront = new MotorEx(engine.hardwareMap, "rightFront");
@@ -46,8 +56,15 @@ public class PizzaMinibot {
leftFront.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
double wheelRadius = 75.0 / 2; // mm
double distancePerTick = Math.PI * wheelRadius * wheelRadius;
leftFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
wheelCircumference = Math.PI * (75.0 * 4); //wheelRadius * wheelRadius; //-- times 2 is a hack... --//
distancePerTick = (28 * 20) / wheelCircumference; // raw motor encoder * gear ratio
leftFront.setDistancePerPulse(distancePerTick);
rightFront.setDistancePerPulse(distancePerTick);
leftBack.setDistancePerPulse(distancePerTick);
@@ -64,6 +81,9 @@ public class PizzaMinibot {
imu.initialize(parameters);
imuAngleOffset = 0;
initialFacing = facing();
gripper = engine.hardwareMap.servo.get("gripper");
arm = engine.hardwareMap.servo.get("arm");
@@ -114,4 +134,46 @@ public class PizzaMinibot {
engine.telemetry.addLine();
}
public double distanceMM(int ticks) {
return distancePerTick * ticks;
}
public double initialFacing() {
return initialFacing;
}
public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
public double heading() {
return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}
public double turnRate() {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
}
public boolean isBetween(double value, double min, double max) {
return value >= min && value <= max;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
public double angleDiff(double from, double to) {
double value = (to - from + 180);
double fmod = (value - 0.0) % (360.0 - 0.0);
return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180;
}
public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
}

185
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousBlueLeftEngine.java Normal file
View File

@@ -0,0 +1,185 @@
package dev.cyberarm.minibots.pizza.engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
import dev.cyberarm.minibots.pizza.states.Rotate;
import dev.cyberarm.minibots.pizza.states.ServoMove;
import dev.cyberarm.minibots.pizza.states.StrafeMove;
import dev.cyberarm.minibots.pizza.states.TankMove;
@Autonomous(name = "Cyberarm Pizza Autonomous BLUE LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop")
public class PizzaAutonomousBlueLeftEngine extends CyberarmEngine {
private PizzaMinibot robot;
@Override
public void setup() {
this.robot = new PizzaMinibot(this);
robot.imu.resetYaw();
/// Close the gripper
addState(new ServoMove(
robot,
"gripper",
PizzaMinibot.GRIPPER_CLOSED,
100
));
/// Move Purple Pixel into position
addState(new TankMove(
robot,
-900,
-900,
-0.15,
-0.17,
10,
5000
));
addState(new Rotate(
robot,
0,
0.15,
1,
5000
));
/// Retreat from pixel
addState(new TankMove(
robot,
150,
150,
0.15,
0.17,
10,
5000
));
/// Rotate "towards" backstage board
addState(new Rotate(
robot,
90,
0.15,
1,
5000
));
/// Move clear of the pixel
addState(new TankMove(
robot,
500,
500,
0.15,
0.17,
10,
5000
));
/// Strafe so arm is in range of board
addState(new StrafeMove(
robot,
1000,
0.5,
10,
5000
));
/// Correct robots rotation "towards" backstage board
addState(new Rotate(
robot,
90,
0.15,
1,
5000
));
/// Move up to the backboard and pre-position arm
addState(new TankMove(
robot,
500,
500,
0.15,
0.17,
10,
5000
));
addParallelStateToLastState(new ServoMove(
robot,
"arm",
PizzaMinibot.ARM_DELIVER,
1000
));
/// Correct rotation; Rotate "towards" backstage board
addState(new Rotate(
robot,
90,
0.15,
1,
5000
));
/// Move up against the backboard
addState(new TankMove(
robot,
200,
200,
0.15,
0.17,
10,
1250
));
/// Correct rotation again
addState(new Rotate(
robot,
90,
0.15,
1,
5000
));
/// Deposit pixel on backboard
addState(new ServoMove(
robot,
"gripper",
PizzaMinibot.GRIPPER_OPEN,
1000
));
/// Stow the arm
addState(new ServoMove(
robot,
"arm",
PizzaMinibot.ARM_STOW,
250
));
/// Move away from the backboard
addState(new TankMove(
robot,
-250,
-250,
-0.15,
-0.17,
10,
5000
));
/// Rotate towards the corner
addState(new Rotate(
robot,
178,
0.15,
1,
5000
));
/// Move into corner
addState(new TankMove(
robot,
-1000,
-1000,
-0.15,
-0.17,
10,
5000
));
}
@Override
public void loop() {
robot.standardTelemetry();
super.loop();
}
}

157
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousRedRightEngine.java Normal file
View File

@@ -0,0 +1,157 @@
package dev.cyberarm.minibots.pizza.engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
import dev.cyberarm.minibots.pizza.states.Rotate;
import dev.cyberarm.minibots.pizza.states.ServoMove;
import dev.cyberarm.minibots.pizza.states.TankMove;
@Autonomous(name = "Cyberarm Pizza Autonomous RED RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop")
public class PizzaAutonomousRedRightEngine extends CyberarmEngine {
private PizzaMinibot robot;
@Override
public void setup() {
this.robot = new PizzaMinibot(this);
robot.imu.resetYaw();
/// Close the gripper
addState(new ServoMove(
robot,
"gripper",
PizzaMinibot.GRIPPER_CLOSED,
100
));
/// Move Purple Pixel into position
addState(new TankMove(
robot,
-900,
-900,
-0.15,
-0.17,
10,
5000
));
addState(new Rotate(
robot,
0,
0.15,
1,
5000
));
/// Retreat from pixel
addState(new TankMove(
robot,
150,
150,
0.15,
0.17,
10,
5000
));
/// Rotate "towards" backstage board
addState(new Rotate(
robot,
270,
0.15,
1,
5000
));
/// Move up to the backboard and pre-position arm
addState(new TankMove(
robot,
900,
900,
0.15,
0.17,
10,
5000
));
addParallelStateToLastState(new ServoMove(
robot,
"arm",
PizzaMinibot.ARM_DELIVER,
1000
));
/// Correct rotation; Rotate "towards" backstage board
addState(new Rotate(
robot,
270,
0.15,
1,
5000
));
/// Move up against the backboard
addState(new TankMove(
robot,
200,
200,
0.15,
0.17,
10,
5000
));
/// Correct rotation again
addState(new Rotate(
robot,
270,
0.15,
1,
5000
));
/// Deposit pixel on backboard
addState(new ServoMove(
robot,
"gripper",
PizzaMinibot.GRIPPER_OPEN,
1000
));
/// Stow the arm
addState(new ServoMove(
robot,
"arm",
PizzaMinibot.ARM_STOW,
250
));
/// Move away from the backboard
addState(new TankMove(
robot,
-250,
-250,
-0.15,
-0.17,
10,
5000
));
/// Rotate towards the corner
addState(new Rotate(
robot,
170,
0.15,
1,
5000
));
/// Move into corner
addState(new TankMove(
robot,
-500,
-500,
-0.15,
-0.17,
10,
5000
));
}
@Override
public void loop() {
robot.standardTelemetry();
super.loop();
}
}

80
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Rotate.java Normal file
View File

@@ -0,0 +1,80 @@
package dev.cyberarm.minibots.pizza.states;
import com.qualcomm.robotcore.hardware.Servo;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
public class Rotate extends CyberarmState {
final private PizzaMinibot robot;
final private String groupName, actionName;
final private double heading, velocity, tolerance;
final private int timeoutMS;
public Rotate(PizzaMinibot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.heading = robot.config.variable(groupName, actionName, "heading").value();
this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
}
public Rotate(PizzaMinibot robot, double heading, double velocity, double tolerance, int timeoutMS) {
this.groupName = "";
this.actionName = "";
this.robot = robot;
this.heading = heading;
this.velocity = velocity;
this.tolerance = tolerance;
this.timeoutMS = timeoutMS;
}
@Override
public void start() {
}
@Override
public void exec() {
if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
return;
}
if (runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
if (robot.angleDiff(robot.facing() + robot.turnRate(), heading) < 0) {
robot.left.set(-velocity);
robot.right.set(velocity);
} else {
robot.left.set(velocity);
robot.right.set(-velocity);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Robot Heading", robot.facing());
engine.telemetry.addData("Robot Target Heading", heading);
engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
}
}

55
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/ServoMove.java Normal file
View File

@@ -0,0 +1,55 @@
package dev.cyberarm.minibots.pizza.states;
import com.qualcomm.robotcore.hardware.Servo;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
public class ServoMove extends CyberarmState {
final private PizzaMinibot robot;
final private String groupName, actionName;
final private String servoName;
final private double position;
final private int timeoutMS;
final private Servo servo;
public ServoMove(PizzaMinibot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.servoName = robot.config.variable(groupName, actionName, "servoName").value();
this.position = robot.config.variable(groupName, actionName, "position").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
this.servo = engine.hardwareMap.servo.get(servoName);
}
public ServoMove(PizzaMinibot robot, String servoName, double position, int timeoutMS) {
this.groupName = "";
this.actionName = "";
this.robot = robot;
this.servoName = servoName;
this.position = position;
this.timeoutMS = timeoutMS;
this.servo = engine.hardwareMap.servo.get(servoName);
}
@Override
public void start() {
this.servo.setPosition(this.position);
}
@Override
public void exec() {
if (runTime() >= timeoutMS) {
setHasFinished(true);
}
}
}

62
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/StrafeMove.java Normal file
View File

@@ -0,0 +1,62 @@
package dev.cyberarm.minibots.pizza.states;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
public class StrafeMove extends CyberarmState {
final private PizzaMinibot robot;
final private String groupName, actionName;
final private double distanceMM, velocity;
final private int tolerance, timeoutMS;
public StrafeMove(PizzaMinibot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value();
this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
}
public StrafeMove(PizzaMinibot robot, double distanceMM, double velocity, int tolerance, int timeoutMS) {
this.groupName = "";
this.actionName = "";
this.robot = robot;
this.distanceMM = distanceMM;
this.velocity = velocity;
this.tolerance = tolerance;
this.timeoutMS = timeoutMS;
}
@Override
public void start() {
robot.leftFront.setTargetDistance(distanceMM);
robot.left.setPositionTolerance(tolerance);
robot.right.setPositionTolerance(tolerance);
double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
robot.leftFront.set(motorVelocity);
robot.rightFront.set(-motorVelocity);
robot.leftBack.set(-motorVelocity);
robot.rightBack.set(motorVelocity);
}
@Override
public void exec() {
if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
}
}

92
TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/TankMove.java Normal file
View File

@@ -0,0 +1,92 @@
package dev.cyberarm.minibots.pizza.states;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.pizza.PizzaMinibot;
public class TankMove extends CyberarmState {
final private PizzaMinibot robot;
final private String groupName, actionName;
final private double leftDistanceMM, rightDistanceMM;
final private double leftVelocity, rightVelocity;
final private int tolerance, timeoutMS;
public TankMove(PizzaMinibot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.leftDistanceMM = robot.config.variable(groupName, actionName, "leftDistanceMM").value();
this.rightDistanceMM = robot.config.variable(groupName, actionName, "rightDistanceMM").value();
this.leftVelocity = robot.config.variable(groupName, actionName, "leftVelocity").value();
this.rightVelocity = robot.config.variable(groupName, actionName, "rightVelocity").value();
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
}
public TankMove(PizzaMinibot robot, double leftDistanceMM, double rightDistanceMM, double leftVelocity, double rightVelocity, int tolerance, int timeoutMS) {
this.groupName = "";
this.actionName = "";
this.robot = robot;
this.leftDistanceMM = leftDistanceMM;
this.rightDistanceMM = rightDistanceMM;
this.leftVelocity = leftVelocity;
this.rightVelocity = rightVelocity;
this.tolerance = tolerance;
this.timeoutMS = timeoutMS;
}
@Override
public void start() {
robot.left.resetEncoder();
robot.right.resetEncoder();
robot.left.setTargetDistance(leftDistanceMM);
robot.right.setTargetDistance(rightDistanceMM);
robot.left.setPositionTolerance(tolerance);
robot.right.setPositionTolerance(tolerance);
robot.left.set(leftVelocity);
robot.right.set(rightVelocity);
}
@Override
public void exec() {
if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
robot.left.set(0);
}
if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
robot.right.set(0);
}
if (
(robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
(Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) ||
runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
engine.telemetry.addData("Left velocity", robot.left.getVelocity());
engine.telemetry.addLine();
engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
engine.telemetry.addData("Right velocity", robot.right.getVelocity());
}
}