// 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; }