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