Neues Rescue-Programm angefangen
This commit is contained in:
neoraider
commit
98c623dc89
10 changed files with 937 additions and 0 deletions
10
Global.c
Normal file
10
Global.c
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
#include "Global.h"
|
||||||
|
|
||||||
|
|
||||||
|
void *operator new(size_t sz) {
|
||||||
|
return malloc(sz);
|
||||||
|
}
|
||||||
|
|
||||||
|
void operator delete(void *p) {
|
||||||
|
free(p);
|
||||||
|
}
|
||||||
41
Global.h
Normal file
41
Global.h
Normal file
|
|
@ -0,0 +1,41 @@
|
||||||
|
#ifndef _ROBOCUP_GLOBAL_H_
|
||||||
|
#define _ROBOCUP_GLOBAL_H_
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
|
||||||
|
// 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
|
||||||
46
Navigation.c
Normal file
46
Navigation.c
Normal file
|
|
@ -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();
|
||||||
|
}
|
||||||
31
Navigation.h
Normal file
31
Navigation.h
Normal file
|
|
@ -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);
|
||||||
|
};
|
||||||
1
Robocup.aps
Normal file
1
Robocup.aps
Normal file
File diff suppressed because one or more lines are too long
171
Robocup.c
Normal file
171
Robocup.c
Normal file
|
|
@ -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;
|
||||||
|
}
|
||||||
88
Sensors.c
Normal file
88
Sensors.c
Normal file
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
149
Sensors.h
Normal file
149
Sensors.h
Normal file
|
|
@ -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
|
||||||
299
qfixSoccerBoard.c
Normal file
299
qfixSoccerBoard.c
Normal file
|
|
@ -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);
|
||||||
|
}
|
||||||
101
qfixSoccerBoard.h
Normal file
101
qfixSoccerBoard.h
Normal file
|
|
@ -0,0 +1,101 @@
|
||||||
|
#include <qfix/qfix.h>
|
||||||
|
|
||||||
|
|
||||||
|
#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
|
||||||
Reference in a new issue