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 ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
public enum ArmPosition {
COLLECT,
@@ -558,6 +559,24 @@ public class Robot {
", 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() {
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() {
robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.controlMotorPIDF(
robot.arm,
"Arm",
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()),
12.0 / robot.getVoltage());
// robot.controlMotorPIDF(
// robot.arm,
// "Arm",
// robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()),
// 12.0 / robot.getVoltage());
robot.controlArmMotor(
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value())
);
}
private void automaticHardwareMonitor() {