There have been some questions about using multi tasking to help with autonomous code. Although we have had some examples on the forum before, I decided to post a new one based on some code that I have used in the past, although somewhat simplified for this post. It has been “superficially” tested with both ROBOTC and ConVEX but the principles are what matter not the language it is written in.
The aim of the code is to allow both “arm” and “drive” control to happen simultaneously with both subsystems being able to move to a given position. The final autonomous code looks like this.
void
AutonFun(void)
{
// Start the Arm PID task
StartTask( ArmPidController );
// Arm going up
SetArmPosition( 1000 );
// start driving
DriveForwardsEncoder( 1000, true );
// wait for arm in position
ArmSystemWaitInPosition( 3000 );
#ifdef CONVEX
vex_printf("done");
#else
writeDebugStreamLine("done");
#endif
}
The general idea here is that the arm is commanded to go to position 1000 (whatever that means is unimportant here, could be up or down), this call does not block meaning it returns immediately. Next, the drive is told to move forwards to position 1000 (and stop, we will come to that), this call blocks until the drive has achieved the goal, the arm may still moving so the code then calls ArmSystemWaitInPosition (the 3000 is a timeout value in mS, again we will explain that later) to wait until the arm has finishing moving.
So lets look at the arm system code first, here it is.
/*-----------------------------------------------------------------------------*/
/* Arm control */
/*-----------------------------------------------------------------------------*/
// Global to hold requested (target) arm position
static int armRequestedValue;
// Arm limits
#define ARM_UPPER_LIMIT 2000
#define ARM_LOWER_LIMIT 600
/*-----------------------------------------------------------------------------*/
/* Set requested arm position and clip to limits */
/*-----------------------------------------------------------------------------*/
static void
SetArmPosition( int position )
{
// crude limiting to upper and lower values
if( position > ARM_UPPER_LIMIT )
armRequestedValue = ARM_UPPER_LIMIT;
else
if( position < ARM_LOWER_LIMIT )
armRequestedValue = ARM_LOWER_LIMIT;
else
armRequestedValue = position;
}
/*-----------------------------------------------------------------------------*/
/* Get requested arm position */
/*-----------------------------------------------------------------------------*/
static int
GetArmPosition(void)
{
return( armRequestedValue );
}
/*-----------------------------------------------------------------------------*/
/* arm pid (actually just P) control task */
/*-----------------------------------------------------------------------------*/
#ifdef CONVEX
task ArmPidController(void *arg)
#else
task ArmPidController()
#endif
{
int armSensorCurrentValue;
int armError;
float armDrive;
static float pid_K = 0.3;
#ifdef CONVEX
(void)arg;
vexTaskRegister("Arm task");
#endif
while( true )
{
// Read the sensor value and scale
armSensorCurrentValue = GetArmSensor();
// calculate error
armError = armRequestedValue - armSensorCurrentValue;
// calculate drive
armDrive = (pid_K * (float)armError);
// limit drive
if( armDrive > 127 )
armDrive = 127;
else
if( armDrive < (-127) )
armDrive = (-127);
// send to motor
SetMotor( ARM_MOTOR, armDrive );
// Don't hog cpu
wait1Msec( 25 );
}
}
/*-----------------------------------------------------------------------------*/
/* Wait for arm to be in position */
/* returns 1 on success or 0 on timeout */
/*-----------------------------------------------------------------------------*/
short
ArmSystemWaitInPosition( short timeout )
{
int armError;
const int loopDelay = 50;
// default timeout 3 seconds
if(timeout <= 0)
timeout = 3000;
// convert mS to loops
timeout /= loopDelay;
while( timeout >= 0 )
{
// small delay
wait1Msec( loopDelay );
// calculate error
armError = GetArmPosition() - GetArmSensor();
// Check the pid error for the arm
if( (abs(armError) < 50 ) )
return(1);
// decrease timeout
timeout--;
}
return(0);
}
The code contains one task, a P controller for the arm. The arm has a potentiometer for feedback and is driven by one motor. The required arm position is set by calling SetArmPosition, the arm control task, ArmPidController, is constantly comparing the requested position with the current arm position and commanding the motor accordingly. We have covered this type of P (ie. simplified PID) controller before.
The function that wait for the arm to be in position is waiting for the error to drop below a suitable threshold, it’s set at 50 in this example, this will be robot dependent so may need tweaking. The function also has a timeout, that is, if the position is not reached within a given time the function will return anyway. If this happens more complex code would be required to determine what to do next, perhaps the arm is stuck, again it will be robot dependent.
There are some functions called from this code that are glue functions, specifically GetArmPosition and GetArmSensor. The content of these will vary depending on the language, for example, here is GetArmSensor. See that attached file for the rest.
static long
GetArmSensor(void)
{
#ifdef CONVEX
return( vexAdcGet( armPot ) );
#else
return( SensorValue armPot ] );
#endif
}
Next is the drive code, here it is.
/*-----------------------------------------------------------------------------*/
/* Arcade drive with two motors */
/*-----------------------------------------------------------------------------*/
void
DriveSystemDrive( int forward, int turn )
{
long drive_l_motor;
long drive_r_motor;
// Set drive
drive_l_motor = forward + turn;
drive_r_motor = forward - turn;
// normalize drive so max is 127 if any drive is over 127
int max = abs(drive_l_motor);
if (abs(drive_r_motor) > max)
max = abs(drive_r_motor);
if (max>127) {
drive_l_motor = 127 * drive_l_motor / max;
drive_r_motor = 127 * drive_r_motor / max;
}
// Send to motors
// left drive
SetMotor( DRIVE_LEFT_MOTOR, drive_l_motor );
// right drive
SetMotor( DRIVE_RIGHT_MOTOR, drive_r_motor );
}
/*-----------------------------------------------------------------------------*/
/* Drive forwards until encoder count has elapsed or timeout reached */
/* optionally stop motors at end */
/*-----------------------------------------------------------------------------*/
void
DriveForwardsEncoder( int distance, bool stopdrive )
{
long cur_position;
long target_position;
long currentTime;
cur_position = GetDriveEncoder();
target_position = cur_position + distance;
// Start driving - bias to left
if(distance > 0)
DriveSystemDrive( DRIVE_FORWARD_SPEED, 0 );
else
DriveSystemDrive( -DRIVE_FORWARD_SPEED, 0 );
// wait for end - 5 seconds maximum
for( currentTime = GetCurrentTime(); currentTime > (GetCurrentTime() - DEFAULT_DRIVE_TIMEOUT);)
{
cur_position = GetDriveEncoder();
if( distance >= 0)
{
if( cur_position >= target_position )
break;
}
else
{
if( cur_position <= target_position )
break;
}
wait1Msec(10);
}
// Stop driving
if( stopdrive )
DriveSystemDrive( 0, 0 );
}
Only two functions here, DriveSystemDrive takes two parameters (forward and turn) and implements arcade drive for a simple two motor robot. DriveForwardsEncoder starts the robot moving either forwards or backwards and then wait for an encoder attached on one wheel (an IME in this case) to change by the required amount. This function blocks, that’s intentional, but will wait for a maximum amount of time set by the constant DEFAULT_DRIVE_TIMEOUT (normally about 5 seconds). The function can optionally stop the motors when the required position is reached.
So there it is, the code is attached, for ROBOTC make sure the first line (#define CONVEX 1) is commented out then include the file in a competition template, it can then be used as follows from autonomous (or operator control).
task autonomous()
{
AutonFun();
while(1)
wait1Msec(10);
}
Same thing for CONVEX, something like this.
msg_t
vexAutonomous( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("auton");
AutonFun();
while(1)
{
// Don't hog cpu
vexSleep( 25 );
}
return (msg_t)0;
}
Obviously the sensors and motors must be setup first. I may do a version for PROS, all that needs is a few function call changes.
autoCode.c (7.23 KB)