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

Compare commits

base: TimeCrafters:4450c7e48a0e566de73835283b7147d40ca6b7f1
Branches Tags
TimeCrafters:master
...
compare: TimeCrafters:71374a57eaab983d9c84d10a88a409b7a0cda91d
Branches Tags
TimeCrafters:master

3 Commits
4450c7e48a ... 71374a57ea

Author SHA1 Message Date
Cyberarm
71374a57ea RedCrab: Added rail leds 2024-02-08 20:33:48 -06:00
SpencerPiha
1339cc8ebf started to erase the velocity code to start working on the autonomous with just power 2024-02-08 20:31:35 -06:00
SpencerPiha
ebbbc9c263 wrote the implementation for tuning velocity, I plan to tune in the teleOp and then copy over to autonomous 2024-02-06 23:47:27 -06:00
7 changed files with 156 additions and 73 deletions
Expand all files Collapse all files

121
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -22,6 +22,8 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import java.util.ArrayList;
import dev.cyberarm.drivers.EncoderCustomKB2040;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
@@ -87,7 +89,9 @@ public class RedCrabMinibot {
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
public final DcMotorEx deadWheelXLeft, deadWheelXRight;
public final EncoderCustomKB2040 deadWheelYCenter;
public final DigitalChannel greenLED, redLED;
public final DigitalChannel ledTopGreen, ledTopRed, ledRail0Green, ledRail0Red, ledRail1Green, ledRail1Red, ledRail2Green, ledRail2Red,
ledRail3Green, ledRail3Red;
public final ArrayList<DigitalChannel> railGreenLEDs = new ArrayList<>(), railRedLEDs = new ArrayList<>();
public final boolean LED_OFF = true, LED_ON = false;
final CyberarmEngine engine;
@@ -252,13 +256,48 @@ public class RedCrabMinibot {
resetDeadWheels();
/// LED(s) ///
greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING
redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red");
ledTopGreen = engine.hardwareMap.get(DigitalChannel.class, "led_top_green");
ledTopRed = engine.hardwareMap.get(DigitalChannel.class, "led_top_red");
// RED and GREEN are swapped for the 'new' LEDs, and I'm to lazy to fix the robot config
ledRail0Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_red");
ledRail0Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_green");
ledRail1Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_red");
ledRail1Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_green");
ledRail2Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_red");
ledRail2Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_green");
ledRail3Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_red");
ledRail3Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_green");
greenLED.setMode(DigitalChannel.Mode.OUTPUT);
redLED.setMode(DigitalChannel.Mode.OUTPUT);
greenLED.setState(LED_OFF);
redLED.setState(LED_ON);
railGreenLEDs.add(ledRail0Green);
railGreenLEDs.add(ledRail1Green);
railGreenLEDs.add(ledRail2Green);
railGreenLEDs.add(ledRail3Green);
railRedLEDs.add(ledRail0Red);
railRedLEDs.add(ledRail1Red);
railRedLEDs.add(ledRail2Red);
railRedLEDs.add(ledRail3Red);
ledTopGreen.setMode(DigitalChannel.Mode.OUTPUT);
ledTopRed.setMode(DigitalChannel.Mode.OUTPUT);
ledRail0Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail0Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail1Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail1Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail2Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail2Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail3Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail3Red.setMode(DigitalChannel.Mode.OUTPUT);
/// Turn off green LEDs and turn red ones on
ledTopGreen.setState(LED_OFF);
ledTopRed.setState(LED_ON);
for (DigitalChannel led : railGreenLEDs) {
led.setState(LED_OFF);
}
for (DigitalChannel led : railRedLEDs) {
led.setState(LED_ON);
}
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -274,8 +313,16 @@ public class RedCrabMinibot {
public void shutdown() {
// Prevent LED(s) from being on while idle
greenLED.setMode(DigitalChannel.Mode.INPUT);
redLED.setMode(DigitalChannel.Mode.INPUT);
ledTopGreen.setMode(DigitalChannel.Mode.INPUT);
ledTopRed.setMode(DigitalChannel.Mode.INPUT);
ledRail0Green.setMode(DigitalChannel.Mode.INPUT);
ledRail0Red.setMode(DigitalChannel.Mode.INPUT);
ledRail1Green.setMode(DigitalChannel.Mode.INPUT);
ledRail1Red.setMode(DigitalChannel.Mode.INPUT);
ledRail2Green.setMode(DigitalChannel.Mode.INPUT);
ledRail2Red.setMode(DigitalChannel.Mode.INPUT);
ledRail3Green.setMode(DigitalChannel.Mode.INPUT);
ledRail3Red.setMode(DigitalChannel.Mode.INPUT);
}
public void reloadConfig() {
@@ -571,4 +618,60 @@ public class RedCrabMinibot {
return Utilities.isBetween(distanceMM, travelledDistanceMM - toleranceMM, travelledDistanceMM + toleranceMM);
}
public void ledController() {
if (autonomous) {
if (engine.runTime() < 20_000.0) { // KEEP CALM and CARRY ON
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_OFF);
ledSetRailGreenLEDs(LED_ON);
} else if (engine.runTime() < 25_000.0) { // RUNNING LOW ON TIME
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_ON);
} else { // 5 SECONDS LEFT!
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_OFF);
}
} else {
if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_OFF);
ledSetRailGreenLEDs(LED_ON);
} else if (engine.runTime() >= 80_000.0) { // GET READY
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_ON);
} else { // KEEP CALM and CARRY ON
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_OFF);
}
}
}
public void ledSetRailGreenLEDs(boolean ledState) {
for (DigitalChannel led : railGreenLEDs) {
led.setState(ledState);
}
}
public void ledSetRailRedLEDs(boolean ledState) {
for (DigitalChannel led : railRedLEDs) {
led.setState(ledState);
}
}
}

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java
View File

