neocturne/rc2007-rescue
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung implementiert.

Browse source
This commit is contained in:
neoraider 2007-04-18 23:00:05 +00:00
parent 847645b3cc
commit e7eeeb0d4e
8 changed files with 150 additions and 66 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
Srf10.cpp
View file

@ -1,6 +1,7 @@
#include "Srf10.h" #include "Srf10.h"
#include "i2c.h" #include "i2c.h"
#include "timer.h"
Srf10::Srf10(uint8_t id) Srf10::Srf10(uint8_t id)
@ -13,6 +14,7 @@ Srf10::Srf10(uint8_t id)
unit = Inches; unit = Inches;
distance = 0; distance = 0;
has_distance = false;
firmware = readFirmware(); firmware = readFirmware();
} }
@ -46,15 +48,25 @@ bool Srf10::setRange(uint8_t range) {
long Srf10::updateDistance() { long Srf10::updateDistance() {
uint8_t data[2] = {0, unit}; uint8_t data[2] = {0, unit};
uint16_t d;
if(!I2CSend(id, data, 2)) return -1; if(!I2CSend(id, data, 2)) return -1;
while(readFirmware() != 0xFF); do {
sleep(1);
} while(readFirmware() == 0xFF);
if(!I2CSendByte(id, 2)) return -1; if(!I2CSendByte(id, 2)) return -1;
if(!I2CRecv(id, data, 2)) return -1; if(I2CRecv(id, data, 2) < 2) return -1;
distance = (((uint16_t)data[0]) << 8) | data[1]; d = (((uint16_t)data[0]) << 8) | data[1];
return distance; if(d == 0) has_distance = false;
else {
has_distance = true;
distance = d;
}
return d;
} }

2
Srf10.h
View file

@ -19,6 +19,7 @@ private:
int firmware; int firmware;
long distance; long distance;
bool has_distance;
Srf10Units unit; Srf10Units unit;
@ -39,6 +40,7 @@ public:
long updateDistance(); long updateDistance();
long getDistance() {return distance;} long getDistance() {return distance;}
bool hasDistance() {return has_distance;}
}; };
#endif #endif

10
global.h
View file

@ -2,6 +2,7 @@
#define _ROBOCUP_GLOBAL_H_ #define _ROBOCUP_GLOBAL_H_
#define F_CPU 16000000UL #define F_CPU 16000000UL
#define TIMER_PRESCALER 8
#define DEFAULT_SPEED 0.7 #define DEFAULT_SPEED 0.7
#define CURVE_SPIN 0.3 #define CURVE_SPIN 0.3
@ -11,9 +12,16 @@
#define CAL_MIN_DIFF 0.3 #define CAL_MIN_DIFF 0.3
#define CAL_MAX_WHITE_DIFF 0.1 #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 { enum Status {
Ok, White, Debris Ok, White, Debris, DebrisWhite, Turn, TurnWhite
}; };
#endif #endif

16
hardware.cpp
View file

