Team 3244

Motion Magic

Team 3244

About Us

Corey Applegate

• 21 Years Industrial Controls Technician using Rockwell Automation PLC and Motion Control, Kuka Robotics, Fanuc Robotics
• 4 years Mentoring a LEGO Mindstorms Club for 5-6 graders
• This is my 4th Season with FIRST
• LimeLight V1 and V2 Beta Tester
• 2019 Java VSCode Beta Tester

​

Motion Magic

Natalie Fischer

• Junior Albany High School
• Member of Both 4226 Huskies and 3244 Granite City Gearheads
• Team responsibilities
• CoDriver, Build, Scouting lead
• Hobbies
• ​

Team 3244

How are some ways we control our systems?

• Open Loop
• An output is commanded with no feedback that it happened.
• Not measurably repeatable

​

• Close Loop (Velocity and Position)
• A setpoint is defined. Then a process calculates the error from the current value to the setpoint and commands the system to react.
• PID(F)
• Motion Magic
• Motion Profile
• Measurably repeatable results

Motion Magic

Team 3244

Motion Magic

Closed Loop Control

PID

Team 3244

Motion Magic

Closed Loop Control

Motion Magic

Team 3244

Motion Magic

Closed Loop Control

Motion Profile

Team 3244

What we will cover

1. Where to Download and how to configure sample code to fit our system
2. Check motor and sensor polarity
1. These must be in phase meaning positive motor command results in positive feedback
3. FeedForward this term is used to tell the system how to scale native units per 100ms to 100% output
4. Set Motion Magic Cruise Velocity and Acceleration
5. kp helps the closed-loop react to error
6. ki closes steady state error when at rest
7. kd dampens overshoot
8. Finally look at the Arbitrary feedforward to remove gravity

​

​

Motion Magic

Team 3244

Tools we will Use

• Phoenix Tuner
• Plot Tab

​

• Console Output

​

• ShuffleBoard

​

• CTRE Sample Code
• https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages

​

• GCGH Motion Magic Worksheet
• Formulas for Motion Magic https://tinyurl.com/GCGH-MM-WS

​

​

​

Motion Magic

Team 3244

Download and configure sample code to fit our system

Page 1 of 3

• We are going to focus on the MotionMagic project
• Configure for our system
• Set the _talon CAN address
• Set the port for the _joystick
• Comment out or set any followers that maybe in your system
• Config Selected Feedback Sensor
• Adjust Sensor and Motor phase (Will Cover more later)
• Zero all the PIDF values
• Zero Cruise Velocity and Acceleration

​

​

​

Motion Magic

Team 3244

Download and configure Page 2 of 3

• Special case setting for Limit switches and referencing the encoder
• /* Zero the sensor */
• _talon.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs);

​

*Review Sample logic then continue

​

• Special Cases and other configurations we commonly use.
• _talon.configClearPositionOnLimitR(true, Constants.kTimeoutMs);
• _talon.configFeedbackNotContinuous(true, Constants.kTimeoutMs);*

​

• *More discussion found here on this: Its a bandaid for missuse when using an analog feedback device but we used it with no problems https://www.chiefdelphi.com/t/using-talon-motion-magic-with-a-potentiometer/349170

​

​

Motion Magic

Team 3244

Download and configure Page 3 of 3

• Other recommended Settings not included with sample
• /* Configure Current Limits */
• _talon.configPeakCurrentLimit(30); //Peak Current 25% motor Max
• _talon.configPeakCurrentDuration(150);
• _talon.configContinuousCurrentLimit(20); //Stall current 15% motor Max

// https://youtu.be/U4pgviiEwLg Citrus Circuits 1678, Presented by 973's Lead Mentor Adam Heard.

​

• /* Configure BreakMode */
• _talon.setNeutralMode(NeutralMode.Brake);

​

• /* Configure IntegralZone */
• _talon.config_IntegralZone(Constants.kSlotIdx, 30);

​

​

​

Motion Magic

Team 3244

Warning

The Example supplied by CTRE assumes no hard limits.

We need to adjust the Motion Magic Code if(button) statements to be sure we

Command the system to operate in the constraints of the robot.

Motion Magic

Team 3244

Warning

Add to the program a way to get the native Units min/max and other setpoints the subsystem can operate safely in.

SmartDashboard.putNumber(“Elevator Position”, _talon.getSelectedSensorPosition(0));

​

Or

​

Use the Plot Tool

Motion Magic

Team 3244

Warning

Change if(button)..

