summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Srf10.cpp20
-rw-r--r--Srf10.h2
-rw-r--r--global.h10
-rw-r--r--hardware.cpp16
-rw-r--r--i2c.cpp78
-rw-r--r--main.cpp66
-rw-r--r--timer.cpp18
-rw-r--r--timer.h6
8 files changed, 150 insertions, 66 deletions
diff --git a/Srf10.cpp b/Srf10.cpp
index 329fc8a..a66828b 100644
--- a/Srf10.cpp
+++ b/Srf10.cpp
@@ -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;
+
+ d = (((uint16_t)data[0]) << 8) | data[1];
- distance = (((uint16_t)data[0]) << 8) | data[1];
+ if(d == 0) has_distance = false;
+ else {
+ has_distance = true;
+ distance = d;
+ }
- return distance;
+ return d;
}
diff --git a/Srf10.h b/Srf10.h
index c7417b8..1b69018 100644
--- a/Srf10.h
+++ b/Srf10.h
@@ -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
diff --git a/global.h b/global.h
index 6aa13e4..3bfb285 100644
--- a/global.h
+++ b/global.h
@@ -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
diff --git a/hardware.cpp b/hardware.cpp
index 51ae18e..e652dda 100644
--- a/hardware.cpp
+++ b/hardware.cpp
@@ -4,6 +4,7 @@
#include "i2c.h"
#include <avr/io.h>
+#include <avr/interrupt.h>
void initHardware() {
@@ -19,18 +20,15 @@ void initHardware() {
DDRD = 0xFF;
PORTD = 0x00;
- TCCR0 = 0x62;
- OCR0 = 0;
-
- TCCR1A = 0xA1;
- OCR1A = 0;
-
+ TCCR0 = 0x62;
+ TCCR1A = 0xA1;
TCCR1B = 0x82;
- OCR1B = 0;
-
TCCR2 = 0x18;
- OCR2 = 0;
+
+ TIMSK = 0x01;
initADC();
initI2C();
+
+ sei();
}
diff --git a/i2c.cpp b/i2c.cpp
index 61450fa..39b6b28 100644
--- a/i2c.cpp
+++ b/i2c.cpp
@@ -1,4 +1,5 @@
#include "i2c.h"
+#include "avr.h"
#include <util/twi.h>
@@ -9,41 +10,43 @@ static void I2CStop();
static bool I2CStartSend(uint8_t addr) {
- TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
-
- while(!(TWCR & (1<<TWINT)));
-
- if (TW_STATUS != TW_START) return false;
-
- TWDR = (addr&0xFE);
- TWCR = (1<<TWEN)|(1<<TWINT);
-
- while(!(TWCR & (1<<TWINT)));
-
- if(TW_STATUS == TW_MT_SLA_ACK)
- return true;
-
- I2CStop();
- return false;
+ 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) {
- TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
-
- while(!(TWCR & (1<<TWINT)));
-
- if (TW_STATUS != TW_START) return false;
-
- TWDR = (addr|1);
- TWCR = (1<<TWEN)|(1<<TWINT);
-
- while(!(TWCR & (1<<TWINT)));
-
- if(TW_STATUS == TW_MR_SLA_ACK)
- return true;
-
- I2CStop();
- return false;
+ 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() {
@@ -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;
}
diff --git a/main.cpp b/main.cpp
index 0da17a9..8fc011b 100644
--- a/main.cpp
+++ b/main.cpp
@@ -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);
- srf10Left->setUnit(Centimeters);
+ srf10Right->setUnit(Centimeters);
+ srf10Right->setGain(DISTANCE_GAIN);
- //Srf10 *srf10Last = srf10Left;
+ srf10Left->setUnit(Centimeters);
+ srf10Left->setGain(DISTANCE_GAIN);
+ 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;
+ }
}
}
diff --git a/timer.cpp b/timer.cpp
new file mode 100644
index 0000000..b4a1b12
--- /dev/null
+++ b/timer.cpp
@@ -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);
+}
diff --git a/timer.h b/timer.h
new file mode 100644
index 0000000..ad4bf05
--- /dev/null
+++ b/timer.h
@@ -0,0 +1,6 @@
+#ifndef _ROBOCUP_TIMER_H_
+#define _ROBOCUP_TIMER_H_
+
+void sleep(unsigned long ms);
+
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-03 18:24:58 +0100