summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--LineSensor.cpp16
-rw-r--r--LineSensor.h14
-rw-r--r--Makefile31
-rw-r--r--Motor.cpp29
-rw-r--r--Motor.h22
-rw-r--r--Navigation.cpp48
-rw-r--r--Navigation.h31
-rw-r--r--avr.cpp13
-rw-r--r--avr.h9
-rw-r--r--hardware.cpp4
-rw-r--r--hardware.h13
-rw-r--r--main.cpp25
-rw-r--r--new_op.cpp10
13 files changed, 229 insertions, 36 deletions
diff --git a/LineSensor.cpp b/LineSensor.cpp
new file mode 100644
index 0000000..a269fa1
--- /dev/null
+++ b/LineSensor.cpp
@@ -0,0 +1,16 @@
+#include "LineSensor.h"
+
+#include "adc.h"
+
+
+LineSensor::LineSensor(int id) {
+ this->id = id;
+
+ updateValue();
+}
+
+float LineSensor::updateValue() {
+ value = getADCValue(id)/1024.0;
+
+ return value;
+}
diff --git a/LineSensor.h b/LineSensor.h
new file mode 100644
index 0000000..6b094f8
--- /dev/null
+++ b/LineSensor.h
@@ -0,0 +1,14 @@
+#ifndef _ROBOCUP_LINESENSOR_H_
+#define _ROBOCUP_LINESENSOR_H_
+
+class LineSensor {
+ private:
+ int id;
+ float value;
+ public:
+ LineSensor(int id);
+ float updateValue();
+ float getValue() {return value;}
+};
+
+#endif
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..c7fe375
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,31 @@
+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 $@ $^
+
+%.o: %.cpp
+ avr-g++ -c -mmcu=atmega32 -o $@ $<
+
+%.d: %.cpp
+ @set -e; rm -f $@; \
+ avr-g++ -M -mmcu=atmega32 -MF $@.$$$$ $<; \
+ sed 's,\($*\)\.o[ :]*,\1.o $@ : ,g' < $@.$$$$ > $@; \
+ rm -f $@.$$$$
+
+program: robocup.hex
+ sudo avrdude -P usb -c avrisp2 -p m32 -U $<
+
+clean:
+ rm -f robocup.hex robocup.elf $(FILES:%=%.o) $(FILES:%=%.d)
+
+.PHONY: clean
diff --git a/Motor.cpp b/Motor.cpp
new file mode 100644
index 0000000..7060967
--- /dev/null
+++ b/Motor.cpp
@@ -0,0 +1,29 @@
+#include "Motor.h"
+
+#include "util.h"
+
+
+Motor::Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask) {
+ this->port = port;
+ this->pwmPort = pwmPort;
+ this->fwdMask = fwdMask;
+ this->revMask = revMask;
+
+ setSpeed(0);
+}
+
+void Motor::setSpeed(int speed) {
+ this->speed = CLAMP(-255, speed, 255);
+
+ if(speed > 0) {
+ *port &= ~revMask;
+ *port |= fwdMask;
+ }
+ else if(speed < 0) {
+ *port &= ~fwdMask;
+ *port |= revMask;
+ }
+ else *port |= fwdMask|revMask;
+
+ *pwmPort = ABS(this->speed);
+}
diff --git a/Motor.h b/Motor.h
new file mode 100644
index 0000000..9849491
--- /dev/null
+++ b/Motor.h
@@ -0,0 +1,22 @@
+#ifndef _ROBOCUP_MOTOR_H_
+#define _ROBOCUP_MOTOR_H_
+
+#include <stdint.h>
+
+
+class Motor {
+ private:
+ volatile uint8_t *port;
+ volatile uint8_t *pwmPort;
+ uint8_t fwdMask;
+ uint8_t revMask;
+
+ int speed;
+ public:
+ Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask);
+
+ void setSpeed(int speed);
+ int getSpeed() {return speed;}
+};
+
+#endif
diff --git a/Navigation.cpp b/Navigation.cpp
new file mode 100644
index 0000000..796bb86
--- /dev/null
+++ b/Navigation.cpp
@@ -0,0 +1,48 @@
+#include "Navigation.h"
+#include "hardware.h"
+
+#include <math.h>
+
+
+Navigation::Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3) {
+ motors[0] = m1;
+ motorPos[0] = p1;
+ motors[1] = m2;
+ motorPos[1] = p2;
+ motors[2] = m3;
+ motorPos[2] = p3;
+
+ speed = direction = spin = 0.0;
+
+ update();
+}
+
+void Navigation::update() {
+ float s1, s2, s3;
+
+ s1 = s2 = s3 = spin;
+
+ s1 += speed*sin((direction-motorPos[0])*M_PI/180);
+ s2 += speed*sin((direction-motorPos[1])*M_PI/180);
+ s3 += speed*sin((direction-motorPos[2])*M_PI/180);
+
+ if(ABS(s1) > 1.0) {
+ s1 /= ABS(s1);
+ s2 /= ABS(s1);
+ s3 /= ABS(s1);
+ }
+ if(ABS(s2) > 1.0) {
+ s1 /= ABS(s2);
+ s2 /= ABS(s2);
+ s3 /= ABS(s2);
+ }
+ if(ABS(s3) > 1.0) {
+ s1 /= ABS(s3);
+ s2 /= ABS(s3);
+ s3 /= ABS(s3);
+ }
+
+ motors[0]->setSpeed(int(s1*255));
+ motors[1]->setSpeed(int(s2*255));
+ motors[2]->setSpeed(int(s3*255));
+}
diff --git a/Navigation.h b/Navigation.h
new file mode 100644
index 0000000..561e342
--- /dev/null
+++ b/Navigation.h
@@ -0,0 +1,31 @@
+#ifndef _ROBOCUP_NAVIGATION_H_
+#define _ROBOCUP_NAVIGATION_H_
+
+#include "Motor.h"
+#include "util.h"
+
+
+class Navigation {
+ private:
+ Motor *motors[3];
+ float motorPos[3];
+
+ float speed;
+ float direction;
+ float spin;
+
+ void update();
+ public:
+ Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3);
+
+ void setSpeed(float speed) {this->speed = CLAMP(0.0, speed, 1.0); update();}
+ float getSpeed() {return speed;}
+
+ void setDirection(float direction) {this->direction = direction; update();}
+ float getDirection() {return direction;}
+
+ void setSpin(float spin) {this->spin = CLAMP(-1.0, spin, 1.0); update();}
+ float getSpin() {return spin;}
+};
+
+#endif
diff --git a/avr.cpp b/avr.cpp
index a5194a4..bd3e22c 100644
--- a/avr.cpp
+++ b/avr.cpp
@@ -2,19 +2,6 @@
#include "adc.h"
#include "util.h"
-void setMotorSpeed(MOTOR *motor, int speed) {
- if(speed > 0) {
- *motor->port &= ~motor->revMask;
- *motor->port |= motor->fwdMask;
- }
- else if(speed < 0) {
- *motor->port &= ~motor->fwdMask;
- *motor->port |= motor->revMask;
- }
- else *motor->port |= motor->fwdMask|motor->revMask;
-
- *motor->pwmPort = CLAMP(0, ABS(speed), 255);
-}
int getButton() {
uint16_t val = getADCValue(7);
diff --git a/avr.h b/avr.h
index 2d13616..de4e6e4 100644
--- a/avr.h
+++ b/avr.h
@@ -4,15 +4,6 @@
#include <stdint.h>
-typedef struct {
- volatile uint8_t *port;
- volatile uint8_t *pwmPort;
- uint8_t fwdMask;
- uint8_t revMask;
-} MOTOR;
-
-void setMotorSpeed(MOTOR *motor, int speed);
-
int getButton();
void waitForButton(int i);
diff --git a/hardware.cpp b/hardware.cpp
index 41c7ea2..505cc9a 100644
--- a/hardware.cpp
+++ b/hardware.cpp
@@ -1,6 +1,8 @@
#include "hardware.h"
+
#include "adc.h"
#include "i2c.h"
+#include "stdlib.h"
void initHardware() {
@@ -14,7 +16,7 @@ void initHardware() {
PORTC = 0x83;
DDRD = 0xFF;
- PORTC = 0x00;
+ PORTD = 0x00;
TCCR0 = 0x62;
TCCR1A = 0xA1;
diff --git a/hardware.h b/hardware.h
index 9b2bcb7..4a341e0 100644
--- a/hardware.h
+++ b/hardware.h
@@ -6,19 +6,6 @@
#include <avr/io.h>
-static const MOTOR MOTOR1 = {
- &PORTD, &OCR1BL, 0x01, 0x02
-};
-
-static const MOTOR MOTOR2 = {
- &PORTD, &OCR1AL, 0x04, 0x08
-};
-
-static const MOTOR MOTOR3 = {
- &PORTB, &OCR0, 0x01, 0x02
-};
-
-
void initHardware();
#endif
diff --git a/main.cpp b/main.cpp
index 2a3f761..f356c52 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,5 +1,30 @@
+#include "hardware.h"
+#include "Motor.h"
+#include "Navigation.h"
+
+
+Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
+Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
+Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
+
+Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
+
+
+static void delay() {
+ for(unsigned long i = 0; i < 25000; i++);
+}
+
+
int main() {
+ initHardware();
+
+ navigation->setSpeed(1.0);
+ for(int i = 0; ; i = (i+1)%360) {
+ navigation->setDirection(i);
+
+ delay();
+ }
return 0;
}
diff --git a/new_op.cpp b/new_op.cpp
new file mode 100644
index 0000000..1867d2d
--- /dev/null
+++ b/new_op.cpp
@@ -0,0 +1,10 @@
+#include <stdlib.h>
+
+
+void *operator new(size_t sz) {
+ return malloc(sz);
+}
+
+void operator delete(void *p) {
+ free(p);
+}