View Single Post
  #25  
Old 10-13-2011, 03:48 PM
jpearman's Avatar
jpearman jpearman is offline
Senior Member
VEX # 8888
 
Join Date: Apr 2011
Location: Los Angeles
Posts: 2,822
Re: RobotC programming tips

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.

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.

Code:
/*                                                                             */
/*  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

Code:
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  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.

Code:
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  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.

Code:
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.

Code:
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.

Code:
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.
Attached Files
File Type: c MultiTask.c (8.7 KB, 166 views)
Reply With Quote