/** @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;
}
int
P3UserDecodeControl( p3comms *MyComms, p3pak *packet )
{
p3cmdfull *cmd = &packet->command.cmdpak.cmd;
int ret = 1;
int index;
switch( cmd->cmd2 )
{
// Set all motors
case CMD2_CONTROL_SETMOTORS:
// Check for valid data length
if( cmd->length == 10 ) {
// data is in range 0-254, shift to +/- 127
vexMotorSet( kVexMotor_1, cmd->data[0] - 0x7F);
vexMotorSet( kVexMotor_2, cmd->data[1] - 0x7F);
vexMotorSet( kVexMotor_3, cmd->data[2] - 0x7F);
vexMotorSet( kVexMotor_4, cmd->data[3] - 0x7F);
vexMotorSet( kVexMotor_5, cmd->data[4] - 0x7F);
vexMotorSet( kVexMotor_6, cmd->data[5] - 0x7F);
vexMotorSet( kVexMotor_7, cmd->data[6] - 0x7F);
vexMotorSet( kVexMotor_8, cmd->data[7] - 0x7F);
vexMotorSet( kVexMotor_9, cmd->data[8] - 0x7F);
vexMotorSet( kVexMotor_10,cmd->data[9] - 0x7F);
// Send ACK
P3Command(MyComms, &Cmd_Ack, packet->dev_id );
}
else
// Send NAK
P3Command(MyComms, &Cmd_Nak_Para_Err, packet->dev_id );
break;
// Set motor by index
case CMD2_CONTROL_SET_MOTOR_BY_INDEX:
// Check for valid data length
if( cmd->length == 2) {
// first data byte is motor index in range 0 to 9
index = cmd->data[0];
// bounds check the index
if( (index >= 0) && (index<=9) ) {
// data is in range 0-254, shift to +/- 127
vexMotorSet( index, cmd->data[1] - 0x7F);
// Send ACK
P3Command(MyComms, &Cmd_Ack, packet->dev_id );
}
else
P3Command(MyComms, &Cmd_Nak_Para_Err, packet->dev_id );
}
else
P3Command(MyComms, &Cmd_Nak_Para_Err, packet->dev_id );
break;
default:
ret = 0;
break;
}
return(ret);
}
void autonDrive( int speed, int inches ) {
float inchesL=0;
float inchesR=0;
int timeout=0;
resetImeLeft();
resetImeRight();
// Start to motors
vexMotorSet( MotorDriveL1, speed-2 );
vexMotorSet( MotorDriveL2, speed-2 );
vexMotorSet( MotorDriveR1, speed );
vexMotorSet( MotorDriveR2, speed );
while( TRUE ) {
vexLcdPrintf( VEX_LCD_DISPLAY_1, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );
vexLcdPrintf( VEX_LCD_DISPLAY_2, VEX_LCD_LINE_2, "L %5.2f R %5.2f", getInchesLeft(), getInchesRight() );
if (inchesL == getInchesLeft() && inchesR == getInchesRight() ) {
timeout++;
if( timeout > 30 ) {
break;
}
}
inchesL = getInchesLeft();
inchesR = getInchesRight();
if( abs(inchesL) > inches || abs(inchesR) > inches ) {
break;
}
vexSleep(5);
}
// Reverse motors to stop coast
vexMotorSet( MotorDriveL1, -speed/2 );
vexMotorSet( MotorDriveL2, -speed/2 );
vexMotorSet( MotorDriveR1, -speed/2 );
vexMotorSet( MotorDriveR2, -speed/2 );
vexSleep( abs(speed/8) );
// Stop motors
vexMotorSet( MotorDriveL1, 0 );
vexMotorSet( MotorDriveL2, 0 );
vexMotorSet( MotorDriveR1, 0 );
vexMotorSet( MotorDriveR2, 0 );
}
/** @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);
}
}
