Hi! Our team has built an arm that has 180degrees of movement (front to back axis). We need to program it to keep the angle that we want (preferably pre-programmed angle states). We wanted to do it by PID and encoder. I calculated the ratios and have accurate angle of arm itself but when testing PID to setPoint it to 15deg it flew all the way back and slammed floor. The encoder values were good i think. The arm is somewhat heavy but motors have more than enough power to power it. (20% is fast enough for moving it)
I need help with programming it to angle states and making it keep angle with no driver input. I’m kinda worried with testing it, bcs i dont want to slam it again. If you know some good way to protect it when tesing that would help.
(the code is here: GitHub - SpiceGears/FRC-2023 at develop
develop branch is failed pid attempt, and main is working code on simple “can power when encoder is not over the top”)
Coding protection… use a clamping function to make sure you limit it while tuning it. As you get dialed in relax the clamp.
public void useOutput(double output, TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
// Add the feedforward to the PID output to get the motor output
double volts = MathUtil.clamp(output + feedforward, -6.0, 6.0);
m_motor.setVoltage(volts);
}
Ok, I haven’t really worked with feedforward before, and i didn’t do TrapeziodProfile thingy for setpoint and constraints, i just had simple angles of arm.
How do I find the velocity and all of the kSkVkA values for feedforward?
And what is this double output?
And if i implement it and change the setpoint going into this function, will the arm will smoothly translate to new setpoint?
I don’t understand the code in super()
public ArmSubsystem() {
super(
new ProfiledPIDController(
ArmConstants.kP,
0,
0,
new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond,
ArmConstants.kMaxAccelerationRadPerSecSquared)),
0);
m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(ArmConstants.kArmOffsetRads);
}
First of all, set kI to 0. That alone probably caused what you saw due to integral windup if you changed your setpoint directly from 0 to 180 or whatever.
Feedforward is the correct answer. You can use the ArmFeedForward class in WPILib (this is a direct link, but i highly recommend reading the whole page). You can get the necessary constants (kG, kS, kV) empirically, or by using sysID (which will also give you kA)
In the example it is the result of the profiled (trapezoid) PID Controller.
A trapezoid profile allows you to control the maximum velocity and acceleration of the output from the PID. You don’t need to do it… you can control the maximum with the clamp too. However, the trapezoid profile allows “smooth” increase in velocity of your arm.
It is also possible that your motor or your encoder are inverted so that the feedback is actively causing the error to get worse, instead of better. Before running your feedback loop, trying running your motor with positive power and ensure that the encoder readings are going up, not down.
private final ArmFeedforward m_feedforward =
new ArmFeedforward(
Constants.ARM.kSVolts, Constants.ARM.kGVolts, Constants.ARM.kVoltSecondPerRad, Constants.ARM.kAVoltSecondSquaredPerRad);
public ArmSubsystem() {
leftArmMaster = new VictorSP(PortMap.ARM.LEFT_MASTER_PORT);
rightArmMaster = new VictorSP(PortMap.ARM.RIGHT_MASTER_PORT);
leftArmSlave = new VictorSP(PortMap.ARM.LEFT_SLAVE_PORT);
rightArmSlave = new VictorSP(PortMap.ARM.RIGHT_SLAVE_PORT);
armGroup = new MotorControllerGroup(leftArmMaster, rightArmMaster, leftArmSlave, rightArmSlave);
armEncoder = new Encoder(PortMap.ARM.ENCODER_PORT_A, PortMap.ARM.ENCODER_PORT_B);
armEncoder.setDistancePerPulse(Constants.ARM.ENCODER_ANGLES_PER_ROTATION / Constants.ARM.ENCODER_TICK_RATE);
// armEncoder.setMaxPeriod(Constants.ARM.ENCODER_MIN_RATE);
armEncoder.setReverseDirection(Constants.ARM.ENCODER_REVERSE);
armEncoder.setSamplesToAverage(Constants.ARM.ENCODER_SAMPLES_TO_AVERAGE);
frontLimitSwitch = new DigitalInput(6);
backLimitSwitch = new DigitalInput(7);
ProfiledPIDController profiledController = new ProfiledPIDController(
Constants.ARM.kP,
Constants.ARM.kI,
Constants.ARM.kD,
new TrapezoidProfile.Constraints(
0.35, // in radians 0.35 == 20deg
0.35)); // in radians 0.35 == 20deg
}
//* Rotate arm to specific angle */
public void rotateArmByAngle(double angle) {
}
How I understand it, I don’t need to worry about the thetas, and just figure out the K’s in this formula for feedforward.
(Kg isnt going to be 9.81, it would be too easy right?)
And I don’t know how to implement it into a method. would something like this work and move the arm? :
No; you can tell this because the units would not make any sense. That equation needs to yield volts. The acceleration of gravity has units of meters per second squared.
You will have to either use manual calculation, an online calculator like ReCalc, or physical test routines to figure out the relevant gains. I suggest doing something a bit more precise than tapping on a joystick - do you have working telemetry so you can read the applied voltage on your dashboard?
not really, but reading voltage to dashboard should be doable easily.
I tested the robot yesterday, today i program it in home and tommorow i’ll have a session of testing it and changing gains etc.
Those are what really determine your gains, so if you’re not sure about those your gains probably won’t be correct. You should try to measure them as accurately as possible.
Efficiency is usually well-estimated by a 10% loss per reduction stage (a little less for gears/belt, a little more for chain).
Are you logging your encoders values? Before we run PID we always log our encoders and check they move to desired values when we move the subsystem manually (to rule out incorrect gear ratio entered in code, encoders inverted, etc).
how do i use SysId?
I want to get ready before tonight so i can manage time efficiently.
From what i expect, I hook it up to robot move the arm up and down, and record the data to .JSON and there in the middle i put this .JSON and the values should be ok?
And from the programming side, I don’t know if this will move the arm parallel to the floor or to zero position, and how does it know where it is, when it doesnt even use encoder?
