OK, part 2 then I need to do some real work today. This may be a little complex for some here but please don’t feel intimidated, it’s good to see a more fully developed example once in a while.
The previous post contained a very simple example demonstrating RobotC multi-tasking that would flash two LEDs at different speeds, not very exciting but actually useful in some cases. The task, flashLed_1, will keep executing no matter what other code you are running. Perhaps the robot is under driver control, LED_1 keeps flashing, perhaps following a line in autonomous mode, LED_1 keeps flashing. Why it’s flashing I have no idea, perhaps bad battery, that’s up to the programmer.
(one caveat here, when using the competition template, RobotC will stop all tasks started in Autonomous mode before entering driver control mode. You need to restart tasks you may want in both phases).
Now for a more complex example, hopefully not too complex but programmers of all abilities are presumably reading the forums.
One thing that I see that always worries me when students are doing motor control is that motors are being switched from full speed forwards to full speed backwards instantaneously. I’ve never seen any damage resulting from this, and I’m sure VEX has designed the motors to handle this stress, but it worries me all the same. To circumvent this situation we are going to use multi-tasking to implement a controlled acceleration and deceleration of the motors, I call this slew rate control, but it goes by other names as well.
To achieve this we will use an intermediate variable to hold the requested speed of the motor and a task to gradually change the current speed of the motor until it reaches the requested speed. As there can be up to 10 motors connected to the cortex microcontroller we need to track this for all 10 motors. We may not want the same acceleration and deceleration for all motors so we will also have a variable for each motor that determines how fast each one can change.
OK, a section by section breakdown of the code.
#pragma config(Motor, port2, DriveLeft_1, tmotorNormal, openLoop)
#pragma config(Motor, port3, DriveLeft_2, tmotorNormal, openLoop)
#pragma config(Motor, port4, DriveRight_1, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port5, DriveRight_2, tmotorNormal, openLoop, reversed)
Usual RobotC setup, we have 4 motors, 2 on each side for driving the robot.
/* */
/* definitions used by driver control */
/* */
/*-----------------------------------------------------------------------------*/
#define JOY_DRIVE_V vexJSLeftV
#define JOY_DRIVE_H vexJSLeftH
#define JOY_THRESHOLD 15
definitions used by the arcade driver control
/*-----------------------------------------------------------------------------*/
/* */
/* definitiona and variables for the motor slew rate controller. */
/* */
/*-----------------------------------------------------------------------------*/
#define MOTOR_NUM kNumbOfTotalMotors
#define MOTOR_MAX_VALUE 127
#define MOTOR_MIN_VALUE (-127)
#define MOTOR_DEFAULT_SLEW_RATE 10 // Default will cause 375mS from full fwd to rev
#define MOTOR_FAST_SLEW_RATE 256 // essentially off
#define MOTOR_TASK_DELAY 15 // task 1/frequency in mS (about 66Hz)
#define MOTOR_DEADBAND 10
// Array to hold requested speed for the motors
int motorReq MOTOR_NUM ];
// Array to hold "slew rate" for the motors, the maximum change every time the task
// runs checking current mootor speed.
int motorSlew MOTOR_NUM ];
A bunch of definitions used by the task controlling acceleration and deceleration. An array, motorReq, to hold the requested speed for each motor. An array, motorSlew, to hold the change in speed for each motor each time we check it.
/*-----------------------------------------------------------------------------*/
/* */
/* Task - compares the requested speed of each motor to the current speed */
/* and increments or decrements to reduce the difference as nexessary */
/* */
/*-----------------------------------------------------------------------------*/
task MotorSlewRateTask()
{
int motorIndex;
int motorTmp;
// Initialize stuff
for(motorIndex=0;motorIndex<MOTOR_NUM;motorIndex++)
{
motorReq[motorIndex] = 0;
motorSlew[motorIndex] = MOTOR_DEFAULT_SLEW_RATE;
}
// run task until stopped
while( true )
{
// run loop for every motor
for( motorIndex=0; motorIndex<MOTOR_NUM; motorIndex++)
{
// So we don't keep accessing the internal storage
motorTmp = motor motorIndex ];
// Do we need to change the motor value ?
if( motorTmp != motorReq[motorIndex] )
{
// increasing motor value
if( motorReq[motorIndex] > motorTmp )
{
motorTmp += motorSlew[motorIndex];
// limit
if( motorTmp > motorReq[motorIndex] )
motorTmp = motorReq[motorIndex];
}
// decreasing motor value
if( motorReq[motorIndex] < motorTmp )
{
motorTmp -= motorSlew[motorIndex];
// limit
if( motorTmp < motorReq[motorIndex] )
motorTmp = motorReq[motorIndex];
}
// finally set motor
motor[motorIndex] = motorTmp;
}
}
// Wait approx the speed of motor update over the spi bus
wait1Msec( MOTOR_TASK_DELAY );
}
}
This is the task that sends new speeds to each motor. The code before the while statement initializes the variables to a known state. Within the while loop there is a for loop that iterates through each motor. The current motor speed is compared to the requested speed and incremented or decremented as necessary. The motor speed is limited to the exact speed requested and then sent to the motor. The task having checked each motor then goes to sleep for a defined time (15mS in this case) before being woken up again by the RobotC task scheduler.
void
DriveLeftMotor( int value )
{
motorReq DriveLeft_1 ] = value;
motorReq DriveLeft_2 ] = value;
}
void
DriveRightMotor( int value )
{
motorReq DriveRight_1 ] = value;
motorReq DriveRight_2 ] = value;
}
Two intermediate subroutines to control left and right drive of the robot. Instead of send a new value to the motor directly the new speed is written into the variable holding the new requested speed.
task ArcadeDrive()
{
int ctl_v;
int ctl_h;
int drive_l;
int drive_r;
// Basic arcade control
while( true )
{
// Get joystick H and V control
ctl_v = vexRT JOY_DRIVE_V ];
ctl_h = vexRT JOY_DRIVE_H ];
// Ignore joystick near center
if( (abs(ctl_v) <= JOY_THRESHOLD) && (abs(ctl_h) <= JOY_THRESHOLD) )
{
drive_l = 0;
drive_r = 0;
}
else
{
// Create drive for left and right motors
drive_l = (ctl_v + ctl_h) / 2;
drive_r = (ctl_v - ctl_h) / 2;
}
// Now send out to motors
DriveLeftMotor( drive_l );
DriveRightMotor( drive_r );
// don't hog CPU
wait1Msec( 25 );
}
}
This task controls the robot’s drive, it’s fairly standard arcade drive control, nothing fancy.
task main()
{
// Start motor slew rate control
StartTask( MotorSlewRateTask );
// Start driver control tasks
StartTask( ArcadeDrive );
// Everything done in other tasks
while( true )
{
wait1Msec( 100 );
}
}
This is the main task and entry point for the program. The “MotorSlewRateTask” is started, the “ArcadeDrive” is started and then it enters a loop that does nothing. All control is handled by the other two tasks. Implementing main in this fashion allows the code to be moved into the competition template easily, to enter driver control simply start the two other tasks.
Now hitting the 10k limit so will post the source as an attachment. Any questions? fire away.
MultiTask.c (8.66 KB)