Neues Rescue-Programm angefangenHEADmaster

1 files changed, 299 insertions, 0 deletions

diff --git a/qfixSoccerBoard.c b/qfixSoccerBoard.c
new file mode 100644
index 0000000..06bd3b8
--- /dev/null
+++ b/qfixSoccerBoard.c
@@ -0,0 +1,299 @@
+#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);

+}