diff options
| -rw-r--r-- | LineSensorArray.cpp | 70 | ||||
| -rw-r--r-- | LineSensorArray.h | 39 | ||||
| -rw-r--r-- | Makefile | 13 | ||||
| -rw-r--r-- | Srf10.cpp | 60 | ||||
| -rw-r--r-- | Srf10.h | 44 | ||||
| -rw-r--r-- | adc.cpp | 2 | ||||
| -rw-r--r-- | avr.cpp | 2 | ||||
| -rw-r--r-- | avr.h | 2 | ||||
| -rw-r--r-- | hardware.cpp | 3 | ||||
| -rw-r--r-- | hardware.h | 5 | ||||
| -rw-r--r-- | main.cpp | 4 |
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 @@ -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; +} @@ -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_*/ @@ -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))); @@ -2,6 +2,8 @@ #include "adc.h" #include "util.h" +#include <stdint.h> + int getButton() { uint16_t val = getADCValue(7); @@ -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() { @@ -1,11 +1,6 @@ #ifndef _ROBOCUP_HARDWARE_H_ #define _ROBOCUP_HARDWARE_H_ -#include "avr.h" - -#include <avr/io.h> - - void initHardware(); #endif @@ -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"); } |