/** @details
* This thread is started when the driver control period is started
*/
msg_t
vexOperator( void *arg )
{
int16_t blink = 0;
(void)arg;
// Must call this
vexTaskRegister("operator");
// Run until asked to terminate
while(!chThdShouldTerminate())
{
// flash led/digi out
vexDigitalPinSet( kVexDigital_1, (blink++ >> 3) & 1);
// status on LCD of encoder and sonar
vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_1, "%4.2fV %8.1f", vexSpiGetMainBattery() / 1000.0, chTimeNow() / 1000.0 );
vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_2, "L %3d R %3d", vexMotorGet( MotorDriveL ), vexMotorGet( MotorDriveR ) );
// Tank drive
// left drive
vexMotorSet( MotorDriveL, vexControllerGet( Ch3 ) );
// right drive
vexMotorSet( MotorDriveR, vexControllerGet( Ch2 ) );
// Don't hog cpu
vexSleep( 25 );
}
return (msg_t)0;
}
/*-----------------------------------------------------------------------------*/
void
apolloUpdateSystem()
{
long systime = chTimeNow()/1000;
int hours, mins, secs;
vt100_cursor( T_X_ORIGIN+2, T_Y_ORIGIN+T_HEIGHT-2 );
hours = systime / 3600;
mins = (systime - (hours * 3600)) / 60;
secs = systime % 60;
vt100_printf("%02d:%02d:%02d ", hours, mins, secs );
vt100_printf("Main %4.2fV ", vexSpiGetMainBattery()/1000.0);
vt100_printf("Backup %4.2fV", vexSpiGetBackupBattery()/1000.0);
}
// 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;
}
