void mainInit()
{
// not used pins as inputs with pull-ups
DDRB &= ~(1 << PB4);
PORTB |= (1 << PB4);
DDRB &= ~(1 << PB5);
PORTB |= (1 << PB5);
s_timerCounter = 0;
s_started = 0;
s_buttonPressed = 0;
s_buttonLock = 0;
s_speed = 40;
s_straightMode = CONTROLLER_LEFT_AND_RIGHT;
sei();
motorsInit();
wheelsInit();
controllerInitWithTimer(&mainTimerTick);
remoteInit();
sensorsInit();
ledInit();
buttonInit();
}
int main(int argc, char** argv) {
if (signal(SIGINT, sigHandler) == SIG_ERR) {
printf("\ncan't catch SIGINT\n");
}
if (parseArguments(argc,argv) != 0) {
fprintf(stderr,"FATAL: Error parsing arguments\n");
exit(EXIT_FAILURE);
}
if (wheelsInit(wheelsConfigPath) != 0) {
fprintf(stderr,"FATAL: Error initialising wheels module\n");
exit(EXIT_FAILURE);
}
if (ultInit(ultrasonicConfigPath) != 0) {
fprintf(stderr,"FATAL: Error initializing ultrasonic module\n");
exit(EXIT_FAILURE);
}
loopFinished = 0;
int westClear = 0;
int northWestClear = 0;
int northClear = 0;
int northEastClear = 0;
int eastClear = 0;
char* someData = '@';
ultrasonicData_t data;
if (!openUltrasonicPipes()){
fprintf(stderr,"Unable to open ultrasonic pipes\n");
exit(EXIT_FAILURE);
}
while (loopFinished == 0) {
if(fwrite(someData,1,1,ultrasonicPipeIn) > 0) {
if(fread(&data,sizeof(data),1,ultrasonicPipeOut) >0) {
westClear = (data.west > MIN_SIDE_DISTANCE);
northClear = (data.north > MIN_FRONT_DISTANCE);
eastClear = (data.east > MIN_SIDE_DISTANCE);
if (northClear) {
fprintf(stdout,"NW,N,NW Clear. Moving Forwared\n");
wheelsMoveFwd(0l,TURN_TIME);
} else if (eastClear) {
fprintf(stdout,"E clear, move back, turn to east\n");
wheelsMoveBack(0l,TURN_TIME);
turnToEast();
} else if (westClear) {
fprintf(stdout,"W clear, turning west\n");
wheelsMoveBack(0l,TURN_TIME);
turnToWest();
} else {
fprintf(stdout,"Moving back\n");
wheelsMoveBack(0l,TURN_TIME);
}
}
}
}
wheelsStop();
exit(EXIT_SUCCESS);
}