@@ -15,29 +15,7 @@ public abstract class RedCrabEngine extends CyberarmEngine {
robot.standardTelemetry();
if (robot.autonomous) {
if (runTime() < 20_000.0) { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() < 25_000.0) { // RUNNING LOW ON TIME
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // 5 SECONDS LEFT!
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
} else {
if (runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() >= 80_000.0) { // GET READY
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
}
robot.ledController();
}
@Override

12
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java
View File

@@ -41,17 +41,17 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
addState(new CameraVisionState(robot));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));

23
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -12,9 +12,12 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class DriveToCoordinatesState extends CyberarmState {
CompetitionRobotV1 robot;
public double xTarget;
public double yTarget;
public double hTarget;
public static double xTarget;
public static double yTarget;
public static double hTarget;
public static double maxVelocityX;
public static double maxVelocityY;
public double maxVelocityH;
public boolean posAchieved = false;
public boolean armDrive;
public int objectPos;
@@ -57,10 +60,16 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void exec() {
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
setHasFinished(true);
// setHasFinished(true);
} else {
if (armDrive) {
@@ -69,7 +78,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
setHasFinished(true);
// setHasFinished(true);
}
}
} else {
@@ -79,13 +88,15 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
setHasFinished(true);
// setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("x pos", robot.positionX);
engine.telemetry.addData("y pos", robot.positionY);
engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH));

29
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -52,7 +52,7 @@ public class CompetitionRobotV1 extends Robot {
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.03, Xi = 0, Xd = 0;
public static double xvp = -0.03, xvi = 0, xvd = 0;
public static double xvp = -0, xvi = 0, xvd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0;
public static double yvp = 0.03, yvi = 0, yvd = 0;
@@ -75,7 +75,7 @@ public class CompetitionRobotV1 extends Robot {
public int oldRightPosition = 0;
public int oldLeftPosition = 0;
public int oldAuxPosition = 0;
public final static double L = 22.5; // distance between left and right encoder in cm
public final static double L = 20.9; // distance between left and right encoder in cm
final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
public final static double R = 3; // wheel radius in cm
final static double N = 8192; // encoder ticks per revolution (REV encoder)
@@ -112,7 +112,7 @@ public class CompetitionRobotV1 extends Robot {
public double xVelocity;
public double yVelocity;
public double deltaTime;
public double deltaTime = 0;
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
@@ -299,27 +299,6 @@ public class CompetitionRobotV1 extends Robot {
double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi);
return output;
}
public double XVeloPIDControl ( double reference, double current){
double error = (reference - current);
xVeloIntegralSum += error * xVeloTimer.seconds();
double derivative = (error - xVeloLastError) / xVeloTimer.seconds();
xTimer.reset();
double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi);
return output;
}
public double YVeloPIDControl ( double reference, double current){
double error = (reference - current);
yVeloIntegralSum += error * yVeloTimer.seconds();
double derivative = (error - yVeloLastError) / xTimer.seconds();
xTimer.reset();
double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi);
return output;
}
public double XPIDControl ( double reference, double current){
double error = (reference - current);
@@ -376,8 +355,6 @@ public class CompetitionRobotV1 extends Robot {
public void DriveToCoordinates () {
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
XVeloPIDControl(targetVelocityX, xVelocity);
YVeloPIDControl(targetVelocityY, yVelocity);
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);

18
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -28,6 +28,9 @@ public class CompetitionTeleOpState extends CyberarmState {
public static double holdPos = 0.55 ;
public double maxVelocityX;
public double maxVelocityY;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
private double targetHeading;
@@ -110,7 +113,6 @@ public class CompetitionTeleOpState extends CyberarmState {
}
}
public void DriveTrainTeleOp () {
boolean lbs1 = engine.gamepad1.left_stick_button;
@@ -337,10 +339,17 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lastExecutedTime = System.currentTimeMillis();
}
@Override
public void exec() {
public void exec() { //------------------------------------------------------------------------------------------------------ EXEC Start
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
robot.OdometryLocalizer();
if (engine.gamepad2.start && engine.gamepad2.x){
@@ -409,10 +418,15 @@ public class CompetitionTeleOpState extends CyberarmState {
ClawControlTeleOp();
robot.velocityChecker();
}
@Override
public void telemetry () {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("Dnl1", robot.Dnl1);
engine.telemetry.addData("Dnr2", robot.Dnr2);
engine.telemetry.addData("x pos", robot.positionX);