LineSensorArray + Srf10 added.

11 files changed, 228 insertions, 16 deletions

diff --git a/LineSensorArray.cpp b/LineSensorArray.cpp
new file mode 100644
index 0000000..48590cc
--- /dev/null
+++ b/LineSensorArray.cpp
@@ -0,0 +1,70 @@
+#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;
+}
diff --git a/LineSensorArray.h b/LineSensorArray.h
new file mode 100644
index 0000000..9f67c60
--- /dev/null
+++ b/LineSensorArray.h
@@ -0,0 +1,39 @@
+#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
diff --git a/Makefile b/Makefile
index bbab11a..20d09eb 100644
--- a/Makefile
+++ b/Makefile
@@ -3,21 +3,17 @@ FILES := $(patsubst %.cpp,%,$(wildcard *.cpp))
 
 all: robocup.elf;
 
-
--include $(FILES:%=%.d)
-
-
 robocup.hex: robocup.elf
 	avr-objcopy -O ihex -R .eeprom $< $@
 
 robocup.elf: $(FILES:%=%.o)
-	avr-g++ -mmcu=atmega32 -o $@ $^
+	avr-g++ -mmcu=atmega32 -Os -o $@ $^
 
 %.o: %.cpp
-	avr-g++ -c -mmcu=atmega32 -o $@ $<
+	avr-g++ -c -mmcu=atmega32 -Os -o $@ $<
 
 %.d: %.cpp
-	avr-g++ -M -mmcu=atmega32 -MF $@ -MT $(patsubst %.cpp,%.o,$<) -MT $@ $<
+	@avr-g++ -MM -mmcu=atmega32 -Os -MF $@ -MT $(patsubst %.cpp,%.o,$<) -MT $@ $<
 
 program: robocup.hex
 	sudo avrdude -P usb -c avrisp2 -p m32 -U $<
@@ -26,3 +22,6 @@ clean:
 	rm -f robocup.hex robocup.elf $(FILES:%=%.o) $(FILES:%=%.d)
 
 .PHONY: clean
+
+
+-include $(FILES:%=%.d)
diff --git a/Srf10.cpp b/Srf10.cpp
new file mode 100644
index 0000000..329fc8a
--- /dev/null
+++ b/Srf10.cpp
@@ -0,0 +1,60 @@
+#include "Srf10.h"
+
+#include "i2c.h"
+
+
+Srf10::Srf10(uint8_t id)
+{
+	this->id = id;
+	
+	gain = 16;
+	range = 255;
+	
+	unit = Inches;
+	
+	distance = 0;
+	
+	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};
+	
+	if(!I2CSend(id, data, 2)) return -1;
+	
+	while(readFirmware() != 0xFF);
+	
+	if(!I2CSendByte(id, 2)) return -1;
+	if(!I2CRecv(id, data, 2)) return -1;
+	
+	distance = (((uint16_t)data[0]) << 8) | data[1];
+	
+	return distance;
+}
diff --git a/Srf10.h b/Srf10.h
new file mode 100644
index 0000000..efb8cfb
--- /dev/null
+++ b/Srf10.h
@@ -0,0 +1,44 @@
+#ifndef SRF10_H_
+#define 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;
+	
+	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;}
+};
+
+#endif /*SRF10_H_*/
diff --git a/adc.cpp b/adc.cpp
index 192d36c..f7d5e04 100644
--- a/adc.cpp
+++ b/adc.cpp
@@ -9,7 +9,7 @@ void initADC() {
 }
 
 uint16_t getADCValue(int port) {
-  ADMUX = (1<<REFS0)|(1<<ADLAR)|(port&0x07);
+  ADMUX = (1<<REFS0)|(port&0x07);
   ADCSRA |= (1<<ADSC);
   
   while(!(ADCSRA & (1<<ADIF)));
diff --git a/avr.cpp b/avr.cpp
index bd3e22c..fca50fb 100644
--- a/avr.cpp
+++ b/avr.cpp
@@ -2,6 +2,8 @@
 #include "adc.h"
 #include "util.h"
 
+#include <stdint.h>
+
 
 int getButton() {
   uint16_t val = getADCValue(7);
diff --git a/avr.h b/avr.h
index de4e6e4..c75d4cf 100644
--- a/avr.h
+++ b/avr.h
@@ -1,7 +1,7 @@
 #ifndef _ROBOCUP_AVR_H_
 #define _ROBOCUP_AVR_H_
 
-#include <stdint.h>
+#include <avr/io.h>
 
 
 int getButton();
diff --git a/hardware.cpp b/hardware.cpp
index 505cc9a..aa3159a 100644
--- a/hardware.cpp
+++ b/hardware.cpp
@@ -2,7 +2,8 @@
 
 #include "adc.h"
 #include "i2c.h"
-#include "stdlib.h"
+
+#include <avr/io.h>
 
 
 void initHardware() {
diff --git a/hardware.h b/hardware.h
index 4a341e0..86cda4f 100644
--- a/hardware.h
+++ b/hardware.h
@@ -1,11 +1,6 @@
 #ifndef _ROBOCUP_HARDWARE_H_
 #define _ROBOCUP_HARDWARE_H_
 
-#include "avr.h"
-
-#include <avr/io.h>
-
-
 void initHardware();
 
 #endif
diff --git a/main.cpp b/main.cpp
index f356c52..9fddb57 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,4 +1,5 @@
 #include "hardware.h"
+#include "avr.h"
 #include "Motor.h"
 #include "Navigation.h"
 
@@ -11,7 +12,8 @@ Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motor
 
 
 static void delay() {
-  for(unsigned long i = 0; i < 25000; i++);
+  for(unsigned long i = 0; i < 25000; i++)
+  	asm("nop");
 }