/** @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;
}
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);
}
}
/** @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;
}
task safetyTask(void *arg) {
(void)arg;
vexTaskRegister("safety");
while (!chThdShouldTerminate()) {
// detect if controller is face down.
if (vexControllerGet(AcclY) < 0) { // figure this out
vexMotorStopAll(); // kill all motors (for now)
//vexTaskEmergencyStop(); // emergency stop, reboot required
}
vexSleep(10);
}
return (task)0;
}
task
ManualArmClawTask(void *arg)
{
(void)arg;
vexTaskRegister("manual arm");
// initialize
SetArmPosition( vexAdcGet( armPot ) );
// use joystick to modify the requested position
while( TRUE )
{
// presets - assume joystick is centered
if( vexControllerGet( Btn8U ) == 1 )
SetArmPosition( ARM_PRESET_H );
else
if( vexControllerGet( Btn8D ) == 1 )
SetArmPosition( ARM_PRESET_L );
// manual control
if( abs(vexControllerGet( Ch2 )) > 15 )
SetArmPosition( GetArmPosition() + (vexControllerGet( Ch2 )) );
// Claw control
if( vexControllerGet( Btn6D ) == 1 )
ClawOpen();
else
if( vexControllerGet( Btn6U ) == 1 )
ClawClose();
else
ClawStop();
// don't hog cpu
wait1Msec(50);
}
}
// 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;
}
/*-----------------------------------------------------------------------------*/
void
apolloUpdate()
{
int i;
// all motor values
for(i=0;i<10;i++)
apolloUpdateMotor( i );
// all digital values
for(i=0;i<12;i++)
apolloUpdateDigital( i );
// all analog values
for(i=0;i<8;i++)
apolloUpdateAnalog( i );
// update current and temp for cortex
apolloUpdateCortex(0);
apolloUpdateCortex(1);
apolloUpdateSystem();
// display the joystick data
apolloUpdateJoystickButton( 0, 0, vexControllerGet(Btn5U), vexControllerGet(Btn5D), -1, -1 );
apolloUpdateJoystickButton( 0, 1, vexControllerGet(Btn6U), vexControllerGet(Btn6D), -1, -1 );
apolloUpdateJoystickButton( 0, 2, vexControllerGet(Btn7U), vexControllerGet(Btn7D), vexControllerGet(Btn7L), vexControllerGet(Btn7R) );
apolloUpdateJoystickButton( 0, 3, vexControllerGet(Btn8U), vexControllerGet(Btn8D), vexControllerGet(Btn8L), vexControllerGet(Btn8R) );
apolloUpdateJoystickAnalog( 0, 0, vexControllerGet(Ch4), vexControllerGet(Ch3) );
apolloUpdateJoystickAnalog( 0, 1, vexControllerGet(Ch1), vexControllerGet(Ch2) );
apolloUpdateJoystickAnalog( 0, 3, vexControllerGet(AcclX), vexControllerGet(AcclY) );
if( (vexControllerCompetitonState() & kFlagXmit2 ) == kFlagXmit2 )
{
// display the joystick data
apolloUpdateJoystickButton( 1, 0, vexControllerGet(Btn5UXmtr2), vexControllerGet(Btn5DXmtr2), -1, -1 );
apolloUpdateJoystickButton( 1, 1, vexControllerGet(Btn6UXmtr2), vexControllerGet(Btn6DXmtr2), -1, -1 );
apolloUpdateJoystickButton( 1, 2, vexControllerGet(Btn7UXmtr2), vexControllerGet(Btn7DXmtr2), vexControllerGet(Btn7LXmtr2), vexControllerGet(Btn7RXmtr2) );
apolloUpdateJoystickButton( 1, 3, vexControllerGet(Btn8UXmtr2), vexControllerGet(Btn8DXmtr2), vexControllerGet(Btn8LXmtr2), vexControllerGet(Btn8RXmtr2) );
apolloUpdateJoystickAnalog( 1, 0, vexControllerGet(Ch4Xmtr2), vexControllerGet(Ch3Xmtr2) );
apolloUpdateJoystickAnalog( 1, 1, vexControllerGet(Ch1Xmtr2), vexControllerGet(Ch2Xmtr2) );
apolloUpdateJoystickAnalog( 1, 3, vexControllerGet(AcclXXmtr2), vexControllerGet(AcclYXmtr2) );
}
// move cursor out of the way
vt100_cursor(T_WIDTH, T_HEIGHT);
}
/** @details
* This function simulates the cortex default code, it was based on the ROBOTC
* implementation but with some changes in structure.
*/
void
vexCortexDefaultDriver(void)
{
int16_t mech1_lim_ccw = 0;
int16_t mech1_lim_cw = 0;
int16_t mech2_lim_ccw = 0;
int16_t mech2_lim_cw = 0;
int16_t mech3_lim_ccw = 0;
int16_t mech3_lim_cw = 0;
int16_t mech4_lim_ccw = 0;
int16_t mech4_lim_cw = 0;
int16_t drive_left = 0;
int16_t drive_right = 0;
int16_t drive_mech1 = 0;
int16_t drive_mech2 = 0;
int16_t drive_mech3 = 0;
int16_t drive_mech4 = 0;
while( !chThdShouldTerminate() )
{
// Set motor direction - a jumper in ports 1 through 10 reversed motor
vexMotorDirectionSet( LeftDrive1, vexDigitalPinGet( kVexDigital_1 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( LeftDrive2, vexDigitalPinGet( kVexDigital_2 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( LeftDrive3, vexDigitalPinGet( kVexDigital_3 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( RightDrive4, vexDigitalPinGet( kVexDigital_4 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( RightDrive5, vexDigitalPinGet( kVexDigital_5 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( Mech1, vexDigitalPinGet( kVexDigital_6 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( Mech2, vexDigitalPinGet( kVexDigital_7 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( Mech3, vexDigitalPinGet( kVexDigital_8 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( Mech4, vexDigitalPinGet( kVexDigital_9 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
vexMotorDirectionSet( RightDrive10, vexDigitalPinGet( kVexDigital_10 ) == JUMPER_IN ? kVexMotorReversed : kVexMotorNormal );
// Sample analog inputs - these are used as limit switches
mech1_lim_ccw = vexAdcGet( kVexAnalog_1 ) < 200 ? 0 : 1;
mech1_lim_cw = vexAdcGet( kVexAnalog_2 ) < 200 ? 0 : 1;
mech2_lim_ccw = vexAdcGet( kVexAnalog_3 ) < 200 ? 0 : 1;
mech2_lim_cw = vexAdcGet( kVexAnalog_4 ) < 200 ? 0 : 1;
mech3_lim_ccw = vexAdcGet( kVexAnalog_5 ) < 200 ? 0 : 1;
mech3_lim_cw = vexAdcGet( kVexAnalog_6 ) < 200 ? 0 : 1;
mech4_lim_ccw = vexAdcGet( kVexAnalog_7 ) < 200 ? 0 : 1;
mech4_lim_cw = vexAdcGet( kVexAnalog_8 ) < 200 ? 0 : 1;
// SINGLE DRIVER - TANK
if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_OUT && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_OUT )
{
drive_left = vexControllerGet( Ch3 ); // up = CW
drive_right = -vexControllerGet( Ch2 ); // up = CCW
drive_mech1 = (vexControllerGet( Btn5U ) * 127) - (vexControllerGet( Btn5D ) * 127); // U = CW, D = CCW
drive_mech2 = (vexControllerGet( Btn6U ) * 127) - (vexControllerGet( Btn6D ) * 127); // U = CW, D = CCW
drive_mech3 = (vexControllerGet( Btn7U ) * 127) - (vexControllerGet( Btn7D ) * 127); // U = CW, D = CCW
drive_mech4 = (vexControllerGet( Btn8U ) * 127) - (vexControllerGet( Btn8D ) * 127); // U = CW, D = CCW
}
else
// DUAL DRIVER - TANK
if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_IN && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_OUT )
{
drive_left = vexControllerGet( Ch3 ); // up = CW
drive_right = -vexControllerGet( Ch2 ); // up = CCW
drive_mech1 = (vexControllerGet( Btn5UXmtr2 ) * 127) - (vexControllerGet( Btn5DXmtr2 ) * 127); // U = CW, D = CCW
drive_mech2 = (vexControllerGet( Btn6UXmtr2 ) * 127) - (vexControllerGet( Btn6DXmtr2 ) * 127); // U = CW, D = CCW
drive_mech3 = vexControllerGet( Ch3Xmtr2 ); // up = CW
drive_mech4 = vexControllerGet( Ch2Xmtr2 ); // up = CW
}
else
// SINGLE DRIVER - ARCADE
if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_OUT && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_IN )
{
drive_left = vexControllerGet( Ch1 ) + vexControllerGet( Ch2 ); // up = CW, right = CW
drive_right = vexControllerGet( Ch1 ) - vexControllerGet( Ch2 ); // up = CCW, right = CW
drive_mech1 = (vexControllerGet( Btn5U ) * 127) - (vexControllerGet( Btn5D ) * 127); // U = CW, D = CCW
drive_mech2 = (vexControllerGet( Btn6U ) * 127) - (vexControllerGet( Btn6D ) * 127); // U = CW, D = CCW
drive_mech3 = vexControllerGet( Ch3 ); // up = CW
drive_mech4 = -vexControllerGet( Ch4 ); // right = CCW
}
else
// DUAL DRIVER - ARCADE
if( vexDigitalPinGet( kVexDigital_11 ) == JUMPER_IN && vexDigitalPinGet( kVexDigital_12 ) == JUMPER_IN )
{
drive_left = vexControllerGet( Ch1 ) + vexControllerGet( Ch2 ); // up = CW, right = CW
drive_right = vexControllerGet( Ch1 ) - vexControllerGet( Ch2 ); // up = CCW, right = CW
drive_mech1 = (vexControllerGet( Btn5UXmtr2 ) * 127) - (vexControllerGet( Btn5DXmtr2 ) * 127); // U = CW, D = CCW
drive_mech2 = (vexControllerGet( Btn6UXmtr2 ) * 127) - (vexControllerGet( Btn6DXmtr2 ) * 127); // U = CW, D = CCW
drive_mech3 = vexControllerGet( Ch3Xmtr2 ); // up = CW
drive_mech4 = vexControllerGet( Ch2Xmtr2 ); // up = CW
}
// Left drive motors
vexMotorSet( LeftDrive1, drive_left );
vexMotorSet( LeftDrive2, drive_left );
vexMotorSet( LeftDrive3, drive_left );
// Right drive motors
vexMotorSet( RightDrive4, drive_right );
vexMotorSet( RightDrive5, drive_right );
vexMotorSet( RightDrive10, drive_right );
// Limit switches
if( ( !mech1_lim_cw && (drive_mech1 > 0)) || ( !mech1_lim_ccw && (drive_mech1 < 0)) )
drive_mech1 = 0;
if( ( !mech2_lim_cw && (drive_mech2 > 0)) || ( !mech2_lim_ccw && (drive_mech2 < 0)) )
drive_mech2 = 0;
if( ( !mech3_lim_cw && (drive_mech3 > 0)) || ( !mech3_lim_ccw && (drive_mech3 < 0)) )
drive_mech3 = 0;
if( ( !mech4_lim_cw && (drive_mech4 > 0)) || ( !mech4_lim_ccw && (drive_mech4 < 0)) )
drive_mech4 = 0;
// Mechanism motors
vexMotorSet( Mech1, drive_mech1 );
vexMotorSet( Mech2, drive_mech2 );
vexMotorSet( Mech3, drive_mech3 );
vexMotorSet( Mech4, drive_mech4 );
// small delay
vexSleep(10);
}
}