@ -4,6 +4,7 @@
#include "i2c.h" #include "i2c.h"
#include <avr/io.h> #include <avr/io.h>
#include <avr/interrupt.h>
void initHardware() { void initHardware() {
@ -19,18 +20,15 @@ void initHardware() {
DDRD = 0xFF; DDRD = 0xFF;
PORTD = 0x00; PORTD = 0x00;
TCCR0 = 0x62; TCCR0 = 0x62;
OCR0 = 0; TCCR1A = 0xA1;
TCCR1A = 0xA1;
OCR1A = 0;
TCCR1B = 0x82; TCCR1B = 0x82;
OCR1B = 0;
TCCR2 = 0x18; TCCR2 = 0x18;
OCR2 = 0;
TIMSK = 0x01;
initADC(); initADC();
initI2C(); initI2C();
sei();
} }

78
i2c.cpp
View file

@ -1,4 +1,5 @@
#include "i2c.h" #include "i2c.h"
#include "avr.h"
#include <util/twi.h> #include <util/twi.h>
@ -9,41 +10,43 @@ static void I2CStop();
static bool I2CStartSend(uint8_t addr) { static bool I2CStartSend(uint8_t addr) {
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA); while(true) {
TWCR |= (1<<TWINT)|(1<<TWSTA);
while(!(TWCR & (1<<TWINT)));
while(!(TWCR & (1<<TWINT)));
if (TW_STATUS != TW_START) return false;
if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
TWDR = (addr&0xFE);
TWCR = (1<<TWEN)|(1<<TWINT); TWDR = (addr&0xFE);
TWCR = (1<<TWEN)|(1<<TWINT);
while(!(TWCR & (1<<TWINT)));
while(!(TWCR & (1<<TWINT)));
if(TW_STATUS == TW_MT_SLA_ACK)
return true; if(TW_STATUS == TW_MT_SLA_ACK)
return true;
I2CStop();
return false; I2CStop();
}
} }
static bool I2CStartRecv(uint8_t addr) { static bool I2CStartRecv(uint8_t addr) {
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA); while(true) {
TWCR |= (1<<TWINT)|(1<<TWSTA);
while(!(TWCR & (1<<TWINT)));
while(!(TWCR & (1<<TWINT)));
if (TW_STATUS != TW_START) return false;
if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
TWDR = (addr|1);
TWCR = (1<<TWEN)|(1<<TWINT); TWDR = (addr|1);
TWCR |= (1<<TWINT);
while(!(TWCR & (1<<TWINT)));
while(!(TWCR & (1<<TWINT)));
if(TW_STATUS == TW_MR_SLA_ACK)
return true; if(TW_STATUS == TW_MR_SLA_ACK)
return true;
I2CStop();
return false; I2CStop();
}
} }
static void I2CStop() { static void I2CStop() {
@ -83,7 +86,7 @@ bool I2CSend(uint8_t addr, uint8_t *data, int length) {
TWCR = (1<<TWEN)|(1<<TWINT); TWCR = (1<<TWEN)|(1<<TWINT);
while(!(TWCR & (1<<TWINT))); while(!(TWCR & (1<<TWINT)));
if(TW_STATUS != TW_MT_DATA_ACK) { if(TW_STATUS != TW_MT_DATA_ACK && i < length-1) {
I2CStop(); I2CStop();
return false; return false;
} }
@ -96,6 +99,8 @@ bool I2CSend(uint8_t addr, uint8_t *data, int length) {
bool I2CRecvByte(uint8_t addr, uint8_t *data) { bool I2CRecvByte(uint8_t addr, uint8_t *data) {
if(!I2CStartRecv(addr)) return false; if(!I2CStartRecv(addr)) return false;
TWCR = (1<<TWEN)|(1<<TWINT);
while(!(TWCR & (1<<TWINT))); while(!(TWCR & (1<<TWINT)));
if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) { if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) {
@ -107,23 +112,24 @@ bool I2CRecvByte(uint8_t addr, uint8_t *data) {
TWCR = (1<<TWEN)|(1<<TWINT); TWCR = (1<<TWEN)|(1<<TWINT);
I2CStop(); I2CStop();
return true; return true;
} }
int I2CRecv(uint8_t addr, uint8_t *data, int length) { int I2CRecv(uint8_t addr, uint8_t *data, int length) {
if(!I2CStartRecv(addr)) return -1; if(!I2CStartRecv(addr)) return -1;
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
for(int i = 0; i < length-1; i++) { for(int i = 0; i < length-1; i++) {
while(!(TWCR & (1<<TWINT))); while(!(TWCR & (1<<TWINT)));
switch(TW_STATUS) { switch(TW_STATUS) {
case TW_MR_DATA_ACK: case TW_MR_DATA_ACK:
data[i] = TWDR; data[i] = TWDR;
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
break; break;
case TW_MR_DATA_NACK: case TW_MR_DATA_NACK:
data[i] = TWDR; data[i] = TWDR;
TWCR = (1<<TWEN)|(1<<TWINT);
I2CStop(); I2CStop();
return i+1; return i+1;
default: default:
@ -132,13 +138,13 @@ int I2CRecv(uint8_t addr, uint8_t *data, int length) {
} }
} }
TWCR = (1<<TWEN)|(1<<TWINT);
while(!(TWCR & (1<<TWINT))); while(!(TWCR & (1<<TWINT)));
switch(TW_STATUS) { switch(TW_STATUS) {
case TW_MR_DATA_ACK: case TW_MR_DATA_ACK:
case TW_MR_DATA_NACK: case TW_MR_DATA_NACK:
data[length-1] = TWDR; data[length-1] = TWDR;
TWCR = (1<<TWEN)|(1<<TWINT);
I2CStop(); I2CStop();
return length; return length;
} }

66
main.cpp
View file

@ -1,6 +1,7 @@
#include "avr.h" #include "avr.h"
#include "global.h" #include "global.h"
#include "hardware.h" #include "hardware.h"
#include "timer.h"
#include "util.h" #include "util.h"
#include "LineSensor.h" #include "LineSensor.h"
#include "LineSensorArray.h" #include "LineSensorArray.h"
@ -9,15 +10,6 @@
#include "Srf10.h" #include "Srf10.h"
static void delay() { static void delay() {
for(unsigned long i = 0; i < 25000; i++) for(unsigned long i = 0; i < 25000; i++)
asm("nop"); asm("nop");
@ -45,14 +37,16 @@ int main() {
lineSensorArray->setMinimumDifference(CAL_MIN_DIFF); lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF); lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
//Srf10 *srf10Right = new Srf10(0xE0); Srf10 *srf10Right = new Srf10(0xE0);
Srf10 *srf10Left = new Srf10(0xE2); Srf10 *srf10Left = new Srf10(0xE2);
//srf10Right->setUnit(Centimeters); srf10Right->setUnit(Centimeters);
srf10Right->setGain(DISTANCE_GAIN);
srf10Left->setUnit(Centimeters); srf10Left->setUnit(Centimeters);
srf10Left->setGain(DISTANCE_GAIN);
//Srf10 *srf10Last = srf10Left; Srf10 *srf10Last = srf10Left;
do { do {
lineSensorArray->update(); lineSensorArray->update();
@ -63,8 +57,8 @@ int main() {
while(true) { while(true) {
lineSensorArray->update(); lineSensorArray->update();
//srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left; srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
//srf10Last->updateDistance(); srf10Last->updateDistance();
switch(status) { switch(status) {
case Ok: case Ok:
@ -73,7 +67,13 @@ int main() {
// TODO: victims!! // TODO: victims!!
// TODO: debris!! if(srf10Last->hasDistance() && srf10Last->getDistance() <= DISTANCE_RECOG) {
navigation->setSpeed(DEFAULT_SPEED);
navigation->setDirection(90.0);
status = Debris;
break;
}
if(lineSensorArray->getSharpness() < 0.0) if(lineSensorArray->getSharpness() < 0.0)
break; break;
@ -121,7 +121,41 @@ int main() {
case White: case White:
break; break;
case Debris: 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; 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;
}
} }
} }

18
timer.cpp Normal file
View file

@ -0,0 +1,18 @@
#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 Normal file
View file

@ -0,0 +1,6 @@
#ifndef _ROBOCUP_TIMER_H_
#define _ROBOCUP_TIMER_H_
void sleep(unsigned long ms);
#endif