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 "i2c.h"
#include "timer.h"
Srf10::Srf10(uint8_t id)
@ -13,6 +14,7 @@ Srf10::Srf10(uint8_t id)
unit = Inches;
distance = 0;
has_distance = false;
firmware = readFirmware();
}
@ -46,15 +48,25 @@ bool Srf10::setRange(uint8_t range) {
long Srf10::updateDistance() {
uint8_t data[2] = {0, unit};
uint16_t d;
if(!I2CSend(id, data, 2)) return -1;
while(readFirmware() != 0xFF);
do {
sleep(1);
} while(readFirmware() == 0xFF);
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;
long distance;
bool has_distance;
Srf10Units unit;
@ -39,6 +40,7 @@ public:
long updateDistance();
long getDistance() {return distance;}
bool hasDistance() {return has_distance;}
};
#endif

10
global.h
View file

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

12
hardware.cpp
View file

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

28
i2c.cpp
View file

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

66
main.cpp
View file

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

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