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
}
int main(void)
{
init(); //Initialise things
for(;;)
{
//Check the estop
if(ESTOP_PRESSED)
{
//Wait until it is released
while(ESTOP_PRESSED || BRAKE_PRESSED)
{
SHUTDOWN;
rgb(red);
//timer1PWMAOff(); //Stop PWM
delay_us(100); //Wait
}
RESTART;
rgb(green);
//timer1PWMA(); //Restart PWM
}
if(BRAKE_PRESSED)
{
//Start at the current duty cycle
s16 braking_duty = pedal_input;
//While the brake pedal is pressed do regenerative braking
while(BRAKE_PRESSED)
{
rgb(purple);
//Read the average current value
s16 current = a2dConvert10bit(AVG_CURRENT_PIN);
//Calculate the error from the target braking current
s16 error = BRAKING_CURRENT - current;
//Scale the error and add to braking duty
braking_duty += error/10;
//Stop the duty from going negative
if(braking_duty < 0)
{
braking_duty = 0;
}
//Set the PWM duty
timer1PWMASet(TOP_COUNT - braking_duty); //inverted
}
rgb(green);
}
//Check the average current
int current = a2dConvert10bit(AVG_CURRENT_PIN);
//Accumulate the amp-seconds the current is above the target current
ampSeconds += (current - TARGET_CURRENT);
//Stop the amp-seconds from going negative
if(ampSeconds < 0)
{
ampSeconds = 0;
}
//Shut down the controller if the current exceeds the maximum current
// or the amp-seconds has exceeded the maximum allowable value
if(ampSeconds > MAX_AMP_SECONDS || current > MAX_CURRENT)
{
SHUTDOWN;
rgb(blue);
}
else
{
RESTART;
rgb(green);
}
//Get the position of the accelerator pedal
pedal_input = a2dConvert10bit(ACCEL_PIN);
//mod
if(pedal_input < 50) pedal_input = 0; else pedal_input = pedal_input - 50;
//Rescale between 0 and TOP_COUNT
pedal_input *= (TOP_COUNT/1024);
//Set the PWM Duty
timer1PWMASet(TOP_COUNT - pedal_input); //inverted
}
}
void setRightPWM(uint8_t pwm)
{
rightPWM = pwm;
timer1PWMASet(pwm);
}
