int main()
{
int wait;
// Set up port for start switch
lcdInit();
// Set up motors and encoders
motorInit();
encodersInit();
servoInit();
// Set up IRs
gp2d12Init(PIN_HEAD_IR);
digitalSetDirection(PIN_IR,AVRRA_INPUT);
digitalSetData(PIN_IR,AVRRA_LOW);
// Set up OP599A - analog already initialized
digitalSetDirection(PIN_PHOTO,AVRRA_INPUT);
digitalSetData(PIN_PHOTO,AVRRA_LOW);
// Set up fan & test
digitalSetData(PIN_FAN, AVRRA_LOW);
digitalSetDirection(PIN_FAN, AVRRA_OUTPUT);
digitalSetData(PIN_FAN, AVRRA_HIGH);
delayms(25);
digitalSetData(PIN_FAN, AVRRA_LOW);
lcdPrint("Wait... ");
wait = digitalGetData(PIN_START);
while(wait) {
// standard delay ..
delayms(10);
wait = digitalGetData(PIN_START);
}
// Start our map
mapInit();
lcdClear();
PrintHeading(lheading,nStart);
sei();
// Run Behaviors
while(1) {
// update odometer
while(rCount > COUNTS_CM(1) ) {
// odometer is in CM
odometer = odometer - 1;
rCount = rCount - COUNTS_CM(1);
}
// now run behaviors
if(arbitrate()>0) {
// let motors wind down
delayms(500);
clearCounters;
plan();
clearCounters;
}
}
}
/** main() - where all the setup happens. */
int main(){
int RXbypass; // Bypass rx?
unsigned char input, checksum;
serialInit(115200); // Startup serial port
// setup variables
count = 0;
servoSetPosition(PIN_SERVO, -80);
// setup sensors
gp2longInit(PIN_PAN_IR);
gp2d12Init(PIN_LEFT_IR);
gp2d12Init(PIN_RIGHT_IR);
gp2d12Init(PIN_L_REAR_IR);
gp2d12Init(PIN_R_REAR_IR);
servoInit(PIN_SERVO,-15,1);
// RX bypass enabled?
digitalSetDirection(PIN_NO_RX, AVRRA_INPUT);
digitalSetData(PIN_NO_RX, AVRRA_HIGH);
if(digitalGetData(PIN_NO_RX) == AVRRA_LOW){
// bypass RX;
RXbypass = TRUE;
TXgo = TRUE;
startFrame();
delayms(5000);
}else{
RXbypass = FALSE;
Print("MechDAR V1.0 Alive\n");
TXgo = FALSE;
}
// setup a 60Hz clock on Timer/Counter 2
TCNT2 = 0;
OCR2A = 240; // this defines 60 Hz Clock
TCCR2A = 0x03; // no A|B compare, mode7 fast pwm
TCCR2B = 0x0F; // clock/1024
TIMSK2 |= 0x01; // interrupt on overflow
// start clock
sei();
while(1){
if((RXbypass == FALSE) && (serialAvailable() > 0)){
// process incoming
if(serialRead() == -1){ // 0xFF
while(serialAvailable() == 0);
input = (unsigned char) serialRead();
while(serialAvailable() == 0);
checksum = (unsigned char) serialRead();
PrintNumber(input);
PrintNumber(checksum);
// if a valid packet...
if((255 - ((input + checksum)%256)) == 0){
// process the data
switch(input){
case REG_LEFT_IR:
PrintNumber(gp2d12GetData(PIN_LEFT_IR));
Print("\n");
break;
case REG_RIGHT_IR:
PrintNumber(gp2d12GetData(PIN_LEFT_IR));
Print("\n");
break;
case REG_L_REAR_IR:
PrintNumber(gp2d12GetData(PIN_LEFT_IR));
Print("\n");
break;
case REG_R_REAR_IR:
PrintNumber(gp2d12GetData(PIN_LEFT_IR));
Print("\n");
break;
case REG_STOP:
TXgo = FALSE;
break;
case REG_GO:
TXgo = TRUE;
startFrame();
break;
default:
// Send a reading from the data frame
PrintNumber(data[(input-REG_PAN_BASE)%10]);
Print("\n");
break;
}
}
}
}
asm("nop");
}
return 0;
};
