// Autonomous control task
msg_t
vexAutonomous( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("auton");
// start the Arm PID task
StartTask( ArmPidController );
// start the claw motor controller
StartTask( ClawController );
// Arm Up
SetArmPosition( 2000 );
// Claw Open
ClawOpen();
wait1Msec(2000);
// Move forward
DriveSystemArcadeDrive( 100, 0 );
wait1Msec(1000);
// Stop drivw
DriveSystemArcadeDrive( 0, 0 );
// Arm down
SetArmPosition( 700 );
wait1Msec(2000);
// close claw
ClawClose();
wait1Msec(1000);
// Arm up
SetArmPosition( 1000 );
wait1Msec(1000);
// Turn
DriveSystemArcadeDrive( 0, 100 );
wait1Msec(1000);
// Stop drivw
DriveSystemArcadeDrive( 0, 0 );
// Claw Open
ClawOpen();
wait1Msec(1000);
// Stop claw motor
ClawStop();
while(1)
{
// Don't hog cpu
vexSleep( 25 );
}
return (msg_t)0;
}
task
DriveTask(void *arg)
{
short forward, turn;
(void)arg;
vexTaskRegister("Drive task");
while( TRUE )
{
// Get controller
if( abs( vexControllerGet( Ch3 ) ) > 10 )
forward = vexControllerGet( Ch3 );
else
forward = 0;
if( abs( vexControllerGet( Ch4 ) ) > 10 )
turn = vexControllerGet( Ch4 );
else
turn = 0;
DriveSystemArcadeDrive( forward, turn );
wait1Msec(25);
}
}
// Driver control task
msg_t
vexOperator( void *arg )
{
int16_t forward;
int16_t turn;
int16_t nexttune = 0;
(void)arg;
// Must call this
vexTaskRegister("operator");
RtttlDemo(nexttune);
// Run until asked to terminate
while(!chThdShouldTerminate())
{
if( vexControllerGet( Btn7 ) )
{
if( vexControllerGet( Btn7R ) )
{
if(++nexttune == NUM_TUNES)
nexttune = 0;
}
if( vexControllerGet( Btn7L ) )
{
if(--nexttune < 0)
nexttune = NUM_TUNES-1;
}
RtttlDemo(nexttune);
// lazy
while(vexControllerGet( Btn7 ))
vexSleep(25);
}
// status on LCD of encoder and sonar
vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_1, "Batt %4.2fV", vexSpiGetMainBattery() / 1000.0 );
// Get controller
if( abs( vexControllerGet( Ch2 )) > 10 )
forward = vexControllerGet( Ch2 );
else
forward = 0;
if( abs( vexControllerGet( Ch1 )) > 10 )
turn = vexControllerGet( Ch1 );
else
turn = 0;
if( abs( vexControllerGet( Ch3 )) > 10 )
SetMotor( 1, vexControllerGet( Ch3 ) );
else
SetMotor( 1, 0 );
// arcade drive
DriveSystemArcadeDrive( forward, turn );
// Don't hog cpu
vexSleep( 25 );
}
return (msg_t)0;
}