Here’s an example use case for this tool: let’s say you wanted to set a profile for a four motor turbo drive (ports 2 and 9 on left, ports 3 and 8 on right) using encoders (dgtl1/2 for left, dgtl3/4 for right). Here’s the initial setup for that configuration:
createMotionProfile(port2);
createMotionProfile(port3);
profileSetSensor (port2, dgtl1);
profileSetSensor (port3, dgtl3);
//set max velocity in ticks (sensor units) per second
profileSetMaxVelocity (port2, 1728); //120rpm * 360 ticks per rev / 60 seconds = 1728 ticks per second
profileSetMaxVelocity (port3, 1728);
//sets motors to follow other motors/profiles. Reversed flag swaps direction of this motor
//<port>, <master to follow>, <reversed>
profileSetMaster (port9, port2, false);
profileSetMaster (port8, port3, false);
Ports 8 and 9 would be set to follow ports 3 and 2 respectively.
createMotionProfile will also autodetect encoders defined in the ROBOTC motors and sensors setup, but not the type/velocity. So you can skip profileSetSensor if you want for encoders configured this way, but you still would need to set the maxVelocity parameter.
To tune the control gains and features, there are a handful of functions:
//would cap the speed during a move to 1200 ticks per second, defaults to maxVelocity above
//moves are computed with using this speed as the max, and will take longer if this value is lowered
//calling profileSetMaxVelocity resets this speed limit
profileSetSpeedLimit (port2, 1200);
//a gain that makes the motor try and follow the acceleration cuve more aggresively
//higher values mean that more power will be added when speeding up/slowing down to help "stick"
//to the speed curve
profileSetAccelerationGain (port2, 0.15);
//time in milliseconds to ramp up to max speed for a move
//think of this like a "slew rate", or a 0-max speed time
//defaults to 1000, or 1 second from 0 to max speed
profileSetAccelerationTime (port2, 1000);
//sets the rate (in samples per second, Hz) in which the controllers try and sample and set motor output
//defaults to 50Hz
profileSetSampleRate (port2, 50);
//the position PID controller needs to run slower than the velocity PID controller
//this function sets the number of velocity cycles that run for every position update
//defaults to 4, should be 3 or higher usually.
profileSetPositionSampleTime (port2, 4);
//sets the amount of acceleration "smoothing" that is done. A value of 0 makes the speed curve
//a sharp trapezoid, a value of 1 makes it a very smooth "S" shape
//defaults to 0.5, can range from 0.0 to 1.0
profileSetJerkRatio (port2, 0.5);
//basically calls SensorValue [sensorPort] = 0;
//may have strange results depending on the type of sensor being used
//when in doubt, just reset the sensor manually if necessary
profileResetPosition (port2);
//sets position PID gains and integral cutoffs
//the integral cutoffs define a range in which the position error is summed and used for the I component.
//loops with an error value outside the defined range leave the errorSum at the previous value
//<motorPort>, <kP>, <kI>, <kD>, <innerICutoff>, <outerICutoff>
//these are the default position PID settings
profileSetPositionController (port2, 3.0, 0.0, 0.0, 30, 150);
//sets velocity PID gains and cutoffs
//these are the default velocity PID settings, except for kP.
//kP is set to (127.0 / maxVelocity) whenever profileSetMaxVelocity is called, but can be overridden here.
profileSetVelocityController (port2, 0.0, 0.0, 0.0, 50, 500);
You can plot the output curves in ROBOTC’s datalogger like my above post by calling the function below. You can only log one motor at a time due to there only being so many datalog series.
while (true) {
profileLog(port2); //port2, or whatever other motor you want
delay (100); //can be any delay you want
}
There’s an advanced feature that allows users to use a pointer to a variable (int or float) in place of a sensor port, in case you’re doing some sensor processing. Once you set this, you can update the value of the variable you used and the controller will detect the changes automatically.
float processedSensorValue = 0.0;
profileSetSensorPtr (port2, &processedSensorValue);
To use the motion profiler to execute moves or control the speed of motors, you call the following functions:
//calculates and executes a move from the current position to the desired position
//two consecutive calls to profileGoTo with the same position value do not stack, it's absolute positioning
//reset the sensor value to 0 in between moves to get relative position control
//<motorPort>, <position>
profileGoTo (port2, 3000);
//sets the target velocity of a motor.
//The motor will try to run at that velocity until another command is executed
profileSetVelocity (port2, 1500);
//sets the raw output of the motor. Equivalent to motor[port2] = value;
//You need to use this function to set the motor value of a profiled motor, else it will be overridden
//This value is linearized using a TrueSpeed lookup table (63 is ~50% speed, 32 is ~25% etc...)
profileSetMotorOutput (port2, 127);
The goTo and setVelocity functions will immediately use the profiler to plan motion curves and then use the built-in PID cascade control to get to the desired position/velocities.
If using the red quadrature encoders (the external ones), be aware that ROBOTC only uses a 16 bit signed integer to store the value of the encoder, so it will overflow after 32,767 ticks. If you want to use a red encoder at higher resolution (by using a gear reduction), you can assign an encoder to a motor in the motors and sensors setup, and then assign the value of getMotorEncoder(nMotor) to a variable and use profileSetSensorPtr() with that value in order to get a 32 bit value (getMotorEncoder() goes all the way to 2,147,483,647). Pro tip: the integrated encoder functions work on red encoders if they’re configured in motors/sensors setup (getMotorEncoder, resetMotorEncoder, etc…).
#pragma config(Sensor, dgtl1, , sensorQuadEncoder)
#pragma config(Sensor, dgtl3, , sensorQuadEncoder)
#pragma config(Motor, port2, , tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl1)
#pragma config(Motor, port3, , tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl3)
#pragma config(Motor, port8, , tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port9, , tmotorVex393TurboSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
int encoderLeftValue = 0;
int encoderRightValue = 0;
task updateEncoderVariables () {
while (1) {
encoderLeftValue = getMotorEncoder (port2);
encoderRightValue = getMotorEncoder (port3);
abortTimeSlice ();
}
}
task main () {
startTask (updateEncoderVariables);
createMotionProfile (port2);
createMotionProfile (port3);
profileSetSensorPtr (port2, &encoderLeftValue);
profileSetSensorPtr (port3, &encoderRightValue);
}
