TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 21:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Sync arm control changes

Browse Source
This commit is contained in:
cyberarm
2023-01-24 12:15:11 -06:00
parent 1724ca82ce
commit e287f3be66
2 changed files with 28 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -46,6 +46,7 @@ public class Robot {
private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>(); private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>(); private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>(); private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
public enum ArmPosition { public enum ArmPosition {
COLLECT, COLLECT,
@@ -558,6 +559,24 @@ public class Robot {
", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks."); ", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
} }
@SuppressLint("NewApi")
public void controlArmMotor(double targetVelocity) {
double time = System.currentTimeMillis();
double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
double deltaTime = (time - lastTiming) * 0.001;
double error = targetVelocity - arm.getVelocity();
double kp = 0.9;
newTargetVelocity += error * kp * deltaTime;
motorTargetVelocity.put("Arm", newTargetVelocity);
motorVelocityLastTiming.put("Arm", time);
arm.setVelocity(newTargetVelocity);
}
public double getVoltage() { public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
} }

14
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java
View File

@@ -127,11 +127,15 @@ public class ArmDriverControl extends CyberarmState {
private void automaticArmVelocity() { private void automaticArmVelocity() {
robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.controlMotorPIDF( // robot.controlMotorPIDF(
robot.arm, // robot.arm,
"Arm", // "Arm",
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()), // robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()),
12.0 / robot.getVoltage()); // 12.0 / robot.getVoltage());
robot.controlArmMotor(
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value())
);
} }
private void automaticHardwareMonitor() { private void automaticHardwareMonitor() {