multitask example code

VEX V5 General Discussion Technical Discussion
1

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)

1 Like
2

I did a quick port of the code below for PROS (only partially tested, too lazy to hook up an IME tonight). The code is almost identical, only a few changes needed. The autonomous function looks like this.

#include "main.h"

void AutonFun(void);

void
statusTask() {
    lcdPrint( uart1, 2, "%d %d", motorGet(1), motorGet(10) );
}

void autonomous() {

    taskRunLoop( statusTask, 50 ); // run every 50mS

    // run autonomous code
    AutonFun();

    while(1) {
        taskDelay(50);
    }
}

The AutonFun code is attached.

The correct way to add this to a PROS project is to add a new file to the “src” directory (right click and select New->File). Do not use the ROBOTC way, ie. including a file with C source code using #include.
autoDemo.c (6.57 KB)

1 Like