Makefile angelegt; Grundfunktionen erweitert

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;
+}

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

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

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);
+}

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

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));
+}

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

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);

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);
 

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;

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

main.cpp

@@ -1,5 +1,30 @@
-int main() {
+#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;
 }

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);
+}