int main(void)
{
init();
for(;;)
{
// Set LED0
SetLED(ON);
// Motor Tests
SetMotor(L_MOT, FWD);
SetIR(L_IRLED, ON);
_delay_ms(2000.0);
SetMotor(L_MOT, REV);
_delay_ms(2000.0);
SetMotor(L_MOT, STOP);
_delay_ms(2000.0);
// Clear LED0;
SetIR(L_IRLED, OFF);
SetLED(OFF);
SetIR(C_IRLED, ON);
// Motor Tests
SetMotor(R_MOT, FWD);
_delay_ms(2000.0);
SetMotor(R_MOT, REV);
_delay_ms(2000.0);
SetIR(C_IRLED, OFF);
SetIR(R_IRLED, ON);
SetMotor(R_MOT, STOP);
_delay_ms(4000.0);
SetIR(R_IRLED, OFF);
/*
// Sensor Tests
// IRLED Tests
SetIR(R_IRLED, ON);
pause(PAUSE_T);
while(bit_is_clear(PINA, PA3)) // Right IR RCV
{
SetLED(ON);
pause(PAUSE_T);
SetLED(OFF);
pause(PAUSE_T);
}
SetIR(R_IRLED, OFF);
SetIR(C_IRLED, ON);
pause(PAUSE_T);
while(bit_is_clear(PINA, PA2))
{
SetLED(ON);
pause(PAUSE_T/2);
SetLED(OFF);
pause(PAUSE_T/2);
}
SetIR(C_IRLED, OFF);
SetIR(L_IRLED, ON);
pause(PAUSE_T);
while(bit_is_clear(PINA, PA1)) // Left IR RCV
{
SetLED(ON);
pause(PAUSE_T/4);
SetLED(OFF);
pause(PAUSE_T/4);
}
SetIR(L_IRLED, OFF);
// Bumpers
while(bit_is_clear(PIND, PD5) ) // || bit_is_clear(PIND, PD5)) //
{
SetLED(ON);
}
SetLED(OFF); */
/*
// Optical Sensor Test
// Initialize Sensor
PORTC &= ~_BV(PC3); // Disable Shutdown
_delay_ms(150.0); // Wait 1.5 secs
PORTC |= _BV(PC7); // Enable NCS
//_delay_us(1.0);
SetLED(ON);
_delay_ms(100.0);
SetLED(OFF);
// Setup SPI
_delay_ms(100.0);
SPCR = _BV(SPE) | _BV(MSTR) | _BV(CPOL) | _BV(SPR0) | _BV(SPR1) | _BV(CPHA);
// Initialize Sensor
SetLED(ON);
OptSensorWrite(0x3A, 0x5A); // Addr = 0x3A + MSB(WRITE) // POST
_delay_ms(100.0);
SetLED(OFF);
_delay_ms(100.0);
OptSensorRead(0x02); // Read Motion Status
OptSensorRead(0x03); // Read Delta_X
OptSensorRead(0x04); // Read Delta_Y
//OptSensorWrite(0x01, 0x01); // Self-Test
SetLED(ON);
_delay_ms(100.0);
SetLED(OFF);
for(;;)
{
data = OptSensorRead(0x02);
while(data & _BV(7)) // Motion!
{
OptSensorRead(0x03);
OptSensorRead(0x04);
data = OptSensorRead(0x02);
SetLED(ON);
}
SetLED(OFF);
//_delay_ms(10.0);
} */
}
return(0);
}
bool Run()
{
//Main loop - get keypresses and interpret them
keystate=GetRemoteKeys();
if(keystate != oldKeystate) {
oldKeystate = keystate;
if(keystate & KEY_HOME) {
return false;
}
if (mode == 1 || mode == 2) { //menu navigation is only relevant to menu modes
if(keystate & KEY_LEFT_BACK) {
//Menu down
if (menuItem<MAXMENU) {
menuItem++;
if(menuItem==3 || menuItem==4) {
menuNum=6;
} else if (menuItem == 5) {
menuNum=3;
} else {
menuNum=1;
}
}
}
if(keystate & KEY_LEFT_FORWARD) {
//Menu up
if (menuItem>1) {
menuItem--;
if(menuItem==3 || menuItem==4) {
menuNum=6;
} else if (menuItem == 5) {
menuNum=3;
} else {
menuNum=1;
}
}
}
}
if (mode == 1) {
if(keystate & KEY_RIGHT_FORWARD) {
//Menu down
if (menuNum<24) {
menuNum++;
}
}
if(keystate & KEY_RIGHT_BACK) {
//Menu up
if (menuNum>1) {
menuNum--;
}
}
if(keystate & KEY_INPUT1) {
//Clear
PlaySound(SOUND_FIRE);
steps=0;
menuItem=1;
menuNum=1;
mode=1;
OpenMotors();
}
if(keystate & KEY_INPUT2) {
//Menu select
if(steps+1<MAXSTEPS) {
steps++;
moveList[steps].cmd=menuItem;
moveList[steps].num=menuNum;
PlaySound(SOUND_BEEP);
}
}
if(keystate & KEY_MENU) {
//Switch to second menu
mode=2;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
} else if (mode == 2) {
if(keystate & KEY_INPUT1) {
//Switch back to first menu
mode=1;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
if (keystate & KEY_INPUT2) {
//Menu select
switch(menuItem) {
case 1: { //Free Roam
mode=4;
PlaySound(SOUND_BEEP);
CloseMotors();
} break;
case 2: { //Load route
LoadRoute();
ClearScreen();
PlaySound(SOUND_BEEP);
SetTextColor(white);
DrawText(5, 65, "Loading...");
Show();
Sleep(500); //Just so they notice...
mode=2;
} break;
case 3: { //Save route
PlaySound(SOUND_BEEP);
SaveRoute();
ClearScreen();
SetTextColor(white);
DrawText(5, 65, "Saving...");
Show();
Sleep(500); //Just so they notice...
mode=2;
} break;
case 4: { //Recalibrate
PlaySound(SOUND_BEEP);
mode=3; //Recalibration mode
menuNum=1; //Use menuNum as steps throught the process - 1=ready, 2=working
} break;
case 5: { //About
PlaySound(SOUND_GO);
mode=5;
CloseMotors();
} break;
}
}
if(keystate & KEY_MENU) {
//Switch back to first menu
mode=1;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
} else if (mode==3) {
//Recalibrate.
if(keystate & KEY_INPUT1) {
//Cancel
PlaySound(SOUND_BEEP);
SetMotors(0,0);
mode=2;
menuItem=4;
menuNum=1;
}
if (keystate & KEY_INPUT2) {
//Start/Stop
PlaySound(SOUND_BEEP);
switch (menuNum) {
case 1: { //Start
menuNum=2;
ResetTimer();
SetMotors(10000,-10000);
}
break;
case 2: { //Stop
rotateSleep = ReadTimer() / 24;
SetMotors(0,0);
mode=2;
menuItem=4;
menuNum=1;
SaveCalibration();
}
}
}
} else if (mode==4 || mode==5) {
if(keystate & KEY_MENU) {
//Switch back to first menu
PlaySound(SOUND_BEEP);
mode=2;
menuItem=1;
menuNum=1;
OpenMotors();
}
if (keystate & KEY_INPUT1) {
IRState = !IRState;
SetIR(IRState);
}
if (keystate & KEY_INPUT2) {
PlaySound(SOUND_BEEP);
mode=2;
menuItem=1;
menuNum=1;
OpenMotors();
}
}
if(keystate & KEY_RUN) {
//Go
ClearScreen();
Show();
PlaySound(SOUND_GO);
//Cycle through steps and execute
int count;
SetTextColor(green);
for(count=1; count<=steps;count++) {
switch(moveList[count].cmd) {
case 1: { //Forward
ClearScreen();
DrawText(5, 100, "%d: Forward %d",count, moveList[count].num);
Show();
SetMotors(10000,10000);
Sleep(FORWARDSLEEP * moveList[count].num);
SetMotors(0,0);
}
break;
case 2: { //Back
ClearScreen();
DrawText(5, 100, "%d: Back %d",count, moveList[count].num);
Show();
SetMotors(-10000,-10000);
Sleep(FORWARDSLEEP * moveList[count].num);
SetMotors(0,0);
}
break;
case 3: { //Right
ClearScreen();
DrawText(5, 100, "%d: Right %d",count, moveList[count].num * 15);
Show();
SetMotors(10000,-10000);
Sleep(rotateSleep * moveList[count].num);
SetMotors(0,0);
}
break;
case 4: { //Left
ClearScreen();
DrawText(5, 100, "%d: Left %d",count, moveList[count].num * 15);
Show();
SetMotors(-10000,10000);
Sleep(rotateSleep * moveList[count].num);
SetMotors(0,0);
}
break;
case 5: { //Fire
ClearScreen();
SetTextColor(red);
DrawText(10, 45, "PEW! PEW! PEW!");
SetTextColor(green);
DrawText(5, 100, "%d: Fire %d",count, moveList[count].num);
Show();
int fireCount;
for(fireCount=0; fireCount<moveList[count].num; fireCount++) {
PlaySound(SOUND_FIRE);
}
}
}
}
//reset menu pointer
menuItem=1;
menuNum=1;
PlaySound(SOUND_GO);
}
DrawMenu();
}
Sleep(50); //to stop the radio being on full time
return true;
}
