void init(void)
{
// Initialize Timer
timerInit();
// Initialize LCD
lcdInit();
ourLcdControlWrite(1<<LCD_ON_CTRL | 1<<LCD_ON_DISPLAY);
// Initialize UART
uartInit();
uartSetBaudRate(CMU_BAUD);
uartSetRxHandler(packetRcv);
rprintfInit(uartSendByte);
// Initialize PWM
outb(DDRD, 0xFF); // set all port D pins to output
timer1PWMInit(8);
timer1PWMAOn();
timer1PWMBOn();
// Initialize Servos
servoInit();
// Initialize CMU
lcdWriteStr("CMUcam2 init", 0, 0);
cmuInit();
}
//Initializes comms with the RasPi
void raspiInit(void) {
uartInit(RASPI_UART);
uartSetBaudRate(RASPI_UART, 115200);
uartSetRxHandler(RASPI_UART, raspiByteReceiver );
}
