Compare commits
No commits in common. "main" and "old" have entirely different histories.
35 changed files with 937 additions and 1022 deletions
|
|
@ -1,10 +1,10 @@
|
|||
#include <stdlib.h>
|
||||
|
||||
|
||||
void *operator new(size_t sz) {
|
||||
return malloc(sz);
|
||||
}
|
||||
|
||||
void operator delete(void *p) {
|
||||
free(p);
|
||||
}
|
||||
#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
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
#include "LineSensor.h"
|
||||
|
||||
#include "adc.h"
|
||||
|
||||
|
||||
LineSensor::LineSensor(int id) {
|
||||
this->id = id;
|
||||
|
||||
updateValue();
|
||||
}
|
||||
|
||||
float LineSensor::updateValue() {
|
||||
value = getADCValue(id)/1024.0;
|
||||
|
||||
return value;
|
||||
}
|
||||
14
LineSensor.h
14
LineSensor.h
|
|
@ -1,14 +0,0 @@
|
|||
#ifndef _ROBOCUP_LINESENSOR_H_
|
||||
#define _ROBOCUP_LINESENSOR_H_
|
||||
|
||||
class LineSensor {
|
||||
private:
|
||||
int id;
|
||||
float value;
|
||||
public:
|
||||
LineSensor(int id);
|
||||
float updateValue();
|
||||
float getValue() {return value;}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
#include "LineSensorArray.h"
|
||||
|
||||
LineSensorArray::LineSensorArray(LineSensor *sensors[5])
|
||||
{
|
||||
for(int i = 0; i < 5; i++)
|
||||
this->sensors[i] = sensors[i];
|
||||
|
||||
line = 0.0;
|
||||
sharpness = 0.0;
|
||||
gw = 0.0;
|
||||
|
||||
min_diff = 0.0;
|
||||
max_white_diff = 0.0;
|
||||
}
|
||||
|
||||
LineSensorArray::LineSensorArray(LineSensor *sensor1, LineSensor *sensor2, LineSensor *sensor3, LineSensor *sensor4, LineSensor *sensor5)
|
||||
{
|
||||
sensors[0] = sensor1;
|
||||
sensors[1] = sensor2;
|
||||
sensors[2] = sensor3;
|
||||
sensors[3] = sensor4;
|
||||
sensors[4] = sensor5;
|
||||
|
||||
line = 0.0;
|
||||
sharpness = 0.0;
|
||||
gw = 0.0;
|
||||
|
||||
min_diff = 0.0;
|
||||
max_white_diff = 0.0;
|
||||
}
|
||||
|
||||
void LineSensorArray::update() {
|
||||
int values[5];
|
||||
int nBlack = 0;
|
||||
bool strange = false;
|
||||
|
||||
|
||||
for(int i = 0; i < 5; i++) {
|
||||
sensors[i]->updateValue();
|
||||
if((values[i] = int(isSensorBlack(i))) == 1)
|
||||
nBlack++;
|
||||
else if(nBlack > 0)
|
||||
strange = true;
|
||||
}
|
||||
|
||||
if(nBlack == 0) {
|
||||
line = 0.0;
|
||||
sharpness = 0.0;
|
||||
}
|
||||
else {
|
||||
line = (-2*values[0]-1*values[1]+values[3]+2*values[4])/(float)nBlack;
|
||||
|
||||
if(strange) sharpness = -1.0;
|
||||
else sharpness = 1/(float)nBlack;
|
||||
}
|
||||
}
|
||||
|
||||
bool LineSensorArray::calibrate() {
|
||||
float white = (sensors[1]->getValue() + sensors[3]->getValue())/2;
|
||||
|
||||
if((sensors[2]->getValue() - white) < min_diff) return false;
|
||||
if(ABS(sensors[1]->getValue() - sensors[3]->getValue()) > max_white_diff) return false;
|
||||
|
||||
if(gw == 0.0)
|
||||
gw = sensors[2]->getValue()/2 + white/2;
|
||||
else
|
||||
gw = sensors[2]->getValue()/4 + white/4 + gw/2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
#ifndef _ROBOCUP_LINESENSORARRAY_H_
|
||||
#define _ROBOCUP_LINESENSORARRAY_H_
|
||||
|
||||
#include "LineSensor.h"
|
||||
#include "util.h"
|
||||
|
||||
|
||||
class LineSensorArray
|
||||
{
|
||||
private:
|
||||
LineSensor *sensors[5];
|
||||
|
||||
float line;
|
||||
float sharpness;
|
||||
float gw;
|
||||
|
||||
float min_diff;
|
||||
float max_white_diff;
|
||||
public:
|
||||
LineSensorArray(LineSensor *sensors[5]);
|
||||
LineSensorArray(LineSensor *sensor1, LineSensor *sensor2, LineSensor *sensor3, LineSensor *sensor4, LineSensor *sensor5);
|
||||
|
||||
void update();
|
||||
bool calibrate();
|
||||
|
||||
bool isSensorBlack(int i) {return sensors[CLAMP(0, i, 4)]->getValue() > gw;}
|
||||
bool isSensorWhite(int i) {return sensors[CLAMP(0, i, 4)]->getValue() <= gw;}
|
||||
|
||||
float getLine() {return line;}
|
||||
float getSharpness() {return sharpness;}
|
||||
|
||||
float getMinimumDifference() {return min_diff;}
|
||||
void setMinimumDifference(float min_diff) {this->min_diff = min_diff;}
|
||||
|
||||
float getMaximumWhiteDifference() {return max_white_diff;}
|
||||
void setMaximumWhiteDifference(float max_white_diff) {this->max_white_diff = max_white_diff;}
|
||||
};
|
||||
|
||||
#endif
|
||||
27
Makefile
27
Makefile
|
|
@ -1,27 +0,0 @@
|
|||
FILES := $(patsubst %.cpp,%,$(wildcard *.cpp))
|
||||
|
||||
|
||||
all: robocup.elf;
|
||||
|
||||
robocup.hex: robocup.elf
|
||||
avr-objcopy -O ihex -R .eeprom $< $@
|
||||
|
||||
robocup.elf: $(FILES:%=%.o)
|
||||
avr-g++ -mmcu=atmega32 -Os -o $@ $^
|
||||
|
||||
%.o: %.cpp
|
||||
avr-g++ -c -mmcu=atmega32 -Os -o $@ $<
|
||||
|
||||
%.d: %.cpp
|
||||
@avr-g++ -MM -mmcu=atmega32 -Os -MF $@ -MT $(patsubst %.cpp,%.o,$<) -MT $@ $<
|
||||
|
||||
program: robocup.hex
|
||||
sudo avrdude -P usb -c avrisp2 -p m32 -U flash:w:$<
|
||||
|
||||
clean:
|
||||
rm -f robocup.hex robocup.elf $(FILES:%=%.o) $(FILES:%=%.d)
|
||||
|
||||
.PHONY: clean
|
||||
|
||||
|
||||
-include $(FILES:%=%.d)
|
||||
29
Motor.cpp
29
Motor.cpp
|
|
@ -1,29 +0,0 @@
|
|||
#include "Motor.h"
|
||||
|
||||
#include "util.h"
|
||||
|
||||
|
||||
Motor::Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask) {
|
||||
this->port = port;
|
||||
this->pwmPort = pwmPort;
|
||||
this->fwdMask = fwdMask;
|
||||
this->revMask = revMask;
|
||||
|
||||
setSpeed(0);
|
||||
}
|
||||
|
||||
void Motor::setSpeed(int speed) {
|
||||
this->speed = CLAMP(-255, speed, 255);
|
||||
|
||||
if(speed > 0) {
|
||||
*port &= ~revMask;
|
||||
*port |= fwdMask;
|
||||
}
|
||||
else if(speed < 0) {
|
||||
*port &= ~fwdMask;
|
||||
*port |= revMask;
|
||||
}
|
||||
else *port |= fwdMask|revMask;
|
||||
|
||||
*pwmPort = ABS(this->speed);
|
||||
}
|
||||
22
Motor.h
22
Motor.h
|
|
@ -1,22 +0,0 @@
|
|||
#ifndef _ROBOCUP_MOTOR_H_
|
||||
#define _ROBOCUP_MOTOR_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
class Motor {
|
||||
private:
|
||||
volatile uint8_t *port;
|
||||
volatile uint8_t *pwmPort;
|
||||
uint8_t fwdMask;
|
||||
uint8_t revMask;
|
||||
|
||||
int speed;
|
||||
public:
|
||||
Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask);
|
||||
|
||||
void setSpeed(int speed);
|
||||
int getSpeed() {return speed;}
|
||||
};
|
||||
|
||||
#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();
|
||||
}
|
||||
|
|
@ -1,48 +0,0 @@
|
|||
#include "Navigation.h"
|
||||
#include "hardware.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
|
||||
Navigation::Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3) {
|
||||
motors[0] = m1;
|
||||
motorPos[0] = p1;
|
||||
motors[1] = m2;
|
||||
motorPos[1] = p2;
|
||||
motors[2] = m3;
|
||||
motorPos[2] = p3;
|
||||
|
||||
speed = direction = spin = 0.0;
|
||||
|
||||
update();
|
||||
}
|
||||
|
||||
void Navigation::update() {
|
||||
float s1, s2, s3;
|
||||
|
||||
s1 = s2 = s3 = spin;
|
||||
|
||||
s1 += speed*sin((direction-motorPos[0])*M_PI/180);
|
||||
s2 += speed*sin((direction-motorPos[1])*M_PI/180);
|
||||
s3 += speed*sin((direction-motorPos[2])*M_PI/180);
|
||||
|
||||
if(ABS(s1) > 1.0) {
|
||||
s1 /= ABS(s1);
|
||||
s2 /= ABS(s1);
|
||||
s3 /= ABS(s1);
|
||||
}
|
||||
if(ABS(s2) > 1.0) {
|
||||
s1 /= ABS(s2);
|
||||
s2 /= ABS(s2);
|
||||
s3 /= ABS(s2);
|
||||
}
|
||||
if(ABS(s3) > 1.0) {
|
||||
s1 /= ABS(s3);
|
||||
s2 /= ABS(s3);
|
||||
s3 /= ABS(s3);
|
||||
}
|
||||
|
||||
motors[0]->setSpeed(int(s1*255));
|
||||
motors[1]->setSpeed(int(s2*255));
|
||||
motors[2]->setSpeed(int(s3*255));
|
||||
}
|
||||
62
Navigation.h
62
Navigation.h
|
|
@ -1,31 +1,31 @@
|
|||
#ifndef _ROBOCUP_NAVIGATION_H_
|
||||
#define _ROBOCUP_NAVIGATION_H_
|
||||
|
||||
#include "Motor.h"
|
||||
#include "util.h"
|
||||
|
||||
|
||||
class Navigation {
|
||||
private:
|
||||
Motor *motors[3];
|
||||
float motorPos[3];
|
||||
|
||||
float speed;
|
||||
float direction;
|
||||
float spin;
|
||||
|
||||
void update();
|
||||
public:
|
||||
Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3);
|
||||
|
||||
void setSpeed(float speed) {this->speed = CLAMP(0.0, speed, 1.0); update();}
|
||||
float getSpeed() {return speed;}
|
||||
|
||||
void setDirection(float direction) {this->direction = direction; update();}
|
||||
float getDirection() {return direction;}
|
||||
|
||||
void setSpin(float spin) {this->spin = CLAMP(-1.0, spin, 1.0); update();}
|
||||
float getSpin() {return spin;}
|
||||
};
|
||||
|
||||
#endif
|
||||
#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
|
||||
72
Srf10.cpp
72
Srf10.cpp
|
|
@ -1,72 +0,0 @@
|
|||
#include "Srf10.h"
|
||||
|
||||
#include "i2c.h"
|
||||
#include "timer.h"
|
||||
|
||||
|
||||
Srf10::Srf10(uint8_t id)
|
||||
{
|
||||
this->id = id;
|
||||
|
||||
gain = 16;
|
||||
range = 255;
|
||||
|
||||
unit = Inches;
|
||||
|
||||
distance = 0;
|
||||
has_distance = false;
|
||||
|
||||
firmware = readFirmware();
|
||||
}
|
||||
|
||||
int Srf10::readFirmware() {
|
||||
uint8_t byte;
|
||||
|
||||
if(!I2CSendByte(id, 0)) return -1;
|
||||
if(!I2CRecvByte(id, &byte)) return -1;
|
||||
|
||||
return byte;
|
||||
}
|
||||
|
||||
bool Srf10::setGain(uint8_t gain) {
|
||||
uint8_t data[2] = {1, gain};
|
||||
|
||||
if(!I2CSend(id, data, 2)) return false;
|
||||
|
||||
this->gain = gain;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Srf10::setRange(uint8_t range) {
|
||||
uint8_t data[2] = {2, range};
|
||||
|
||||
if(!I2CSend(id, data, 2)) return false;
|
||||
|
||||
this->range = range;
|
||||
return true;
|
||||
}
|
||||
|
||||
long Srf10::updateDistance() {
|
||||
uint8_t data[2] = {0, unit};
|
||||
uint16_t d;
|
||||
|
||||
|
||||
if(!I2CSend(id, data, 2)) return -1;
|
||||
|
||||
do {
|
||||
sleep(1);
|
||||
} while(readFirmware() == 0xFF);
|
||||
|
||||
if(!I2CSendByte(id, 2)) return -1;
|
||||
if(I2CRecv(id, data, 2) < 2) return -1;
|
||||
|
||||
d = (((uint16_t)data[0]) << 8) | data[1];
|
||||
|
||||
if(d == 0) has_distance = false;
|
||||
else {
|
||||
has_distance = true;
|
||||
distance = d;
|
||||
}
|
||||
|
||||
return d;
|
||||
}
|
||||
46
Srf10.h
46
Srf10.h
|
|
@ -1,46 +0,0 @@
|
|||
#ifndef _ROBOCUP_SRF10_H_
|
||||
#define _ROBOCUP_SRF10_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
enum Srf10Units {
|
||||
Inches = 0x50, Centimeters = 0x51, Microseconds = 0x52
|
||||
};
|
||||
|
||||
class Srf10
|
||||
{
|
||||
private:
|
||||
uint8_t id;
|
||||
|
||||
uint8_t gain;
|
||||
uint8_t range;
|
||||
|
||||
int firmware;
|
||||
|
||||
long distance;
|
||||
bool has_distance;
|
||||
|
||||
Srf10Units unit;
|
||||
|
||||
int readFirmware();
|
||||
public:
|
||||
Srf10(uint8_t id);
|
||||
|
||||
uint8_t getGain() {return gain;}
|
||||
bool setGain(uint8_t gain);
|
||||
|
||||
uint8_t getRange() {return range;}
|
||||
bool setRange(uint8_t range);
|
||||
|
||||
Srf10Units getUnit() {return unit;}
|
||||
void setUnit(Srf10Units unit) {this->unit = unit;}
|
||||
|
||||
int getFirmware() {return firmware;}
|
||||
|
||||
long updateDistance();
|
||||
long getDistance() {return distance;}
|
||||
bool hasDistance() {return has_distance;}
|
||||
};
|
||||
|
||||
#endif
|
||||
28
Tcs230.cpp
28
Tcs230.cpp
|
|
@ -1,28 +0,0 @@
|
|||
#include "Tcs230.h"
|
||||
#include "avr.h"
|
||||
|
||||
|
||||
Tcs230::Tcs230()
|
||||
{
|
||||
enabled = false;
|
||||
color = Red;
|
||||
scaling = Off;
|
||||
}
|
||||
|
||||
void Tcs230::setEnabled(bool enabled) {
|
||||
this->enabled = enabled;
|
||||
|
||||
PORTC = (PORTC&~0x40)|(enabled?0x40:0);
|
||||
}
|
||||
|
||||
void Tcs230::setColor(Tcs230Colors color) {
|
||||
this->color = color;
|
||||
|
||||
PORTC = (PORTC&~0x0C)|((color<<2)&0x0C);
|
||||
}
|
||||
|
||||
void Tcs230::setScaling(Tcs230Scalings scaling) {
|
||||
this->scaling = scaling;
|
||||
|
||||
PORTC = (PORTC&~0x30)|((scaling<<4)&0x30);
|
||||
}
|
||||
31
Tcs230.h
31
Tcs230.h
|
|
@ -1,31 +0,0 @@
|
|||
#ifndef _ROBOCUP_TCS230_H_
|
||||
#define _ROBOCUP_TCS230_H_
|
||||
|
||||
enum Tcs230Colors {
|
||||
Red = 0, Blue = 1, Clear = 2, Green = 3
|
||||
};
|
||||
|
||||
enum Tcs230Scalings {
|
||||
Off = 0, TwoPercent = 1, TwentyPercent = 2, Full = 3
|
||||
};
|
||||
|
||||
class Tcs230
|
||||
{
|
||||
private:
|
||||
bool enabled;
|
||||
Tcs230Colors color;
|
||||
Tcs230Scalings scaling;
|
||||
public:
|
||||
Tcs230();
|
||||
|
||||
bool getEnabled() {return enabled;}
|
||||
void setEnabled(bool enabled);
|
||||
|
||||
Tcs230Colors getColor() {return color;}
|
||||
void setColor(Tcs230Colors color);
|
||||
|
||||
Tcs230Scalings getScaling() {return scaling;}
|
||||
void setScaling(Tcs230Scalings scaling);
|
||||
};
|
||||
|
||||
#endif
|
||||
18
adc.cpp
18
adc.cpp
|
|
@ -1,18 +0,0 @@
|
|||
#include "adc.h"
|
||||
|
||||
#include <avr/io.h>
|
||||
|
||||
|
||||
void initADC() {
|
||||
ADMUX = (1<<REFS0);
|
||||
ADCSRA = (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
|
||||
}
|
||||
|
||||
uint16_t getADCValue(int port) {
|
||||
ADMUX = (1<<REFS0)|(port&0x07);
|
||||
ADCSRA |= (1<<ADSC);
|
||||
|
||||
while(!(ADCSRA & (1<<ADIF)));
|
||||
|
||||
return ADC;
|
||||
}
|
||||
10
adc.h
10
adc.h
|
|
@ -1,10 +0,0 @@
|
|||
#ifndef _ROBOCUP_ADC_H_
|
||||
#define _ROBOCUP_ADC_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
void initADC();
|
||||
uint16_t getADCValue(int port);
|
||||
|
||||
#endif
|
||||
59
avr.cpp
59
avr.cpp
|
|
@ -1,59 +0,0 @@
|
|||
#include "avr.h"
|
||||
#include "adc.h"
|
||||
#include "global.h"
|
||||
#include "util.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
int getButton() {
|
||||
uint16_t val = getADCValue(7);
|
||||
|
||||
if(val < 144) return 5;
|
||||
if(val < 228) return 4;
|
||||
if(val < 304) return 3;
|
||||
if(val < 376) return 2;
|
||||
if(val < 600) return 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void waitForButton(int i) {
|
||||
if(i <= 0) while(getButton() == 0);
|
||||
else while(getButton() != CLAMP(1, i, 5));
|
||||
|
||||
while(getButton() != 0);
|
||||
}
|
||||
|
||||
void beep(unsigned long freq) {
|
||||
const int prescalers[7] = {1, 8, 32, 64, 128, 256, 1024};
|
||||
|
||||
if(freq == 0) {
|
||||
beepOff();
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
for(int i = 0; i < 7; i++) {
|
||||
if(F_CPU/freq/2/prescalers[i] <= 256) {
|
||||
TCCR2 = (TCCR2&~0x07)|(i+1);
|
||||
OCR2 = F_CPU/freq/2/prescalers[i] - 1;
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
beepOff();
|
||||
}
|
||||
|
||||
void beepOff() {
|
||||
TCCR2 &= ~0x07;
|
||||
}
|
||||
|
||||
void ledOn() {
|
||||
PORTA &= ~0x20;
|
||||
}
|
||||
|
||||
void ledOff() {
|
||||
PORTA |= 0x20;
|
||||
}
|
||||
16
avr.h
16
avr.h
|
|
@ -1,16 +0,0 @@
|
|||
#ifndef _ROBOCUP_AVR_H_
|
||||
#define _ROBOCUP_AVR_H_
|
||||
|
||||
#include <avr/io.h>
|
||||
|
||||
|
||||
int getButton();
|
||||
void waitForButton(int i);
|
||||
|
||||
void beep(unsigned long freq);
|
||||
void beepOff();
|
||||
|
||||
void ledOn();
|
||||
void ledOff();
|
||||
|
||||
#endif
|
||||
27
global.h
27
global.h
|
|
@ -1,27 +0,0 @@
|
|||
#ifndef _ROBOCUP_GLOBAL_H_
|
||||
#define _ROBOCUP_GLOBAL_H_
|
||||
|
||||
#define F_CPU 16000000UL
|
||||
#define TIMER_PRESCALER 8
|
||||
|
||||
#define DEFAULT_SPEED 0.7
|
||||
#define CURVE_SPIN 0.3
|
||||
#define TURN_SPIN 0.5
|
||||
#define STRAFE_DIRECTION 30.0
|
||||
|
||||
#define CAL_MIN_DIFF 0.3
|
||||
#define CAL_MAX_WHITE_DIFF 0.1
|
||||
|
||||
#define DISTANCE_GAIN 9
|
||||
#define DISTANCE_RECOG 4
|
||||
#define DISTANCE_MAX 6
|
||||
#define DISTANCE_MIN 5
|
||||
#define DISTANCE_SPIN 0.3
|
||||
#define DISTANCE_ANGLE 30.0
|
||||
|
||||
|
||||
enum Status {
|
||||
Ok, White, Debris, DebrisWhite, Turn, TurnWhite
|
||||
};
|
||||
|
||||
#endif
|
||||
34
hardware.cpp
34
hardware.cpp
|
|
@ -1,34 +0,0 @@
|
|||
#include "hardware.h"
|
||||
|
||||
#include "adc.h"
|
||||
#include "i2c.h"
|
||||
|
||||
#include <avr/io.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
|
||||
void initHardware() {
|
||||
DDRA = 0x20;
|
||||
PORTA = 0xFF;
|
||||
|
||||
DDRB = 0xFF;
|
||||
PORTB = 0x00;
|
||||
|
||||
DDRC = 0x7C;
|
||||
PORTC = 0x83;
|
||||
|
||||
DDRD = 0xFF;
|
||||
PORTD = 0x00;
|
||||
|
||||
TCCR0 = 0x62;
|
||||
TCCR1A = 0xA1;
|
||||
TCCR1B = 0x82;
|
||||
TCCR2 = 0x18;
|
||||
|
||||
TIMSK = 0x01;
|
||||
|
||||
initADC();
|
||||
initI2C();
|
||||
|
||||
sei();
|
||||
}
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
#ifndef _ROBOCUP_HARDWARE_H_
|
||||
#define _ROBOCUP_HARDWARE_H_
|
||||
|
||||
void initHardware();
|
||||
|
||||
#endif
|
||||
154
i2c.cpp
154
i2c.cpp
|
|
@ -1,154 +0,0 @@
|
|||
#include "i2c.h"
|
||||
#include "avr.h"
|
||||
|
||||
#include <util/twi.h>
|
||||
|
||||
|
||||
static bool I2CStartSend(uint8_t addr);
|
||||
static bool I2CStartRecv(uint8_t addr);
|
||||
static void I2CStop();
|
||||
|
||||
|
||||
static bool I2CStartSend(uint8_t addr) {
|
||||
while(true) {
|
||||
TWCR |= (1<<TWINT)|(1<<TWSTA);
|
||||
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
|
||||
|
||||
TWDR = (addr&0xFE);
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS == TW_MT_SLA_ACK)
|
||||
return true;
|
||||
|
||||
I2CStop();
|
||||
}
|
||||
}
|
||||
|
||||
static bool I2CStartRecv(uint8_t addr) {
|
||||
while(true) {
|
||||
TWCR |= (1<<TWINT)|(1<<TWSTA);
|
||||
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
|
||||
|
||||
TWDR = (addr|1);
|
||||
TWCR |= (1<<TWINT);
|
||||
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS == TW_MR_SLA_ACK)
|
||||
return true;
|
||||
|
||||
I2CStop();
|
||||
}
|
||||
}
|
||||
|
||||
static void I2CStop() {
|
||||
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
|
||||
}
|
||||
|
||||
|
||||
void initI2C() {
|
||||
TWBR = 10;
|
||||
TWCR = (1<<TWEN);
|
||||
TWSR = 0;
|
||||
}
|
||||
|
||||
bool I2CSendByte(uint8_t addr, uint8_t data) {
|
||||
if(!I2CStartSend(addr)) return false;
|
||||
|
||||
TWDR=data;
|
||||
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS != TW_MT_DATA_ACK) {
|
||||
I2CStop();
|
||||
return false;
|
||||
}
|
||||
|
||||
I2CStop();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool I2CSend(uint8_t addr, uint8_t *data, int length) {
|
||||
if(!I2CStartSend(addr)) return false;
|
||||
|
||||
for(int i = 0; i < length; i++) {
|
||||
TWDR=data[i];
|
||||
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS != TW_MT_DATA_ACK && i < length-1) {
|
||||
I2CStop();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
I2CStop();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool I2CRecvByte(uint8_t addr, uint8_t *data) {
|
||||
if(!I2CStartRecv(addr)) return false;
|
||||
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) {
|
||||
I2CStop();
|
||||
return false;
|
||||
}
|
||||
|
||||
*data = TWDR;
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
|
||||
I2CStop();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int I2CRecv(uint8_t addr, uint8_t *data, int length) {
|
||||
if(!I2CStartRecv(addr)) return -1;
|
||||
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
|
||||
|
||||
for(int i = 0; i < length-1; i++) {
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
switch(TW_STATUS) {
|
||||
case TW_MR_DATA_ACK:
|
||||
data[i] = TWDR;
|
||||
break;
|
||||
case TW_MR_DATA_NACK:
|
||||
data[i] = TWDR;
|
||||
I2CStop();
|
||||
return i+1;
|
||||
default:
|
||||
I2CStop();
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
TWCR = (1<<TWEN)|(1<<TWINT);
|
||||
while(!(TWCR & (1<<TWINT)));
|
||||
|
||||
switch(TW_STATUS) {
|
||||
case TW_MR_DATA_ACK:
|
||||
case TW_MR_DATA_NACK:
|
||||
data[length-1] = TWDR;
|
||||
I2CStop();
|
||||
return length;
|
||||
}
|
||||
|
||||
I2CStop();
|
||||
return -1;
|
||||
}
|
||||
13
i2c.h
13
i2c.h
|
|
@ -1,13 +0,0 @@
|
|||
#ifndef _ROBOCUP_I2C_H_
|
||||
#define _ROBOCUP_I2C_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
void initI2C();
|
||||
bool I2CSendByte(uint8_t addr, uint8_t data);
|
||||
bool I2CSend(uint8_t addr, uint8_t *data, int length);
|
||||
bool I2CRecvByte(uint8_t addr, uint8_t *data);
|
||||
int I2CRecv(uint8_t addr, uint8_t *data, int length);
|
||||
|
||||
#endif
|
||||
169
main.cpp
169
main.cpp
|
|
@ -1,169 +0,0 @@
|
|||
#include "avr.h"
|
||||
#include "global.h"
|
||||
#include "hardware.h"
|
||||
#include "timer.h"
|
||||
#include "util.h"
|
||||
#include "LineSensor.h"
|
||||
#include "LineSensorArray.h"
|
||||
#include "Motor.h"
|
||||
#include "Navigation.h"
|
||||
#include "Srf10.h"
|
||||
|
||||
|
||||
static void delay() {
|
||||
for(unsigned long i = 0; i < 25000; i++)
|
||||
asm("nop");
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
Status status = Ok;
|
||||
|
||||
initHardware();
|
||||
|
||||
Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
|
||||
Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
|
||||
Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
|
||||
|
||||
Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
|
||||
|
||||
LineSensor *lineSensors[5];
|
||||
|
||||
for(int i = 0; i < 5; i++)
|
||||
lineSensors[i] = new LineSensor(i);
|
||||
|
||||
LineSensorArray *lineSensorArray = new LineSensorArray(lineSensors);
|
||||
|
||||
lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
|
||||
lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
|
||||
|
||||
Srf10 *srf10Right = new Srf10(0xE0);
|
||||
Srf10 *srf10Left = new Srf10(0xE2);
|
||||
|
||||
srf10Right->setUnit(Centimeters);
|
||||
srf10Right->setGain(DISTANCE_GAIN);
|
||||
|
||||
srf10Left->setUnit(Centimeters);
|
||||
srf10Left->setGain(DISTANCE_GAIN);
|
||||
|
||||
Srf10 *srf10Last = srf10Left;
|
||||
|
||||
do {
|
||||
lineSensorArray->update();
|
||||
} while(!lineSensorArray->calibrate());
|
||||
|
||||
ledOn();
|
||||
waitForButton(0);
|
||||
ledOff();
|
||||
|
||||
sleep(4000);
|
||||
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
|
||||
while(true) {
|
||||
lineSensorArray->update();
|
||||
|
||||
srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
|
||||
srf10Last->updateDistance();
|
||||
|
||||
switch(status) {
|
||||
case Ok:
|
||||
if(lineSensorArray->getLine() == 0.0 && lineSensorArray->getSharpness() == 1.0)
|
||||
lineSensorArray->calibrate();
|
||||
|
||||
// TODO: victims!!
|
||||
|
||||
if(srf10Last->hasDistance() && srf10Last->getDistance() <= DISTANCE_RECOG) {
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
navigation->setDirection(90.0);
|
||||
|
||||
status = Debris;
|
||||
break;
|
||||
}
|
||||
|
||||
if(lineSensorArray->getSharpness() < 0.0)
|
||||
break;
|
||||
else if(lineSensorArray->getSharpness() == 0.0) {
|
||||
// TODO: white?
|
||||
}
|
||||
else {
|
||||
if(lineSensorArray->getLine() == 0.0) {
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
navigation->setDirection(0.0);
|
||||
navigation->setSpin(0.0);
|
||||
}
|
||||
else if(ABS(lineSensorArray->getLine()) < 1.0) {
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
navigation->setDirection(0.0);
|
||||
|
||||
if(lineSensorArray->getLine() < 0.0)
|
||||
navigation->setSpin(CURVE_SPIN);
|
||||
else
|
||||
navigation->setSpin(-CURVE_SPIN);
|
||||
}
|
||||
else if(ABS(lineSensorArray->getLine()) < 1.5) {
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
|
||||
if(lineSensorArray->getLine() < 0.0) {
|
||||
navigation->setDirection(STRAFE_DIRECTION);
|
||||
navigation->setSpin(CURVE_SPIN);
|
||||
}
|
||||
else {
|
||||
navigation->setDirection(-STRAFE_DIRECTION);
|
||||
navigation->setSpin(-CURVE_SPIN);
|
||||
}
|
||||
}
|
||||
else {
|
||||
navigation->setSpeed(0.0);
|
||||
|
||||
if(lineSensorArray->getLine() < 0.0)
|
||||
navigation->setSpin(TURN_SPIN);
|
||||
else
|
||||
navigation->setSpin(-TURN_SPIN);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
case White:
|
||||
break;
|
||||
case Debris:
|
||||
case DebrisWhite:
|
||||
if(srf10Right->getDistance() < srf10Left->getDistance())
|
||||
navigation->setSpin(DISTANCE_SPIN);
|
||||
else if(srf10Right->getDistance() > srf10Left->getDistance())
|
||||
navigation->setSpin(-DISTANCE_SPIN);
|
||||
else
|
||||
navigation->setSpin(0);
|
||||
|
||||
if((srf10Right->getDistance()+srf10Left->getDistance())/2 > DISTANCE_MAX)
|
||||
navigation->setDirection(90.0-DISTANCE_ANGLE);
|
||||
else if((srf10Right->getDistance()+srf10Left->getDistance())/2 < DISTANCE_MIN)
|
||||
navigation->setDirection(90.0+DISTANCE_ANGLE);
|
||||
else
|
||||
navigation->setDirection(90.0);
|
||||
|
||||
if(status == Debris && lineSensorArray->isSensorWhite(2))
|
||||
status = DebrisWhite;
|
||||
else if(status == DebrisWhite && lineSensorArray->isSensorBlack(2)) {
|
||||
navigation->setSpeed(0.0);
|
||||
navigation->setSpin(TURN_SPIN);
|
||||
status = Turn;
|
||||
}
|
||||
break;
|
||||
case Turn:
|
||||
if(lineSensorArray->isSensorWhite(2))
|
||||
status = TurnWhite;
|
||||
break;
|
||||
case TurnWhite:
|
||||
if(lineSensorArray->isSensorBlack(2)) {
|
||||
navigation->setSpeed(DEFAULT_SPEED);
|
||||
navigation->setDirection(0.0);
|
||||
navigation->setSpin(0.0);
|
||||
|
||||
status = Ok;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
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
|
||||
18
timer.cpp
18
timer.cpp
|
|
@ -1,18 +0,0 @@
|
|||
#include "timer.h"
|
||||
#include "global.h"
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
|
||||
static volatile unsigned long ticks = 0;
|
||||
|
||||
|
||||
SIGNAL(SIG_OVERFLOW0) {
|
||||
ticks++;
|
||||
}
|
||||
|
||||
void sleep(unsigned long ms) {
|
||||
unsigned long t = ticks + ms * (F_CPU/TIMER_PRESCALER/256000);
|
||||
|
||||
while(ticks < t);
|
||||
}
|
||||
6
timer.h
6
timer.h
|
|
@ -1,6 +0,0 @@
|
|||
#ifndef _ROBOCUP_TIMER_H_
|
||||
#define _ROBOCUP_TIMER_H_
|
||||
|
||||
void sleep(unsigned long ms);
|
||||
|
||||
#endif
|
||||
9
util.h
9
util.h
|
|
@ -1,9 +0,0 @@
|
|||
#ifndef _ROBOCUP_UTIL_H_
|
||||
#define _ROBOCUP_UTIL_H_
|
||||
|
||||
#define MIN(a,b) ((a<b)?a:b)
|
||||
#define MAX(a,b) ((a>b)?a:b)
|
||||
#define CLAMP(min,x,max) ((x<min)?min:(x>max)?max:x)
|
||||
#define ABS(a) ((a<0)?-a:a)
|
||||
|
||||
#endif
|
||||
Reference in a new issue