diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index c58fc48..d22734a 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -48,6 +48,7 @@ public class RedCrabMinibot { public static double CLAW_ARM_kI = 0.0; public static double CLAW_ARM_kD = 0.0; public static double CLAW_ARM_kF = 0.0; + public static double CLAW_ARM_kPosP = 0.0; public static int CLAW_ARM_POSITION_TOLERANCE = 1; public static double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 public static double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 @@ -206,7 +207,7 @@ public class RedCrabMinibot { /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- --- Run Mode - clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); @@ -376,18 +377,26 @@ public class RedCrabMinibot { case "kF": // Feedforward CLAW_ARM_kF = v.value(); break; + case "KPosP": // Positional Proportional + CLAW_ARM_kPosP = v.value(); + break; } } - int armPos = clawArm.getCurrentPosition(); - clawArmPIDFController.setTolerance(CLAW_ARM_POSITION_TOLERANCE); - clawArmPIDFController.setPIDF(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF); - double pidf = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition()); +// int armPos = clawArm.getCurrentPosition(); +// clawArmPIDFController.setTolerance(CLAW_ARM_POSITION_TOLERANCE); +// clawArmPIDFController.setPIDF(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF); +// double pidf = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition()); // double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; // Limit pidf's value to max power range - double power = Range.clip(pidf, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED); +// double power = Range.clip(pidf, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED); - clawArm.setPower(power); + clawArm.setVelocityPIDFCoefficients(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF); + clawArm.setPositionPIDFCoefficients(CLAW_ARM_kPosP); + + double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES); + + clawArm.setVelocity(velocity); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json index de76ea2..abfa660 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json @@ -1 +1 @@ -{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 11:14:42 -0600","spec_version":2,"revision":417},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx5000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix8000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 0","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"04-00","comment":"@PathEnactor - Enact path","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90 degrees","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"06-00","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx80"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.1"},{"name":"kI","value":"Dx0.0"},{"name":"kP","value":"Dx0.5"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx180.0"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"velocity_max_in_mm","value":"Dx610"},{"name":"velocity_slow_in_mm","value":"Dx250"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0.7"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 11:45:16 -0600","spec_version":2,"revision":419},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx5000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix8000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 0","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"04-00","comment":"@PathEnactor - Enact path","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90 degrees","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"06-00","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx80"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.1"},{"name":"kI","value":"Dx0.0"},{"name":"kP","value":"Dx0.5"},{"name":"KPosP","value":"Dx1.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60.0"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"velocity_max_in_mm","value":"Dx610"},{"name":"velocity_slow_in_mm","value":"Dx250"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0.7"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file