// Driver control task
msg_t
vexOperator( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("operator");
// Start manual driving
StartTask( DriveTask );
// start the Arm PID task
StartTask( ArmPidController );
// start the claw motor controller
StartTask( ClawController );
// start manual arm/claw control
StartTask( ManualArmClawTask );
// Run until asked to terminate
while(!chThdShouldTerminate())
{
// 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 )
{
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 ClawController(void *arg)
{
int hold_delay = 0;
(void)arg;
vexTaskRegister("claw control");
while( TRUE )
{
switch( clawCmd )
{
case kClawOpenCommand:
// look for fully open (around 500 counts)
if( vexMotorPositionGet( MotorClaw ) > 400 )
SetMotor( MotorClaw , CLAW_OPEN_LOW_SPEED);
else
SetMotor( MotorClaw , CLAW_OPEN_HIGH_SPEED);
break;
case kClawCloseCommand:
// look for fully closed (enc == 0)
if( vexMotorPositionGet( MotorClaw ) < 100 )
SetMotor( MotorClaw , CLAW_CLOSE_LOW_SPEED);
else
{
// This test checks for slow motor movement when partially closed
// half second delay before check starts
if(hold_delay++ > 20)
{
if( SmartMotorGetSpeed( MotorClaw ) > -40 )
SetMotor( MotorClaw , CLAW_CLOSE_LOW_SPEED);
}
else
SetMotor( MotorClaw , CLAW_CLOSE_HIGH_SPEED );
}
// learn minimum encoder position
if( vexMotorPositionGet( MotorClaw ) < 0 )
vexMotorPositionSet( MotorClaw, 0);
break;
case kClawStopCommand:
SetMotor( MotorClaw , 0 );
clawCmd = kClawNoCommand;
break;
default:
break;
}
// reset hold delay
if(clawCmd != kClawCloseCommand)
hold_delay = 0;
// Don't hog cpu
wait1Msec(25);
}
}
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);
}
}
task ArmPidController(void *arg)
{
int armSensorCurrentValue;
int armError;
float armDrive;
static float pid_K = 0.3;
(void)arg;
vexTaskRegister("arm pid");
while( TRUE )
{
// Read the sensor value and scale
armSensorCurrentValue = vexAdcGet( armPot );
// calculate error
armError = armRequestedValue - armSensorCurrentValue;
// calculate drive
armDrive = (pid_K * (float)armError);
// limit drive
if( armDrive > 127 )
armDrive = 127;
else
if( armDrive < (-127) )
armDrive = (-127);
// send to motor
SetMotor( MotorArm, armDrive);
// Don't hog cpu
wait1Msec( 25 );
}
}
// 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;
}
/** @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;
}
// Driver control task
msg_t
vexOperator( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("operator");
// call default code - blocks until termination
vexCortexDefaultDriver();
return (msg_t)0;
}
task c_vexAutonomous(void *arg) {
(void)arg;
vexTaskRegister("auton");
StartTask(armTask);
autonomous();
while (!chThdShouldTerminate()) {
vexSleep(25);
}
return (task)0;
}
// real time display of motor and sensor data (UNUSED; done in main shell task)
task apolloTask(void *arg) {
(void)arg;
vexTaskRegister("apollo");
apolloInit();
while (!chThdShouldTerminate()) {
apolloUpdate();
}
apolloDeinit();
return (task)0;
}
task c_vexOperator(void *arg) {
(void)arg;
vexTaskRegister("operator");
StartTask(driveTask); // drive
StartTask(armTask); // arm
StartTask(intakeTask); // intake
StartTask(pneumaticsTask); // pneumatics
while (!chThdShouldTerminate()) { // sleep forever
vexSleep(25);
}
return (task)0;
}
// Autonomous control task
msg_t
vexAutonomous( void *arg )
{
(void)arg;
// Must call this
vexTaskRegister("auton");
while(1)
{
// 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;
}
/*-----------------------------------------------------------------------------*/
static msg_t
vexRobotcTask(void *arg)
{
// All robotc tasks enter here and then call the user provided function
if(arg != NULL)
{
// register the task
tfunc_t func = arg;
vexTaskRegister( "robotc_task" );
// call user supplied function, this may not return
return func(NULL);
}
else
return (msg_t)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 )
{
(void)arg;
int delay, i;
// Must call this
vexTaskRegister("operator");
while(1)
{
// run a pattern with speed 50
for(delay=100;delay>10;delay-=10)
RunPattern( &pattern_0[0], sizeof(pattern_0)/sizeof(short), delay );
for(i=0;i<20;i++)
RunPattern( &pattern_0[0], sizeof(pattern_0)/sizeof(short), 10 );
// not really needed but will leave here
vexSleep(10);
}
return (msg_t)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;
}
