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 ); } }
/*-----------------------------------------------------------------------------*/ void apolloUpdateAnalog(int pin) { vt100_cursor( A_X_POS+12, A_Y_POS+2+pin ); vt100_printf("%4d", vexAdcGet(pin)); }
void vexAdcDebug(vexStream *chp, int argc, char *argv[]) { int i; (void)argc; (void)argv; for(i=0;i<8;i++) chprintf( chp, "channel %d = %4d\r\n", i, vexAdcGet(i) ); }
int32_t vexSensorValueGet( tVexSensors sensor ) { if( sensor <= kVexSensorAnalog_8 ) return( vexAdcGet( (int16_t)sensor ) ); else if( sensor <= kVexSensorDigital_12 ) { int16_t chan; tVexDigitalPin pin = (tVexDigitalPin)( sensor - kVexSensorDigital_1 ); tVexSensorType type = vexDigitalTypeGet( pin ); switch( type ) { case kVexSensorNone: break; case kVexSensorDigitalInput: return( vexDigitalPinGet(pin ) ); break; case kVexSensorDigitalOutput: return( vexDigitalPinGet(pin ) ); break; case kVexSensorInterrupt: return( vexDigitalPinGet(pin ) ); break; case kVexSensorQuadEncoder: chan = vexDigitalChannelGet( pin ); return( vexEncoderGet( chan ) ); break; case kVexSensorSonarCm: chan = vexDigitalChannelGet( pin ); return( vexSonarGetCm( chan ) ); break; case kVexSensorSonarInch: chan = vexDigitalChannelGet( pin ); return( vexSonarGetInch( chan ) ); break; default: break; } } else if( sensor <= kVexSensorIme_8 ) { int16_t chan = sensor - kVexSensorIme_1; return( vexImeGetCount( chan) ); } return(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); } }
/** @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); } }