hopefully this is correct, getting this under the 32k forum post limit was a bit awkward
code part 2
/*-----------------------------------------------------------------------------*/
/* Check the touch LED and color sensors */
/*-----------------------------------------------------------------------------*/
void
iqCheckSensors()
{
static bool robotTouchSensorPressed;
// Touch LED
if( robotTouchSensor.installed() ) {
if( robotTouchSensor.pressing() ) {
if(!robotTouchSensorPressed) {
robotEnabled = !robotEnabled;
robotTouchSensorPressed = true;
}
}
else {
robotTouchSensorPressed = false;
}
}
// Color sensor
if( robotColorSensor.installed() ) {
// close object ?
// TODO !
if( robotColorSensor.value() > 200 ) {
colorType color;
color = robotColorSensor.colorname12();
if( color == colorType::red )
robotEnabled = false;
if( color == colorType::green )
robotEnabled = true;
}
}
// Display enabled status on the touch LED
if( robotTouchSensor.installed()) {
if( robotRunning ) {
if(robotEnabled)
robotTouchSensor.on(colorType::green);
else
robotTouchSensor.on(colorType::red);
}
else
robotTouchSensor.on(colorType::none);
}
}
/*-----------------------------------------------------------------------------*/
/* Task used to drive the robot and handle all the discovered sensors */
/*-----------------------------------------------------------------------------*/
int
iqRunRobotTask()
{
while(1)
{
// Request to kill this task, we need to cleanup first
// set to disabled to stop motors
if( !robotRunning )
robotEnabled = false;
// Drive motors
iqRunRobotDrive();
// Aux motors
iqRunRobotAux();
// Check sensors
iqCheckSensors();
// done
if( !robotRunning )
break;
// Don't hog the cpu
task::sleep(25);
}
return(0);
}
/*-----------------------------------------------------------------------------*/
/* Calibrate gyro if installed */
/*-----------------------------------------------------------------------------*/
void
iqCalibrateGyro()
{
if( robotGyroSensor.installed() ) {
short count = 20;
// VEX sets touch sensor yellow when calibrating
if( robotTouchSensor.installed() )
robotTouchSensor.on( colorType::yellow );
robotGyroSensor.startCalibration(gyroCalibrationType::calNormal);
// delay so calibrate flag can be set internally to the gyro
task::sleep(100);
Brain.Screen.clearScreen();
// wait for calibration to finish or 2 seconds, whichever is longer
while( robotGyroSensor.isCalibrating() || (count-- > 0) ) {
char str[32];
sprintf( str, "Calibrating... %02d", count/10 );
Brain.Screen.printAt( 1, 1, str);
task::sleep(100);
}
// reset so this is 0 heading
robotGyroSensor.resetHeading();
}
}
/*-----------------------------------------------------------------------------*/
/* Display one line on the IQ display with inverted text if selected */
/*-----------------------------------------------------------------------------*/
void
iqDisplayLine( int row, bool select, const char *str )
{
if( row < 1 || row > 5 )
return;
int ypos = (row-1) * 11;
Brain.Screen.setPenColor( colorType::white );
Brain.Screen.setFillColor( colorType::white );
if( select ) {
Brain.Screen.drawRectangle( 0, ypos, 128,11);
Brain.Screen.printAt( row, 1, str );
Brain.Screen.invertRectangle( 0, ypos, 128, 11);
}
else {
Brain.Screen.drawRectangle( 0, ypos, 128, 11);
Brain.Screen.printAt( row, 1, str );
}
}
/*-----------------------------------------------------------------------------*/
/* Redisplay the configuration menu */
/*-----------------------------------------------------------------------------*/
void
iqMenuConfigurationDisplay( tConfigSelect selected, int offset )
{
char str[32];
int rowOffset = offset * -1;
iqDisplayLine( LCD_ROW1 + rowOffset, selected == kConfig_exit, "Exit " );
switch( configDriveMode )
{
case kJoystickTank: strcpy( str, "Control 2 Joystick "); break;
case kJoystickArcadeLeft: strcpy( str, "Control Left Stick "); break;
case kJoystickArcadeRight: strcpy( str, "Control Right Stick"); break;
default: strcpy( str, "Control Error "); break;
}
iqDisplayLine( LCD_ROW2 + rowOffset, selected == kConfig_0, str );
switch( configDriveTurn )
{
case kNormal: strcpy( str, "DriveTurn Normal "); break;
case kReverse: strcpy( str, "DriveTurn Reverse "); break;
default: strcpy( str, "DriveTurn Error "); break;
}
iqDisplayLine( LCD_ROW3 + rowOffset, selected == kConfig_1, str );
switch( IQMotors[ PORT1 ].direction )
{
case kNormal: strcpy( str, "Drive Fwd Normal "); break;
case kReverse: strcpy( str, "Drive Fwd Reverse "); break;
default: strcpy( str, "Drive Fwd Error "); break;
}
iqDisplayLine( LCD_ROW4 + rowOffset, selected == kConfig_2, str );
switch( IQMotors[ PORT4 ].direction )
{
case kNormal: strcpy( str, "Motor 4 Normal "); break;
case kReverse: strcpy( str, "Motor 4 Reverse "); break;
default: strcpy( str, "Motor 4 Error "); break;
}
iqDisplayLine( LCD_ROW5 + rowOffset, selected == kConfig_3, str );
switch( IQMotors[ PORT5 ].direction )
{
case kNormal: strcpy( str, "Motor 5 Normal ");break;
case kReverse: strcpy( str, "Motor 5 Reverse ");break;
default: strcpy( str, "Motor 5 Error "); break;
}
iqDisplayLine( LCD_ROW6 + rowOffset, selected == kConfig_4, str );
switch( IQMotors[ PORT10 ].direction )
{
case kNormal: strcpy( str, "Motor 10 Normal ");break;
case kReverse: strcpy( str, "Motor 10 Reverse ");break;
default: strcpy( str, "Motor 10 Error "); break;
}
iqDisplayLine( LCD_ROW7 + rowOffset, selected == kConfig_5, str );
switch( IQMotors[ PORT11 ].direction )
{
case kNormal: strcpy( str, "Motor 11 Normal ");break;
case kReverse: strcpy( str, "Motor 11 Reverse ");break;
default: strcpy( str, "Motor 11 Error "); break;
}
iqDisplayLine( LCD_ROW8 + rowOffset, selected == kConfig_6, str );
switch( (int)sonarFront )
{
case 1 : strcpy( str, "Sonar Front ");break;
default: strcpy( str, "Sonar Rear ");break;
}
iqDisplayLine( LCD_ROW9 + rowOffset, selected == kConfig_7, str );
}
/*-----------------------------------------------------------------------------*/
/* Action the select button when in the configure menu */
/* returns true if the exit item was selected */
/*-----------------------------------------------------------------------------*/
bool
iqMenuConfigurationSelect( tConfigSelect selected )
{
switch(selected)
{
case kConfig_exit:
return( true );
break;
case kConfig_0:
switch(configDriveMode)
{
case kJoystickTank: configDriveMode = kJoystickArcadeRight; break;
case kJoystickArcadeRight: configDriveMode = kJoystickArcadeLeft; break;
case kJoystickArcadeLeft: configDriveMode = kJoystickTank; break;
default: configDriveMode = kJoystickTank; break;
}
break;
case kConfig_1:
configDriveTurn = (configDriveTurn == kNormal) ? kReverse : kNormal;
break;
case kConfig_2:
// Use motors on port1 and port6 as reference even if they are not installed
IQMotors[ PORT1 ].direction = (IQMotors[ PORT1 ].direction == kNormal) ? kReverse : kNormal;
IQMotors[ PORT6 ].direction = (IQMotors[ PORT6 ].direction == kNormal) ? kReverse : kNormal;
break;
case kConfig_3:
IQMotors[ PORT4 ].direction = (IQMotors[ PORT4 ].direction == kNormal) ? kReverse : kNormal;
break;
case kConfig_4:
IQMotors[ PORT5 ].direction = (IQMotors[ PORT5 ].direction == kNormal) ? kReverse : kNormal;
break;
case kConfig_5:
IQMotors[ PORT10 ].direction = (IQMotors[ PORT10 ].direction == kNormal) ? kReverse : kNormal;
break;
case kConfig_6:
IQMotors[ PORT11 ].direction = (IQMotors[ PORT11 ].direction == kNormal) ? kReverse : kNormal;
break;
case kConfig_7:
sonarFront = !sonarFront;
break;
default:
break;
}
return( false );
}
/*-----------------------------------------------------------------------------*/
/* Action the "configure" menu item */
/*-----------------------------------------------------------------------------*/
void
iqMenuConfiguration()
{
static tConfigSelect configSelect = kConfig_exit;
int *pconfigSelect = (int *)&configSelect;
static int offset = 0;
bool done = false;
bool update = true;
bool dirty = false;
// Wait for LCD button release
while( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing())
task::sleep(10);
Brain.Screen.clearScreen();
while( !done ) {
// Display menu
if( update ) {
iqMenuConfigurationDisplay( configSelect, offset );
update = false;
}
// Check LCD buttons
if( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() ) {
if( Brain.buttonDown.pressing() ) {
if( configSelect == kConfig_7 )
configSelect = kConfig_7;
else
(*pconfigSelect)++;
if( configSelect > kConfig_3 && offset == 0 )
offset = 4;
}
if( Brain.buttonUp.pressing() ) {
if( configSelect == kConfig_exit )
configSelect = kConfig_exit;
else
(*pconfigSelect)--;
if( configSelect < kConfig_3 && offset == 4 )
offset = 0;
}
if( Brain.buttonCheck.pressing() ) {
if( iqMenuConfigurationSelect( configSelect ) ) {
if( dirty )
iqSettingsStore();
done = true;
}
else
dirty = true;
}
// Exit doesn't really work
//if( nLCDButtons == kButtonExit ) {
// done = true;
// }
// Wait for LCD button release
while( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() )
task::sleep(10);
// update display
update = true;
}
task::sleep(25);
}
// erase before we leave this sub menu
Brain.Screen.clearScreen();
}
/*-----------------------------------------------------------------------------*/
/* Function to draw up or down arrow as the LCD character set seems limited */
/*-----------------------------------------------------------------------------*/
void
drawArrow( int xpos, int ypos, bool dir )
{
int i;
//Brain.Screen.setPenColor(colorType::white);
//Brain.Screen.drawRectangle(xpos, ypos, 8, 5, colorType::white );
Brain.Screen.setPenColor(colorType::black);
if( dir ) { // Up arrow
for(i=0;i<=4;i++)
Brain.Screen.drawLine( xpos+i, ypos+i, xpos + (8-i), ypos+i );
}
else { // Down arrow
for(i=0;i<=4;i++)
Brain.Screen.drawLine( xpos+(4-i), ypos+i, xpos + (4+i), ypos+i );
}
}
/*-----------------------------------------------------------------------------*/
/* Display controller status */
/* Long form formatting to try and match the VEX version under driver control */
/* ROBOTC standard text position does not match the VEX display */
/*-----------------------------------------------------------------------------*/
void
iqRunDisplayJoystickStatus()
{
// background text
Brain.Screen.printAt( LCD_ROW1, 1, "Button Axis" );
Brain.Screen.printAt( LCD_ROW2, 1, "E" );
Brain.Screen.printAt( LCD_ROW3, 1, "F" );
Brain.Screen.printAt( LCD_ROW4, 1, "R" );
Brain.Screen.printAt( LCD_ROW5, 1, "L" );
Brain.Screen.printAt( LCD_ROW2, 8, "A" );
Brain.Screen.printAt( LCD_ROW3, 8, "B" );
Brain.Screen.printAt( LCD_ROW4, 8, "C" );
Brain.Screen.printAt( LCD_ROW5, 8, "D" );
// Display button status
if( Controller.ButtonEDown.pressing() )
drawArrow( 13, (LCD_ROW2*11)-8, false );
else
Brain.Screen.printAt( LCD_ROW2, 3, " " );
if( Controller.ButtonEUp.pressing() )
drawArrow( 25, (LCD_ROW2*11)-8, true );
else
Brain.Screen.printAt( LCD_ROW2, 5, " " );
if( Controller.ButtonFDown.pressing() )
drawArrow( 13, (LCD_ROW3*11)-8, false );
else
Brain.Screen.printAt( LCD_ROW3, 3, " " );
if( Controller.ButtonFUp.pressing() )
drawArrow( 25, (LCD_ROW3*11)-8, true );
else
Brain.Screen.printAt( LCD_ROW3, 5, " " );
if( Controller.ButtonRDown.pressing() )
drawArrow( 13, (LCD_ROW4*11)-8, false );
else
Brain.Screen.printAt( LCD_ROW4, 3, " " );
if( Controller.ButtonRUp.pressing() )
drawArrow( 25, (LCD_ROW4*11)-8, true );
else
Brain.Screen.printAt( LCD_ROW4, 5, " " );
if( Controller.ButtonLDown.pressing() )
drawArrow( 13, (LCD_ROW5*11)-8, false );
else
Brain.Screen.printAt( LCD_ROW5, 3, " " );
if( Controller.ButtonLUp.pressing())
drawArrow( 25, (LCD_ROW5*11)-8, true );
else
Brain.Screen.printAt( LCD_ROW5, 5, " " );
// Display controller value
Brain.Screen.printAt( LCD_ROW2, 12, "%+4d", Controller.AxisA.value() );
Brain.Screen.printAt( LCD_ROW3, 12, "%+4d", Controller.AxisB.value() );
Brain.Screen.printAt( LCD_ROW4, 12, "%+4d", Controller.AxisC.value() );
Brain.Screen.printAt( LCD_ROW5, 12, "%+4d", Controller.AxisD.value() );
// Percent symbol needs fixing somehow (reversed??)
// and dropped one line
Brain.Screen.printAt( LCD_ROW2, 16, "%%");
Brain.Screen.printAt( LCD_ROW3, 16, "%%");
Brain.Screen.printAt( LCD_ROW4, 16, "%%");
Brain.Screen.printAt( LCD_ROW5, 16, "%%");
}
/*-----------------------------------------------------------------------------*/
/* Display device status information when running */
/*-----------------------------------------------------------------------------*/
void
iqRunDisplayDeviceStatus( int page )
{
IQ_DeviceType type;
short index;
short port;
short deviceCount = 0;
short deviceStart = (page * 4) + 1;
short displayRow = LCD_ROW1;
Brain.Screen.printAt( LCD_ROW1, 1, "Port Status" );
if( robotTotalDevices == 0 )
return;
// Get all device info
for(index=(short)PORT1;index<=(short)PORT12;index++) {
port = index + 1;
type = vex::device(index).type();
// found a device
if( type != kDeviceTypeNoSensor )
deviceCount++;
else
continue;
// device for this page ?
if( deviceCount < deviceStart )
continue;
// next row
displayRow++;
// 4 per page
if( displayRow > LCD_ROW5 )
return;
// Motor
if( type == kDeviceTypeMotorSensor ) {
IQMotors[ index ].velocity = IQMotors[ index ].instance.velocity(velocityUnits::rpm);
Brain.Screen.printAt( displayRow, 1, "%2d Motor %+4d rpm", port, (int)IQMotors[ index ].velocity );
}
// bumper switch
if( type == kDeviceTypeBumperSensor ) {
if( vex::bumper(index).value() )
Brain.Screen.printAt( displayRow, 1, "%2d Bumper Pressed ", port );
else
Brain.Screen.printAt( displayRow, 1, "%2d Bumper Released", port );
}
// Touch LED
if( type == kDeviceTypeLedSensor ) {
if( vex::touchled(index).value() )
Brain.Screen.printAt( displayRow, 1, "%2d Led Pressed ", port );
else
Brain.Screen.printAt( displayRow, 1, "%2d Led Released", port );
}
// Color sensor
if( type == kDeviceTypeRgbSensor ) {
Brain.Screen.printAt( displayRow, 1, "%2d Color ", port );
vex::colorsensor c(index);
if( c.isNearObject() ) {
switch( c.colorname12() )
{
default: Brain.Screen.printAt( displayRow,10, "No Object "); break;
case colorType::red_violet: Brain.Screen.printAt( displayRow,10, "RED/VIOLET"); break;
case colorType::red: Brain.Screen.printAt( displayRow,10, "RED "); break;
case colorType::red_orange: Brain.Screen.printAt( displayRow,10, "DRK ORANGE"); break;
case colorType::orange: Brain.Screen.printAt( displayRow,10, "ORANGE "); break;
case colorType::yellow_orange: Brain.Screen.printAt( displayRow,10, "DRK YELLOW"); break;
case colorType::yellow: Brain.Screen.printAt( displayRow,10, "YELLOW "); break;
case colorType::yellow_green: Brain.Screen.printAt( displayRow,10, "LIME GREEN"); break;
case colorType::green: Brain.Screen.printAt( displayRow,10, "GREEN "); break;
case colorType::blue_green: Brain.Screen.printAt( displayRow,10, "BLUE/GREEN"); break;
case colorType::blue: Brain.Screen.printAt( displayRow,10, "BLUE "); break;
case colorType::blue_violet: Brain.Screen.printAt( displayRow,10, "DRK BLUE "); break;
case colorType::violet: Brain.Screen.printAt( displayRow,10, "VIOLET "); break;
}
}
else
Brain.Screen.printAt( displayRow, 10, "No Object ");
}
// Gyro
if( type == kDeviceTypeGyroSensor ) {
int angle = vex::gyro(index).angle();
if( angle < 0 ) angle += 360;
Brain.Screen.printAt( displayRow, 1, "%2d Gyro %3d deg", port, angle );
}
// Sonar
if( type == kDeviceTypeSonarSensor ) {
float dist = vex::sonar(index).distance(distanceUnits::mm);
Brain.Screen.printAt( displayRow, 1, "%2d Dist %4d in", port, (int)(dist/25.4) );
}
}
}
/*-----------------------------------------------------------------------------*/
/* Display logo on the home screen when running */
/*-----------------------------------------------------------------------------*/
void
iqRunDisplayLogo()
{
drawBitmap( getLogo() );
}
/*-----------------------------------------------------------------------------*/
/* Display the little check box with some text next to it */
/*-----------------------------------------------------------------------------*/
void
iqDrawSelectButtonAndText( int xpos, int row, const char *text )
{
int ypos = (row * 11) - 3;
Brain.Screen.setPenColor( colorType::black );
Brain.Screen.drawLine( xpos+ 1, ypos, xpos+ 9, ypos );
Brain.Screen.drawLine( xpos+ 1, ypos-8, xpos+ 9, ypos-8 );
Brain.Screen.drawLine( xpos , ypos-1, xpos , ypos-7 );
Brain.Screen.drawLine( xpos+10, ypos-1, xpos+10, ypos-7 );
Brain.Screen.drawLine( xpos+ 2, ypos-3, xpos+ 4, ypos-1 );
Brain.Screen.drawLine( xpos+ 4, ypos-1, xpos+ 8, ypos-5 );
Brain.Screen.printAt( row, 4, text );
}
/*-----------------------------------------------------------------------------*/
/* Display information for home screen when running */
/*-----------------------------------------------------------------------------*/
void
iqRunDisplayHome( bool showExit = false )
{
static long nPgmStartTime = Brain.Timer.time();
// display static text
Brain.Screen.printAt( LCD_ROW1, 1, "Running" );
// Calculate robot running time
long secs = (Brain.Timer.time() - nPgmStartTime)/1000;
int mins = secs / 60;
int hours = mins / 60;
// display program running time
Brain.Screen.printAt( LCD_ROW1, 10, "%02d:%02d:%02d", hours, mins % 60, secs % 60 );
// run/stop/exit select box
if( showExit )
iqDrawSelectButtonAndText( 0, LCD_ROW5, "Exit");
else
if( robotEnabled )
iqDrawSelectButtonAndText( 0, LCD_ROW5, "Stop");
else
iqDrawSelectButtonAndText( 0, LCD_ROW5, "Run ");
// Display sonar
if( robotSonarSensor.installed()) {
int dist = robotSonarSensor.distance(distanceUnits::mm);
if( dist > 1000 )
Brain.Screen.printAt(LCD_ROW5, 9, "----", dist);
else
Brain.Screen.printAt( LCD_ROW5, 9, "%04d", dist);
}
// Display gyro
if( robotGyroSensor.installed()) {
int heading = robotGyroSensor.angle();
if( heading < 0 ) heading += 360;
Brain.Screen.printAt( LCD_ROW5, 14, "%04d", heading);
}
}
/*-----------------------------------------------------------------------------*/
/* Action the "run" menu item */
/*-----------------------------------------------------------------------------*/
void
iqMenuRun()
{
bool done = false;
int select = 0;
int select_old = 0;
int selectMax = 2 + (robotTotalDevices-1)/4;
int pressedTime;
// Clear the LCD
Brain.Screen.clearScreen();
// Draw the ROBOTC logo
iqRunDisplayLogo();
// Wait for LCD button release
while( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() )
task::sleep(10);
// Update LCD and check buttons
while(!done)
{
// Check LCD buttons
if( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() ) {
pressedTime = 0;
// Down button pushed ?
if( Brain.buttonDown.pressing() )
if( select++ == selectMax ) select = 0;
// Up button pushed ?
if( Brain.buttonUp.pressing() )
if( select-- == 0 ) select = selectMax;
// Select button pushed ?
if( Brain.buttonCheck.pressing() )
robotEnabled = !robotEnabled;
// Wait for LCD button release
// If select is held then simulate exit
while( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing()) {
if( select == 0 && Brain.buttonCheck.pressing()&& pressedTime++ == 120 ) {
iqRunDisplayHome( true );
done = true;
}
task::sleep(10);
}
// change of screen ?
if( select != select_old ) {
// Clear the LCD display ready for a new status screen
Brain.Screen.clearScreen();
// Only draw the logo bitmap once when screen 0 is selected
if( select == 0 )
iqRunDisplayLogo();
}
select_old = select;
}
// Update status based on which status screen has been selected
switch( select )
{
case 0:
iqRunDisplayHome();
break;
case 1:
iqRunDisplayJoystickStatus();
break;
case 2:
case 3:
case 4:
iqRunDisplayDeviceStatus( select-2 );
break;
default:
break;
}
// no need to run often
task::sleep(50);
}
// cleanup
Brain.Screen.clearScreen();
}
/*-----------------------------------------------------------------------------*/
/* Run a task that handles the main menu */
/*-----------------------------------------------------------------------------*/
int
iqMenuTask()
{
static int select = 0;
bool update = true;
// Run menu forever
while( true ) {
if( update ) {
// Update main menu, only three items
iqDisplayLine( LCD_ROW1, select == 0, "Run" );
iqDisplayLine( LCD_ROW2, select == 1, "Configure" );
iqDisplayLine( LCD_ROW3, select == 2, "Reset to Default" );
update = false;
}
// Check LCD buttons
if( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() ) {
if( Brain.buttonDown.pressing() )
if( select++ == 2 ) select = 2;
if( Brain.buttonUp.pressing() )
if( select-- == 0 ) select = 0;
if( Brain.buttonCheck.pressing() ) {
// reset
if( select == 2 ) iqResetToDefaults();
// Enter configure menu
if( select == 1 ) iqMenuConfiguration();
// Enter run mode
if( select == 0 ) {
// Gyro sensor installed ?
if( robotGyroSensor.installed() )
iqCalibrateGyro();
// Set everything as enabled
robotRunning = true;
robotEnabled = true;
// Start the control task
vex::task t1(iqRunRobotTask, vex::task::taskPriorityHigh);
// and display information on the LCD while running...
iqMenuRun();
// When we return to this point user has indicated program should stop
// force task to stop motors and quit
robotRunning = false;
task::sleep(100);
// task should be done
}
}
// Wait for LCD button release
while( Brain.buttonUp.pressing() || Brain.buttonDown.pressing() || Brain.buttonCheck.pressing() )
task::sleep(10);
// update display
update = true;
}
// Don't hog cpu
task::sleep(25);
}
return(0);
}
/*-----------------------------------------------------------------------------*/
/* Discover all the motors and sensors connected to the VexIQ */
/*-----------------------------------------------------------------------------*/
void
iqDiscoverDevices()
{
int type;
short index;
// Clear data
for(index=(short)PORT1;index<=(short)PORT12;index++) {
IQMotors[ index ].installed = false;
IQMotors[ index ].direction = kNormal;
IQMotors[ index ].fwdTouch = NULL;
IQMotors[ index ].revTouch = NULL;
}
// We have no way to save settings in the simplified code
// so set to user defaults, full version overrides this
IQMotors[ (short)PORT1 ].direction = DRIVE_FWD_DEFAULT;
IQMotors[ (short)PORT6 ].direction = DRIVE_FWD_DEFAULT;
IQMotors[ (short)PORT4 ].direction = MOTOR_04_DEFAULT;
IQMotors[ (short)PORT5 ].direction = MOTOR_05_DEFAULT;
IQMotors[ (short)PORT10 ].direction = MOTOR_10_DEFAULT;
IQMotors[ (short)PORT11 ].direction = MOTOR_11_DEFAULT;
//
robotTotalDevices = 0;
// Get all device info
for(index=(short)PORT1;index<=(short)PORT12;index++) {
type = vex::device(index).type();
console::write("device at %2d is %2d\n", index, type );
// Device found ?
if( type != kDeviceTypeNoSensor)
robotTotalDevices++;
// Motor ?
if( type == kDeviceTypeMotorSensor ) {
// We expect left motors on port 1 and 7
// We expect right motors on port 6 and 12
// Aux motors on ports 4,5,10 & 11
IQMotors[ index ].installed = true;
}
// Configure bumper switch to stop motors 4 and 10
if( type == kDeviceTypeBumperSensor ) {
// These seem backwards but match's VEX implementation
if( index == (short)PORT2 )
IQMotors[ PORT4 ].revTouch = &Bumper2;
if( index == (short)PORT3 )
IQMotors[ PORT4 ].fwdTouch = &Bumper3;
if( index == (short)PORT8 )
IQMotors[ PORT10 ].revTouch = &Bumper8;
if( index == (short)PORT9 )
IQMotors[ PORT10 ].fwdTouch = &Bumper9;
}
// Touch LED on any port used to start stop robot
if( type == kDeviceTypeLedSensor )
robotTouchSensor = vex::touchled(index);
// Color sensor on any port used to start stop robot
if( type == kDeviceTypeRgbSensor ) {
robotColorSensor = vex::colorsensor(index);
}
// Gyro on any port used to reorientate robot
if( type == kDeviceTypeGyroSensor )
robotGyroSensor = vex::gyro(index);
// Sonar on any port used to stop robot
if( type == kDeviceTypeSonarSensor )
robotSonarSensor = vex::sonar(index);
}
}
int main() {
// Look for motors and sensors
iqDiscoverDevices();
// recover configuration
iqSettingsRetrieve();
// start the main menu task
vex::thread menu( iqMenuTask );
this_thread::sleep_for(100);
vex::task::dump();
while(1) {
// Allow other tasks to run
this_thread::sleep_for(10);
}
}
here’s the project
iqDefault.iqcpp.zip (11.5 KB)