diff options
| -rw-r--r-- | LineSensor.cpp | 16 | ||||
| -rw-r--r-- | LineSensor.h | 14 | ||||
| -rw-r--r-- | Makefile | 31 | ||||
| -rw-r--r-- | Motor.cpp | 29 | ||||
| -rw-r--r-- | Motor.h | 22 | ||||
| -rw-r--r-- | Navigation.cpp | 48 | ||||
| -rw-r--r-- | Navigation.h | 31 | ||||
| -rw-r--r-- | avr.cpp | 13 | ||||
| -rw-r--r-- | avr.h | 9 | ||||
| -rw-r--r-- | hardware.cpp | 4 | ||||
| -rw-r--r-- | hardware.h | 13 | ||||
| -rw-r--r-- | main.cpp | 25 | ||||
| -rw-r--r-- | new_op.cpp | 10 |
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); +} @@ -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 @@ -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); @@ -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;
@@ -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 @@ -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); +} |