From 280d688219185a1079fc6e995658341c4a9127c5 Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Thu, 28 Apr 2011 17:26:47 +0200
Subject: Move pwm output code into MotorMixer-class.

---
 main.cpp | 66 +++++-----------------------------------------------------------
 1 file changed, 5 insertions(+), 61 deletions(-)

(limited to 'main.cpp')

diff --git a/main.cpp b/main.cpp
index 3b37344..7589f35 100644
--- a/main.cpp
+++ b/main.cpp
@@ -4,6 +4,7 @@
 #include "bma150.h"
 #include "ak8975.h"
 #include "ppmsum.h"
+#include "motormixer.h"
 
 #include <ch.h>
 #include <hal.h>
@@ -28,6 +29,7 @@ class LEDThread : public BaseThread<LEDThread, 128> {
 LEDThread led_thread;
 
 PPMSum ppmsum;
+MotorMixer motors;
 
 class USBThread : public BaseThread<USBThread, 256> {
 	private:
@@ -119,8 +121,6 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 				sensordata[10] = roll;
 				sensordata[11] = yaw;
 				
-				
-				
 				usbs.write(syncword, sizeof(syncword));
 				usbs.write(buf, sizeof(buf));
 				
@@ -131,6 +131,8 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
 					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
 				
+				motors.update(ppmsum.data[2], 0, 0, 0);
+				
 				time += MS2ST(10);
 				if(time > chTimeNow()) {
 					chThdSleepUntil(time);
@@ -176,55 +178,6 @@ class ADCThread : public BaseThread<ADCThread, 128> {
 
 ADCThread adc_thread;
 
-void foo(PWMDriver*) {
-	
-	int16_t thrust, pitch, roll, yaw;
-	
-	thrust = ppmsum.data[2];
-	if(thrust < 0) {
-		thrust = 0;
-	}
-	if(thrust > 1000) {
-		thrust = 1000;
-	}
-	
-	pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
-	roll  = ((ppmsum.data[0] - 500) * thrust) >> 10;
-	yaw   = ((ppmsum.data[3] - 500) * thrust) >> 10;
-	
-	//pitch = (-i2c_thread.x) >> 5;
-	//roll = i2c_thread.y >> 5;
-	//yaw = i2c_thread.z >> 5;
-	
-	int16_t motor_1 = thrust + pitch + roll - yaw;
-	int16_t motor_2 = thrust + pitch - roll + yaw;
-	int16_t motor_3 = thrust - pitch + roll + yaw;
-	int16_t motor_4 = thrust - pitch - roll - yaw;
-	
-	sensordata[20] = motor_1;
-	sensordata[21] = motor_2;
-	sensordata[22] = motor_3;
-	sensordata[23] = motor_4;
-	
-	pwmEnableChannel(&PWMD2, 0, 1000 + motor_1);
-	pwmEnableChannel(&PWMD2, 1, 1000 + motor_2);
-	pwmEnableChannel(&PWMD2, 2, 1000 + motor_3);
-	pwmEnableChannel(&PWMD2, 3, 1000 + motor_4);
-}
-
-static PWMConfig pwmcfg = {
-	1000000,
-	1000000 / 50,
-	foo,
-	{
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL}
-	},
-	0
-};
-
 int main(void) {
 	halInit();
 	chSysInit();
@@ -239,16 +192,7 @@ int main(void) {
 	
 	adc_thread.start();
 	
-	pwmStart(&PWMD2, &pwmcfg);
-	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-	palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-	palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-	palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-	
-	pwmEnableChannel(&PWMD2, 0, 1000);
-	pwmEnableChannel(&PWMD2, 1, 1000);
-	pwmEnableChannel(&PWMD2, 2, 1000);
-	pwmEnableChannel(&PWMD2, 3, 1000);
+	motors.start();
 	
 	usb_thread.usbs = &usbs;
 	usb_thread.start();
-- 
cgit v1.2.3