/** @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;
}
/** @details
* This thread is started when the driver control period is started
*/
msg_t
vexOperator( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("operator");
vexLcdClearLine(VEX_LCD_DISPLAY_1, VEX_LCD_LINE_T);
vexLcdClearLine(VEX_LCD_DISPLAY_1, VEX_LCD_LINE_B);
// Run until asked to terminate
while(!chThdShouldTerminate())
{
// show some user parameters
vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_T, "%02X %02X %02X %02X", userp->data[0],userp->data[1],userp->data[2],userp->data[3]);
// buttons 8 change user parameters
if( vexControllerGet( Btn8U ) == 1 )
{
userp->data[0]++;
vexControllerReleaseWait(Btn8U);
}
if( vexControllerGet( Btn8R ) == 1 )
{
userp->data[1]++;
vexControllerReleaseWait(Btn8R);
}
if( vexControllerGet( Btn8D) == 1 )
{
userp->data[2]++;
vexControllerReleaseWait(Btn8D);
}
if( vexControllerGet( Btn8L) == 1 )
{
userp->data[3]++;
vexControllerReleaseWait(Btn8L);
}
// Button 7 Up saves
if( vexControllerGet( Btn7U ) == 1 )
{
vexFlashUserParamWrite( userp );
vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_B, "Saved...");
vexControllerReleaseWait(Btn7U);
}
// Don't hog cpu
vexSleep( 25 );
}
return (msg_t)0;
}
void autonDrive( int speed, int inches ) {
float inchesL=0;
float inchesR=0;
int timeout=0;
resetImeLeft();
resetImeRight();
// Start to motors
vexMotorSet( MotorDriveL1, speed-2 );
vexMotorSet( MotorDriveL2, speed-2 );
vexMotorSet( MotorDriveR1, speed );
vexMotorSet( MotorDriveR2, speed );
while( TRUE ) {
vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );
vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );
if (inchesL == getInchesLeft() && inchesR == getInchesRight() ) {
timeout++;
if( timeout > 30 ) {
break;
}
}
inchesL = getInchesLeft();
inchesR = getInchesRight();
if( abs(inchesL) > inches || abs(inchesR) > inches ) {
break;
}
vexSleep(5);
}
// Reverse motors to stop coast
vexMotorSet( MotorDriveL1, -speed/2 );
vexMotorSet( MotorDriveL2, -speed/2 );
vexMotorSet( MotorDriveR1, -speed/2 );
vexMotorSet( MotorDriveR2, -speed/2 );
vexSleep( abs(speed/8) );
// Stop motors
vexMotorSet( MotorDriveL1, 0 );
vexMotorSet( MotorDriveL2, 0 );
vexMotorSet( MotorDriveR1, 0 );
vexMotorSet( MotorDriveR2, 0 );
}
void
RtttlDemo( int tune )
{
char *n;
if( tune < NUM_TUNES )
{
n = vexAudioPlayRtttl( MyTunes[tune], 128, 1 );
vexLcdPrintf( 1, 1, "%s", n );
}
}
// 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;
}