if (_joy.getRawButton(1)) {

/* Motion Magic */

double targetPos = minTravlePos;

_talon.set(ControlMode.MotionMagic, targetPos);

... //Keep the String builder

​

}else if (_joy.getRawButton(2)) { //Delete Button 2 condition and use for our second setpoint

/* Motion Magic */

double targetPos = maxTravlePos;

_talon.set(ControlMode.MotionMagic, targetPos);

... //Keep the String builder

}else{......... //command talon in percentOutput

​

Motion Magic

Team 3244

Test Motor Direction and Sensor Phase

• Download the program into the Robot
• Be cautious when enabling it maybe a good Idea to uncouple if possible the actuators from the motors and feedback
• Observations to make
• When joystick pressed up (Positive Command)
• Do the green LED’s blink?
• Does the system move in a logical positive direction?
• If not flip _talon.setInverted(boolean)**
• Using Phoenix Tuner is encoder increasing? With Positive command
• If not flip _talon.setSensorPhase(boolean)**
• ** setting inverted will not flip the sensorPhase. SetInverted only changed the logical direction if the sensor was correct it will remain in Phase after the output is set to Inverted.
• Make necessary changes and repete sensor bring up procedure to ensure sensor phase and general health

Motion Magic

*We can now record our Min/Max Travels

Team 3244

Feed Forward

This term is used to tell the system how to scale velocity native units

per 100ms to 100% Output

​

• While Plotting run the system at 100% within its limits and record the max velocity in native units per 100ms (Elevator Demo ~32)
• Enter the max observe value into the formula https://tinyurl.com/GCGH-MM-WS
• F-gain = (100% X 1023) / maxObserve (Elevator Demo = 31.96)

​

​

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#how-to-calculate-kf

​

Motion Magic

Team 3244

Set Cruise Velocity And Acceleration

• To calculate Cruise Velocity using a percentage of the maxObserve native units per 100ms
• 3/4 speed == 75% * maxObserve (Elevator Demo ~24)
• To Calculate Acceleration in velocity units per second (where velocity units = change in sensor per 100ms).
• For the Elevator Demo a one sec Acceleration would be [24]
• We want a .75 sec Acc so use [32]

​

*Start with modest values here and work your way up to full speed

** These value are integers so round to a whole number

​

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#setting-motion-magic-cruise-velocity-and-acceleration

Motion Magic

Team 3244

Warning

Set the

minTravlePos

maxTravlePos

​

Motion Magic

Team 3244

Set kp (proportional value)

• Add P-gain so that the closed-loop can react to error.
• Use the Phoenix Tuner Plot or DriverStation Council to capture the Error Running the system with just F-gain.
• (Elevator Demo ~22 units)
• A good place to start is to apply 10% more throttle with the observed error.
• (10% X 1023) / (observed error) = kp (Elevator Demo = 4.65)

​

• Download the new values and test the system. Manually adjust the kp value until the system responds adequately. (Elevator Demo = 6)

​

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#dialing-kp

Motion Magic

Team 3244

Set kd (derivative value)

• To correct for over shoot D-gain is added. Suggested is to start with 10 * kp

​

​

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#dialing-kd

Motion Magic

Team 3244

Set ki (integral value)

• Observe where the system settles out to after several moves and to different positions.
• Configure the IZone _talon.config_IntegralZone(Constants.kSlotIdx, 30); to something large enough to cover typical error

​

• Suggested is to set ki to something small like 0.001

​

​

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#dialing-ki

Motion Magic

Team 3244

Arbitrary FeedForward (gravity Compensation)

• Very Useful for Arms (almost required).
• On every program loop ~20ms you get the current degrees from horizontal and multiply the cos() of that value by the percent output required to hold the arm at horizontal.

// get the radians of the arm Horizontal = 0 Vertical = 90

double theta = Math.toRadians(CurrentArmAngle());

// get a range of 0 to 1 to multiply by feedforward.

// when in horizontal position, value should be 1

// when in vertical up or down position, value should be 0

double gravityCompensation = Math.cos(theta);

// horizontalHoldOutput is the minimum power required to hold the arm up when horizontal

// this is a range of 0-1

double arb_feedForward = gravityCompensation * horizontalHoldOutput;

​

_talon.set(ControlMode.MotionMagic, targetPos, DemandType.ArbitraryFeedForward, arb_feedForward);

​

Motion Magic

Team 3244

Other Considerations

• The TalonSRX controlMode will remain in the last state even after the robot is disabled. If the last mode was set to MotionMagic and the mechanism is moved when the robot is Re-enabled the TalonSRX will drive the output back to the last commanded position ignoring the Motion Magic Velocity and Acceleration setting.
• It is a good idea to command each subsystem that uses Motion Magic or other closed loop modes to set the drive into Percent Output using the _talon.set(controlMode.percentOutput,0.0);

Motion Magic

Team 3244

Questions