How Can We Help?
< All Topics
Print

Steps to get Motion Profiling Working

Posted
Updated
ByFro
  1. Determine what UOM you will use (ft, in, m, mm), measure everything in that unit
  2. Ensure robot moves forward when given positive voltage
  3. Ensure encoders increase when robot moves forward
  4. Ensure gyro angle increases when turning right (CW) and negate output
    1. (Most MP code assumes CCW rotation is positive)
  5. Determine encoder PPR (pulses per revolution) and ensure it is reading correctly
    1. Roll robot forward 1 wheel rotation to be sure it is close enough
  6. Determine gear-ratio between the encoder and the shaft (may be 1/GR)
  7. Determine the wheel-base – wheel center to wheel center
  8. Determine effective wheel-size which will ensure correct distance/velocity calculations
    1. This value may change from robot to robot and even over a season
    2. Hand-roll robot forward a precise, well-measured distance and count encoder ticks
    3. Distance travelled = distance-per-pulse * pulses
      1. DPP = (EWS * PI) / (PPR * GR)
      2. Solving for EWS – Distance / pulses = EWS * PI / (PPR * GR)
      3. EWS = (PPR * GR * Distance) / (pulses * PI)
      4. If miscalculated, try inverting the gear-ratio
    4. Hand-roll robot different precise distances (5, 10, 15, 20 ft)
      1. Compute EWS at each distance
      2. Compute an average EWS
  9. Put the AEWS into the robot characterizer and characterize
    1. kV will be the FF value for the SmartController (SparkMax, TalonSRX)
    2. ArbitraryFF = kA * Accel + kS
      1. Set with the new velocity target if using kA
      2. If not using kA, just set ArbitraryFF = kS
    3. PID values – at least P & D set on SmartController PID
    4. Extrapolate theoretical maxVelocity from maxVel = (12 – kS) / kV 
      1. If needed, get theoretical maxAccel from maxAcc = (12 – kS) / kA
    5. Use max velocity and acceleration when creating trajectories in PathWeaver
      1. Probably can cut it back to 80% or even ½
      2. Check out PathWeaver guidance on this 
  10. Write 3 functions in the DriveTrain class
    1. A function which calculates the current Pose2d using the Odometry classes
    2. A function that takes 2 wheel velocities and sets the velocity control of the SC
      1. Use the guidance from previous point, especially WRT kA & ArbitraryFF
    3. A function which returns the current Pose2d
    4. Use these functions to create a RamseteCommand that takes the last 2 functions
    5. Make Auto use this with the requested Trajectory, or put it on a button
  11. Test simple paths, then harder paths
    1. Multiple runs (3-5), look for consistency before worrying about correct targeting
    2. Determine how much deviation there is between end locations
      1. Mark end location of a particular corner of the robot
      2. What is the diameter of a circle around those points?
    3. Adjust P values up in small increments, see if consistency or targeting improves
    4. Run longer and more difficult paths
  12. Write a Callable that can generate Trajectories using a Future
    1. Use to generate selected trajectory at startup rather than waiting for all
    2. Write a FunctionalCommand that executes the runnable on init and finishes when the future is done, storing the trajectory in a static location
    3. Use .andThen() to run the RamseteCommand
    4. And – use it to generate vision-target trajectories!
Table of Contents