299 lines
7.5 KiB
C
299 lines
7.5 KiB
C
#include "qfixSoccerBoard.h"
|
|
|
|
|
|
static int speedMotor0 = 0;
|
|
static int speedMotor1 = 0;
|
|
//static int speedMotor2 = 0;
|
|
//static int speedMotor3 = 0;
|
|
//static int speedMotor4 = 0;
|
|
//static int speedMotor5 = 0;
|
|
|
|
static unsigned long timerVal = 0;
|
|
static unsigned long sleepVal = 0;
|
|
|
|
static int beepFreq = 0;
|
|
static int beepFreqTimer = 0;
|
|
static long beepTimer = 0;
|
|
|
|
|
|
// PWM routine //
|
|
SIGNAL (SIG_OVERFLOW0)
|
|
{
|
|
const int OFFSET=50; // motor does not work with very low ratio
|
|
static int counter=255+OFFSET;
|
|
|
|
if (speedMotor0==0) cbi(PORTB, 3); // enable1 = 0
|
|
else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 3); // enable1 = 1
|
|
else cbi(PORTB, 3); // enable1 = 0
|
|
|
|
if (speedMotor1==0) cbi(PORTG, 2); // enable2 = 0
|
|
else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTG, 2); // enable2 = 1
|
|
else cbi(PORTG, 2); // enable2 = 0
|
|
|
|
/*if (speedMotor2==0) cbi(PORTB, 4); // enable3 = 0
|
|
else if (abs(speedMotor2)+OFFSET >= counter) sbi(PORTB, 4); // enable3 = 1
|
|
else cbi(PORTB, 4); // enable3 = 0
|
|
|
|
if (speedMotor3==0) cbi(PORTB, 6); // enable4 = 0
|
|
else if (abs(speedMotor3)+OFFSET >= counter) sbi(PORTB, 6); // enable4 = 1
|
|
else cbi(PORTB, 6); // enable4 = 0
|
|
|
|
if (speedMotor4==0) cbi(PORTB, 5); // enable5 = 0
|
|
else if (abs(speedMotor4)+OFFSET >= counter) sbi(PORTB, 5); // enable5 = 1
|
|
else cbi(PORTB, 5); // enable5 = 0
|
|
|
|
if (speedMotor5==0) cbi(PORTB, 7); // enable6 = 0
|
|
else if (abs(speedMotor5)+OFFSET >= counter) sbi(PORTB, 7); // enable6 = 1
|
|
else cbi(PORTB, 7); // enable6 = 0*/
|
|
|
|
if (counter==0) counter=255+OFFSET;
|
|
else counter--;
|
|
|
|
if(timerVal > 0) timerVal--;
|
|
if(sleepVal > 0) sleepVal--;
|
|
|
|
if(beepTimer > 0) beepTimer--;
|
|
|
|
if(beepTimer == 0) cbi(PORTB, 4);
|
|
else {
|
|
if(beepFreqTimer) beepFreqTimer--;
|
|
else beepFreqTimer = beepFreq;
|
|
|
|
if(beepFreqTimer > beepFreq/2) sbi(PORTB, 4);
|
|
else cbi(PORTB, 4);
|
|
}
|
|
}
|
|
|
|
void initTimer()
|
|
{
|
|
TIMSK0=1; // timer 0 overflow for interrupt (8 bit timer)
|
|
TCCR0A=2; // prescaler 8 => 0.5us
|
|
TCNT0=56;
|
|
sei(); // enable interrupts
|
|
}
|
|
|
|
|
|
SoccerBoard::SoccerBoard()
|
|
{
|
|
// PORT A: Digital In (PA0, PA1) //
|
|
DDRA= 255 - (1+2); // all bits output (motor direction) except PA0,PA1 (digital in)
|
|
PORTA=1+2; // set pullups for digital in
|
|
|
|
// PORT B: LEDs (PB0, PB2), Motor enable (PB3-PB7) //
|
|
DDRB = 1+4; // PB0 + PB4 = LEDs -> output
|
|
DDRB |= 8+16+32+64+128; // PB3 - PB7 = Motor enable -> output
|
|
PORTB |= 1+4; // set bits 0 and 2 -> LEDs off
|
|
PORTB &= 255-(8+16+32+64+128); // clear bits 3-7 -> motor disable
|
|
|
|
// PORT C: Power Output //
|
|
DDRC = 255; // direction port D, all bits output
|
|
PORTC = 0; // clear all bits -> power on
|
|
|
|
// PORT D: I2C, USB, CAN //
|
|
DDRD = 0; // all bits input
|
|
PORTD = 1+2; // set bits 0,1 -> I2C pullUps
|
|
|
|
// PORT E: Digital In (PE2 - PE7) //
|
|
DDRE=0; // all bits input
|
|
PORTE=4+8+16+32+64+128; // set pullups for digital in
|
|
|
|
// PORT F: Analog In //
|
|
DDRF=0; // all bits input
|
|
ADCSRA=128; // set A/D enable bit (ADEN)
|
|
|
|
// PORT G: Buttons (PG3, PG4), motor enable (PG2)
|
|
DDRG = BV(PG2); // PG2 output
|
|
PORTG= BV(PG3)+BV(PG4); // set pullups for buttons
|
|
cbi(PORTG,PG2); // clear PG2 -> motor disable
|
|
|
|
initTimer();
|
|
}
|
|
|
|
|
|
void SoccerBoard::ledOn(int i)
|
|
{
|
|
if (i==0) cbi(PORTB, PB0); // clear bit -> LED on
|
|
else if (i==1) cbi(PORTB, PB2); // clear bit -> LED on
|
|
}
|
|
|
|
|
|
void SoccerBoard::ledOff(int i)
|
|
{
|
|
if (i==0) sbi(PORTB, PB0); // set bit -> LED off
|
|
else if (i==1) sbi(PORTB, PB2); // set bit -> LED off
|
|
}
|
|
|
|
void SoccerBoard::ledsOff()
|
|
{
|
|
PORTB|=BV(PB0)+BV(PB2); // set bits -> LEDs off
|
|
}
|
|
|
|
void SoccerBoard::led(int i, bool state)
|
|
{
|
|
if (state) ledOn(i); else ledOff(i);
|
|
}
|
|
|
|
|
|
void SoccerBoard::powerOn(int i)
|
|
{
|
|
if ((i<0) || (i>7)) return;
|
|
cbi(PORTC, i);
|
|
}
|
|
|
|
|
|
void SoccerBoard::powerOff(int i)
|
|
{
|
|
if ((i<0) || (i>7)) return;
|
|
sbi(PORTC, i);
|
|
}
|
|
|
|
|
|
void SoccerBoard::power(int i, bool state)
|
|
{
|
|
if (state) powerOn(i); else powerOff(i);
|
|
}
|
|
|
|
|
|
bool SoccerBoard::button(int i)
|
|
{
|
|
if (i==0) return ( (PING & BV(PG4)) == 0);
|
|
else if (i==1) return ( (PING & BV(PG3)) == 0);
|
|
else return false; // bad approach...
|
|
}
|
|
|
|
|
|
void SoccerBoard::ledMeter(int i,int maxvalue = 256)
|
|
{
|
|
//led(0, (i>100)); //modified by meyma
|
|
//led(1, (i>200));
|
|
|
|
if(i < (maxvalue / 4))
|
|
{
|
|
ledOff(0);
|
|
ledOff(1);
|
|
}
|
|
else if(i < (maxvalue / 2))
|
|
{
|
|
ledOn(0);
|
|
ledOff(1);
|
|
}
|
|
else if(i < ((maxvalue /4) * 3))
|
|
{
|
|
ledOff(0);
|
|
ledOn(1);
|
|
}
|
|
else {
|
|
ledOn(0);
|
|
ledOn(1);
|
|
}
|
|
}
|
|
|
|
void SoccerBoard::motor(int i, int speed)
|
|
{
|
|
if ((i<0) || (i>1)) return;
|
|
|
|
if (i==0) {
|
|
speedMotor0 = speed;
|
|
if (speed>0) sbi(PORTA, 3); // input1 = 1
|
|
else cbi(PORTA, 3); // input1 = 0
|
|
}
|
|
|
|
else if (i==1) {
|
|
speedMotor1 = speed;
|
|
if (speed>0) sbi(PORTA, 2); // input2 = 1
|
|
else cbi(PORTA, 2); // input2 = 0
|
|
}
|
|
|
|
/*else if (i==2) {
|
|
speedMotor2 = speed;
|
|
if (speed>0) sbi(PORTA, 5); // input3 = 1
|
|
else cbi(PORTA, 5); // input3 = 0
|
|
}
|
|
|
|
else if (i==3) {
|
|
speedMotor3 = speed;
|
|
if (speed>0) sbi(PORTA, 4); // input4 = 1
|
|
else cbi(PORTA, 4); // input4 = 0
|
|
}
|
|
|
|
else if (i==4) {
|
|
speedMotor4 = speed;
|
|
if (speed>0) sbi(PORTA, 7); // input5 = 1
|
|
else cbi(PORTA, 7); // input5 = 0
|
|
}
|
|
|
|
else if (i==5) {
|
|
speedMotor5 = speed;
|
|
if (speed>0) sbi(PORTA, 6); // input6 = 1
|
|
else cbi(PORTA, 6); // input6 = 0
|
|
}*/
|
|
}
|
|
|
|
void SoccerBoard::motorsOff()
|
|
{
|
|
motor(0,0);
|
|
motor(1,0);
|
|
motor(2,0);
|
|
motor(3,0);
|
|
motor(4,0);
|
|
motor(5,0);
|
|
}
|
|
|
|
|
|
// return 0-255 //
|
|
int SoccerBoard::analog(int i)
|
|
{
|
|
if ((i<0) || (i>7)) return -1;
|
|
|
|
ADMUX=i; // select analog input and start A/D
|
|
sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
|
|
sbi(ADCSRA, ADSC); // start conversion
|
|
while (ADCSRA & 64); // wait until ADSC is low again
|
|
int value = ADCH; // read 8 bit value fom ADCH
|
|
return value;
|
|
}
|
|
|
|
|
|
bool SoccerBoard::digital(int i)
|
|
{
|
|
if ((i<0) || (i>7)) return false; // bad solution...
|
|
|
|
if (i==0) return (PINA & 1);
|
|
else if (i==1) return (PINA & 2);
|
|
else return (PINE & (1<<i)) ;
|
|
}
|
|
|
|
|
|
void SoccerBoard::waitForButton(int i)
|
|
{
|
|
if ((i<0) || (i>3)) return; // bad solution...
|
|
while (!button(i)) { /* do nothing */ }
|
|
while (button(i)) { /* do nothing */ }
|
|
}
|
|
|
|
void SoccerBoard::beep(int freq) {
|
|
beep(freq, -1);
|
|
}
|
|
|
|
void SoccerBoard::beep(int freq, long msecs) {
|
|
beepTimer = 7*msecs;
|
|
beepFreq = 7000 / freq;
|
|
}
|
|
|
|
void SoccerBoard::beepOff() {
|
|
beepTimer = 0;
|
|
}
|
|
|
|
void SoccerBoard::setTimer(unsigned long msecs) {
|
|
timerVal = 7*msecs;
|
|
}
|
|
|
|
bool SoccerBoard::timer() {
|
|
return (timerVal <= 0);
|
|
}
|
|
|
|
void SoccerBoard::sleep(unsigned long msecs) {
|
|
sleepVal = 7*msecs;
|
|
|
|
while(sleepVal > 0);
|
|
}
|