void grabBonusBall()
{
// Get in position
//setPose(BB_READY);
setServo(TILT, TILT_BACK);
setServo(LIFT, LIFT_BB_READY);
myDelay(40);
// Drive up to the corner
timer1PWMASet(255);
timer1PWMBSet(255);
forward();
myDelay(20);
brake();
// Execute the pickup sequence
setServo(BOOM, BOOM_BB_GRAB);
myDelay(10);
setServo(TILT, TILT_BB_GRAB);
myDelay(10);
setServo(BOOM, BOOM_UP);
myDelay(15);
setServo(LIFT, LIFT_UP);
myDelay(0);
// Back away from the corner
timer1PWMASet(200);
timer1PWMBSet(200);
reverse();
myDelay(15);
brake();
}
void led_wave_set(LED_BLOCK block, uint16_t duty)
{
switch(block)
{
case LED_PIN_ESC:
timer3PWMCSet(duty);
break;
case LED_PIN_Fx:
timer3PWMASet(duty);
break;
case LED_PIN_PAD:
timer0PWMSet(duty);
#ifdef KBDMOD_M5
if(!isLED3000)
timer1PWMASet(duty);
#endif
break;
case LED_PIN_BASE:
timer3PWMBSet(duty);
break;
case LED_PIN_WASD:
timer1PWMCSet(duty);
break;
case LED_PIN_ARROW18:
timer1PWMBSet(duty);
#ifdef KBDMOD_M5
if(isLED3000)
timer1PWMASet(duty);
#endif
break;
case LED_PIN_VESEL:
timer1PWMASet(duty);
break;
case LED_PIN_ALL:
timer3PWMCSet(duty);
timer3PWMASet(duty);
timer3PWMBSet(duty);
timer1PWMCSet(duty);
timer1PWMBSet(duty);
timer1PWMASet(duty);
timer0PWMSet(duty);
break;
default:
break;
}
}
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 setLeftPWM(uint8_t pwm)
{
leftPWM = pwm;
timer1PWMBSet(pwm);
}
void setPWM(int xValue) {
timer1PWMBSet(xValue);
}
