From 7da669b97ec71f138c74d562b8a3b219d3f98a50 Mon Sep 17 00:00:00 2001 From: neoraider Date: Fri, 13 Apr 2007 17:58:03 +0000 Subject: Makefile angelegt; Grundfunktionen erweitert --- LineSensor.cpp | 16 ++++++++++++++++ LineSensor.h | 14 ++++++++++++++ Makefile | 31 +++++++++++++++++++++++++++++++ Motor.cpp | 29 +++++++++++++++++++++++++++++ Motor.h | 22 ++++++++++++++++++++++ Navigation.cpp | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ Navigation.h | 31 +++++++++++++++++++++++++++++++ avr.cpp | 13 ------------- avr.h | 9 --------- hardware.cpp | 4 +++- hardware.h | 13 ------------- main.cpp | 25 +++++++++++++++++++++++++ new_op.cpp | 10 ++++++++++ 13 files changed, 229 insertions(+), 36 deletions(-) create mode 100644 LineSensor.cpp create mode 100644 LineSensor.h create mode 100644 Makefile create mode 100644 Motor.cpp create mode 100644 Motor.h create mode 100644 Navigation.cpp create mode 100644 Navigation.h create mode 100644 new_op.cpp 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 + + +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 + + +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 -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 -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 + + +void *operator new(size_t sz) { + return malloc(sz); +} + +void operator delete(void *p) { + free(p); +} -- cgit v1.2.3