void init_motors()
{
//off
cbi(PORTD, 4); //motor off
cbi(PORTD, 5); //motor off
cbi(PORTD, 7); //motor off
cbi(PORTB, 3); //motor off
sbi(DDRD, 4); //motor outs
sbi(DDRD, 5); //motor outs
sbi(DDRD, 7); //motor outs
sbi(DDRB, 3); //motor outs
timerInit();
cbi(TIMSK, TOIE0); // disable timer 0 overflow interrupt
cbi(TIMSK, TOIE1); // disable timer 1 overflow interrupt
cbi(TIMSK, TOIE2); // disable timer 2 overflow interrupt
timer0SetPrescaler(TIMER_CLK_DIV1);
timer1SetPrescaler(TIMER_CLK_DIV1);
timer2SetPrescaler(TIMER_CLK_DIV1);
// setup PWM timer 0
OCR0 = 0; // duty cycle 0%
// enable timer0 as PWM phase correct, todo: use fast pwm
sbi(TCCR0,WGM00);
cbi(TCCR0,WGM01);
// turn on channel (OC0) PWM output
// set OC0 as non-inverted PWM
cbi(TCCR0,COM00);
sbi(TCCR0,COM01);
// setup timer 1A/B
timer1PWMInit(8); // pwm 8 bit
timer1PWMASet(0); // duty cycle 0%
timer1PWMBSet(0); // duty cycle 0%
timer1PWMAOn();
timer1PWMBOn();
//setup PWM timer 2
OCR2 = 0; // duty cycle 0%
// enable timer2 as PWM phase correct, todo: use fast pwm
sbi(TCCR2,WGM20);
cbi(TCCR2,WGM21);
// turn on channel (OC0) PWM output
// set OC0 as non-inverted PWM
cbi(TCCR2,COM20);
sbi(TCCR2,COM21);
// enable timer interrupt
sbi(TIMSK, TOIE0); //enable timer 0 overflow interrupt
}
void initFullLEDState(void) {
if(ledInited) return;
ledInited = 1;
/*
timer0 = usb, ps/2 interface
timer1 = pwm
timer2 = custom macro
*/
timer1Init();
timer1PWMInit(8);
timer1PWMBOn();
// DEBUG_PRINT(("______________________ init LED State_____________\n"));
initFullLEDStateAfter();
}
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();
}