From 98c623dc8969151c0872a2d102d486f9a735d614 Mon Sep 17 00:00:00 2001 From: neoraider Date: Tue, 3 Apr 2007 12:47:03 +0000 Subject: Neues Rescue-Programm angefangen --- Global.c | 10 ++ Global.h | 41 ++++++++ Navigation.c | 46 +++++++++ Navigation.h | 31 ++++++ Robocup.aps | 1 + Robocup.c | 171 +++++++++++++++++++++++++++++++ Sensors.c | 88 ++++++++++++++++ Sensors.h | 149 +++++++++++++++++++++++++++ qfixSoccerBoard.c | 299 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ qfixSoccerBoard.h | 101 ++++++++++++++++++ 10 files changed, 937 insertions(+) create mode 100644 Global.c create mode 100644 Global.h create mode 100644 Navigation.c create mode 100644 Navigation.h create mode 100644 Robocup.aps create mode 100644 Robocup.c create mode 100644 Sensors.c create mode 100644 Sensors.h create mode 100644 qfixSoccerBoard.c create mode 100644 qfixSoccerBoard.h diff --git a/Global.c b/Global.c new file mode 100644 index 0000000..29fdf79 --- /dev/null +++ b/Global.c @@ -0,0 +1,10 @@ +#include "Global.h" + + +void *operator new(size_t sz) { + return malloc(sz); +} + +void operator delete(void *p) { + free(p); +} diff --git a/Global.h b/Global.h new file mode 100644 index 0000000..1cf4ccc --- /dev/null +++ b/Global.h @@ -0,0 +1,41 @@ +#ifndef _ROBOCUP_GLOBAL_H_ +#define _ROBOCUP_GLOBAL_H_ + +#include + + +// Geschwindigkeiten +#define DEFAULT_SPEED 255 +#define SPEED_RUECK 50 + +// Status +#define STATUS_OK 0 +#define STATUS_WEISS 1 +#define STATUS_HINDERNIS_L 2 // Hindernis vorne links +#define STATUS_HINDERNIS_R 3 // Hindernis vorne rechts +#define STATUS_HINDERNIS_WAR_L 4 // Hindernis war vorne links +#define STATUS_HINDERNIS_WAR_R 5 // Hindernis war vorne rechts +#define STATUS_HINDERNIS_VOR_L 6 // Hindernis ist gleich vorne links +#define STATUS_HINDERNIS_VOR_R 7 // Hindernis ist gleich vorne rechts + +// Sensoren +#define KALI_MINDIFF 80 // minimale Abweichung s/w +#define KALI_WEISSMAXDIFF 5 // maximale Abweichung weiss1/weiss2 + +#define HINDERNIS_ERKENNUNGSWERT 155 // 100 +#define HINDERNIS_ERKENNUNGSWERT_SEITL 80 +#define HINDERNIS_SOLLWERT 90 +#define HINDERNIS_ABW 5 // 10 +#define HINDERNIS_SLEEP 100 + +// Opfer +#define OPFER_TIMEOUT 500 +#define OPFER_WAIT 2000 +#define OPFER_TIMER 1000 +#define OPFER_BEEP 440 + + +void *operator new(size_t sz); +void operator delete(void *p); + +#endif diff --git a/Navigation.c b/Navigation.c new file mode 100644 index 0000000..9a3f69d --- /dev/null +++ b/Navigation.c @@ -0,0 +1,46 @@ +#include "Navigation.h" + + +void Navigation::start() { + int r = m_speed, l = m_speed; + + + switch(m_richtung) + { + case -2: + l = 0; + break; + case -1: + l /= 4; + break; + case 1: + r /= 4; + break; + case 2: + r = 0; + } + + m_board->motor(MOTOR_PORT_LINKS, l); + m_board->motor(MOTOR_PORT_RECHTS, r); + + m_gestartet = true; +} + +void Navigation::stop() { + m_board->motor(MOTOR_PORT_LINKS, 0); + m_board->motor(MOTOR_PORT_RECHTS, 0); + + m_gestartet = false; +} + +void Navigation::setSpeed(int speed) { + m_speed = (speed > 255) ? 255 : (speed < -255) ? -255 : speed; + + if(m_gestartet) start(); +} + +void Navigation::setRichtung(int richtung) { + m_richtung = richtung; + + if(m_gestartet) start(); +} diff --git a/Navigation.h b/Navigation.h new file mode 100644 index 0000000..3f3f1ec --- /dev/null +++ b/Navigation.h @@ -0,0 +1,31 @@ +#include "qfixSoccerBoard.h" +#include "Global.h" + + +// Physikalische Motorausgänge +#define MOTOR_PORT_LINKS 0 +#define MOTOR_PORT_RECHTS 1 + + +class Navigation { + private: + SoccerBoard *m_board; + bool m_gestartet; + int m_speed; + int m_richtung; + public: + Navigation(SoccerBoard *board) { + m_gestartet = false; + m_speed = DEFAULT_SPEED; + m_richtung = 0; + + m_board = board; + } + + int getRichtung() {return m_richtung;} + + void start(); + void stop(); + void setSpeed(int speed); + void setRichtung(int richtung); +}; diff --git a/Robocup.aps b/Robocup.aps new file mode 100644 index 0000000..fff9965 --- /dev/null +++ b/Robocup.aps @@ -0,0 +1 @@ +Robocup16-Feb-2007 18:11:3722-Feb-2007 17:07:39241016-Feb-2007 18:11:3744, 12, 0, 491AVR GCCdefault\Robocup.elfC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\JTAGICE mkIIATmega128.xmlfalseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31Auto000Robocup.cqfixSoccerBoard.cSensors.cGlobal.cNavigation.cqfixSoccerBoard.hSensors.hGlobal.hNavigation.hdefaultNOat90can128100Robocup.elfdefault\1C:\Programme\WinAVR\include\-Wall -gdwarf-2 -xc++ -DF_CPU=16000000UL -O0 -fsigned-chardefault1C:\Programme\WinAVR\bin\avr-gcc.exeC:\Programme\WinAVR\utils\bin\make.exeC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\qfixSoccerBoard.hC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Sensors.hC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Global.hC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Navigation.hC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Robocup.cC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\qfixSoccerBoard.cC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Sensors.cC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Global.cC:\Dokumente und Einstellungen\Emily\Desktop\Robocup\Navigation.c00000Robocup.c100001Sensors.c100002Sensors.h100003Global.h100004qfixSoccerBoard.h100005qfixSoccerBoard.c100006Global.c100007Navigation.c100008Navigation.h1303 73 1025 46271 0303 72 1025 46177 52Maximized294 66 1016 45816 43364 183 990 42520 23386 212 1012 45494 0297 68 1019 460294 0320 125 946 3670 0342 154 968 39614 0364 183 990 42514 0 diff --git a/Robocup.c b/Robocup.c new file mode 100644 index 0000000..08258df --- /dev/null +++ b/Robocup.c @@ -0,0 +1,171 @@ +#include "Global.h" +#include "Navigation.h" +#include "Sensors.h" +#include "qfixSoccerBoard.h" + + + +void OpferSignal(SoccerBoard *board) { + board->ledOn(0); + board->beep(OPFER_BEEP); + board->sleep(OPFER_WAIT); + board->ledOff(0); + board->beepOff(); +} + + +int main() { + int status = STATUS_OK; + int linie = 0, last_linie = 0; + SoccerBoard board; + Sensors sensors(&board); + Navigation nav(&board); + + do { + sensors.update(); + } while(!sensors.kalibrieren()); + + board.ledOn(0); + board.waitForButton(0); + board.ledOff(0); + board.sleep(4000); + + nav.start(); + + while(true) { + sensors.update(); + sensors.auswerten(); + + last_linie = linie; + linie = sensors.getLinie(); + + switch(status) { + case STATUS_OK: + if(linie == 0) sensors.kalibrieren(); + + if(sensors.getOpfer() && board.timer()) { + nav.stop(); + OpferSignal(&board); + nav.start(); + board.setTimer(OPFER_TIMER); + break; + } + + if(sensors.hindernisV()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0); + board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + + if(sensors.getDsVL() > sensors.getDsVR()) { + // Hindernis eher links -> wir fahren rechts rum + nav.setRichtung(2); + status = STATUS_HINDERNIS_VOR_L; // status variable setzen + } + else { + // Hindernis eher rechts -> wir fahren links rum + nav.setRichtung(-2); + status = STATUS_HINDERNIS_VOR_R; // status variable setzen + } + + break; + } + + switch(linie) { + case LINIE_FEHLER: + break; + case LINIE_WEISS: + /*if(abs(last_linie) == 2) break; + + nav.setRichtung(0); + status = STATUS_WEISS;*/ + + break; + default: + nav.setRichtung(linie); + } + + break; + case STATUS_WEISS: + if(sensors.getOpfer() && board.timer()) { + nav.stop(); + OpferSignal(&board); + nav.start(); + board.setTimer(OPFER_TIMER); + continue; + } + + if(linie != LINIE_WEISS) status = STATUS_OK; + break; + case STATUS_HINDERNIS_VOR_L: + if(sensors.hindernisL() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_L; + break; + case STATUS_HINDERNIS_VOR_R: + if(sensors.hindernisR() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_R; + break; + case STATUS_HINDERNIS_L: + if(sensors.hindernisVL()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0); + board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + nav.setRichtung(2); + status = STATUS_HINDERNIS_VOR_L; + } + else if(linie == LINIE_WEISS) { + if(sensors.getDsL() > HINDERNIS_SOLLWERT + HINDERNIS_ABW) + nav.setRichtung(2); + else if(sensors.getDsL() < HINDERNIS_SOLLWERT - HINDERNIS_ABW) + nav.setRichtung(-2); + else + nav.setRichtung(0); + } + else { + status = STATUS_HINDERNIS_WAR_L; + nav.setRichtung(0); + } + break; + case STATUS_HINDERNIS_R: + if(sensors.hindernisVR()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0); + board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + nav.setRichtung(-2); + status = STATUS_HINDERNIS_VOR_R; + } + else if(linie == LINIE_WEISS) { + if(sensors.getDsR() > HINDERNIS_SOLLWERT + HINDERNIS_ABW) + nav.setRichtung(-2); + else if(sensors.getDsR() < HINDERNIS_SOLLWERT - HINDERNIS_ABW) + nav.setRichtung(2); + else + nav.setRichtung(0); + } + else { + status = STATUS_HINDERNIS_WAR_R; + nav.setRichtung(0); + } + break; + case STATUS_HINDERNIS_WAR_L: + if(linie != LINIE_WEISS) break; + + board.sleep(40); + status = STATUS_OK; + nav.setRichtung(2); + + break; + case STATUS_HINDERNIS_WAR_R: + if(linie != LINIE_WEISS) break; + + board.sleep(40); + status = STATUS_OK; + nav.setRichtung(-2); + } + } + + return 0; +} diff --git a/Sensors.c b/Sensors.c new file mode 100644 index 0000000..ffbdb96 --- /dev/null +++ b/Sensors.c @@ -0,0 +1,88 @@ +#include "qfixSoccerBoard.h" +#include "Global.h" +#include "Sensors.h" + + +void Sensor::update() { + m_board->analog(m_port); + m_board->analog(m_port); + m_wert = m_board->analog(m_port); +} + + + +void Sensors::update() { + m_lsLinks->update(); + m_lsMitte->update(); + m_lsRechts->update(); + + m_dsL->update(); + m_dsVL->update(); + m_dsV->update(); + m_dsVR->update(); + m_dsR->update(); +} + +bool Sensors::kalibrieren() { + int links = m_lsLinks->getWert(); + int mitte = m_lsMitte->getWert(); + int rechts = m_lsRechts->getWert(); + + int weiss = (rechts + links) / 2; + int diff = mitte - weiss; + + + if(links > mitte || rechts > mitte) return false; + + if(diff < KALI_MINDIFF) return false; + if(abs(rechts - links) > KALI_WEISSMAXDIFF) return false; + + if(!m_kalibriert) { + m_gw_sg = mitte - diff/8; + m_gw_gw = weiss + diff/8; + m_gw_ws = weiss - diff/8; + m_gw_sw = weiss + diff/2; + + m_kalibriert = true; + } + else { + m_gw_sg = m_gw_sg/2 + mitte/2 - diff/16; + m_gw_gw = m_gw_gw/2 + weiss/2 + diff/16; + m_gw_ws = m_gw_ws/2 + weiss/2 - diff/16; + m_gw_sw = m_gw_sw/2 + weiss/2 + diff/4; + } + + return true; +} + +void Sensors::auswerten() { + // Linie + m_liniePos = int(SW_SCHWARZ(m_lsRechts)) - int(SW_SCHWARZ(m_lsLinks)); //Linie links oder rechts -> -1 oder 1, beides oder keines -> 0 + m_liniePos *= (1+int(SW_WEISS(m_lsMitte))); //Kurvenstärke bestimmen + + if((!m_liniePos) && SW_WEISS(m_lsMitte)) //alles weiß? + m_liniePos = LINIE_WEISS; + + if(SW_SCHWARZ(m_lsLinks) && SW_SCHWARZ(m_lsRechts)) // beide äußeren schwarz? -> fehler + m_liniePos = LINIE_FEHLER; + + + // Opfer + if(m_sLinks) m_sLinks--; + if(m_sMitte) m_sMitte--; + if(m_sRechts) m_sRechts--; + + if(m_gLinks) m_gLinks--; + if(m_gMitte) m_gMitte--; + if(m_gRechts) m_gRechts--; + + if(C_SILBER(m_lsLinks)) m_sLinks = OPFER_TIMEOUT; + if(C_SILBER(m_lsMitte)) m_sMitte = OPFER_TIMEOUT; + if(C_SILBER(m_lsRechts)) m_sRechts = OPFER_TIMEOUT; + + if(C_GRUEN(m_lsLinks)) m_gLinks = OPFER_TIMEOUT; + if(C_GRUEN(m_lsMitte)) m_gMitte = OPFER_TIMEOUT; + if(C_GRUEN(m_lsRechts)) m_gRechts = OPFER_TIMEOUT; +} + + diff --git a/Sensors.h b/Sensors.h new file mode 100644 index 0000000..54e3d4b --- /dev/null +++ b/Sensors.h @@ -0,0 +1,149 @@ +#ifndef _ROBOCUP_SENSORS_H_ +#define _ROBOCUP_SENSORS_H_ + +#include "qfixSoccerBoard.h" + +#define SENSOR_PORT_LINIE_LINKS 0 +#define SENSOR_PORT_LINIE_MITTE 1 +#define SENSOR_PORT_LINIE_RECHTS 2 + +#define SENSOR_PORT_D_L 3 +#define SENSOR_PORT_D_VL 4 +#define SENSOR_PORT_D_V 5 +#define SENSOR_PORT_D_VR 6 +#define SENSOR_PORT_D_R 7 + +#define LINIE_WEISS 70 +#define LINIE_FEHLER 71 + +#define SW_SCHWARZ(sensor) (sensor->getWert() > m_gw_sw) +#define SW_WEISS(sensor) (sensor->getWert() <= m_gw_sw) + +#define C_SCHWARZ(sensor) (sensor->getWert() > m_gw_sg) +#define C_GRUEN(sensor) (sensor->getWert() > m_gw_gw && sensor->getWert() <= m_gw_sg) +#define C_WEISS(sensor) (sensor->getWert() > m_gw_ws && sensor->getWert() <= m_gw_gw) +#define C_SILBER(sensor) (sensor->getWert() <= m_gw_ws) + + +class Sensor { + private: + SoccerBoard *m_board; + int m_port; + int m_wert; + + public: + Sensor(SoccerBoard *board, int port) { + m_board = board; + m_port = port; + } + + void update(); + + int getWert() { + return m_wert; + } +}; + + +class Sensors { + private: + SoccerBoard *m_board; + + bool m_kalibriert; + + Sensor *m_lsLinks, *m_lsMitte, *m_lsRechts; + int m_sLinks, m_sMitte, m_sRechts; + int m_gLinks, m_gMitte, m_gRechts; + int m_liniePos; + + int m_gw_sg; // Grenzwert schwarz/grün + int m_gw_gw; // Grenzwert grün/weiß + int m_gw_ws; // Grenzwert weiß/silber + int m_gw_sw; // Grenzwert schwarz/weiß + + Sensor *m_dsL, *m_dsVL, *m_dsV, *m_dsVR, *m_dsR; + public: + Sensors(SoccerBoard *board) { + m_board = board; + + m_kalibriert = false; + + m_lsLinks = new Sensor(board, SENSOR_PORT_LINIE_LINKS); + m_lsMitte = new Sensor(board, SENSOR_PORT_LINIE_MITTE); + m_lsRechts = new Sensor(board, SENSOR_PORT_LINIE_RECHTS); + + m_dsL = new Sensor(board, SENSOR_PORT_D_L); + m_dsVL = new Sensor(board, SENSOR_PORT_D_VL); + m_dsV = new Sensor(board, SENSOR_PORT_D_V); + m_dsVR = new Sensor(board, SENSOR_PORT_D_VR); + m_dsR = new Sensor(board, SENSOR_PORT_D_R); + + m_sLinks = m_sMitte = m_sRechts = 0; + m_gLinks = m_gMitte = m_gRechts = 0; + + m_liniePos = 0; + } + + ~Sensors() { + delete m_lsLinks; + delete m_lsMitte; + delete m_lsRechts; + + delete m_dsL; + delete m_dsVL; + delete m_dsV; + delete m_dsVR; + delete m_dsR; + } + + int getLinie() {return m_liniePos;} + bool getOpfer() { + return /*(m_sLinks && m_sMitte && m_sRechts) || */(m_gLinks && m_gMitte && m_gRechts); + } + + bool hindernisL() { + return (m_dsL->getWert() > HINDERNIS_ERKENNUNGSWERT_SEITL); + } + + bool hindernisVL() { + return (m_dsVL->getWert() > HINDERNIS_ERKENNUNGSWERT); + } + + bool hindernisV() { + return (m_dsV->getWert() > HINDERNIS_ERKENNUNGSWERT); + } + + bool hindernisVR() { + return (m_dsVR->getWert() > HINDERNIS_ERKENNUNGSWERT); + } + + bool hindernisR() { + return m_dsR->getWert(); + } + + int getDsL() { + return m_dsL->getWert(); + } + + int getDsVL() { + return m_dsVL->getWert(); + } + + int getDsV() { + return m_dsV->getWert(); + } + + int getDsVR() { + return m_dsVR->getWert(); + } + + int getDsR() { + return m_dsR->getWert(); + } + + void update(); + void auswerten(); + bool kalibrieren(); +}; + +#endif 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<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); +} diff --git a/qfixSoccerBoard.h b/qfixSoccerBoard.h new file mode 100644 index 0000000..9278504 --- /dev/null +++ b/qfixSoccerBoard.h @@ -0,0 +1,101 @@ +#include + + +#ifndef qfixSoccerBoard_h +#define qfixSoccerBoard_h + + +/** +* \class SoccerBoard +* \brief Represents the controller board "SoccerBoard". +* \author Stefan Enderle +* +* The class SoccerBoard represents the +* physical SoccerBoard with all its inputs and outputs. +* With this class it is possible to drive the motors, +* put on LEDs, check the buttons and get data from the +* analog and digital inputs. +*/ + +class SoccerBoard +{ + +public: + /** Constructor for the SoccerBoard class. + */ + SoccerBoard(); + + /** Puts on LED i. i must be 0 or 1. + */ + void ledOn(int i); + + /** Puts off LED i. i must be 0 or 1. + */ + void ledOff(int i); + + /** Puts off all LEDs. + */ + void ledsOff(); + + /** Puts LED i on if state is true, else off. i must be 0 or 1. + */ + void led(int i, bool state); + + /** Puts the power output i on + */ + void powerOn(int i); + + /** Puts the power output i off + */ + void powerOff(int i); + + /** Puts the power output i on if state is true, else off. + */ + void power(int i, bool state); + + /** Checks the state of button i. If it is pressed, true is returned, + * else false. + */ + bool button(int i); + + /** Uses the four LEDs on the board to display the value i + * with 0 <= i <= 255 + */ + void ledMeter(int i,int maxvalue); + + /** Sets motor i to the given speed. -255 <= speed <= 255. + */ + void motor(int i, int speed); + + void stop(int i); + + /** Puts off all motors. + */ + void motorsOff(); + + /** returns the value of the analog port i. 0 <= value <= 255. + */ + int analog(int i); + + /** returns true if the digital port is logical high, else false. + */ + bool digital(int i); + + /** Waits until button i is pressed and released again. + */ + void waitForButton(int i); + + void beep(int freq); + void beep(int freq, long msecs); + void beepOff(); + + void setTimer(unsigned long msecs); + bool timer(); + + void sleep(unsigned long msecs); +}; + + +void initTimer(); + +#endif -- cgit v1.2.3