From 280d688219185a1079fc6e995658341c4a9127c5 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 28 Apr 2011 17:26:47 +0200 Subject: Move pwm output code into MotorMixer-class. --- main.cpp | 66 +++++-------------------------------------------------- motormixer.cpp | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ motormixer.h | 12 ++++++++++ 3 files changed, 86 insertions(+), 61 deletions(-) create mode 100644 motormixer.cpp create mode 100644 motormixer.h 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 #include @@ -28,6 +29,7 @@ class LEDThread : public BaseThread { LEDThread led_thread; PPMSum ppmsum; +MotorMixer motors; class USBThread : public BaseThread { private: @@ -119,8 +121,6 @@ class I2CThread : public BaseThread { sensordata[10] = roll; sensordata[11] = yaw; - - usbs.write(syncword, sizeof(syncword)); usbs.write(buf, sizeof(buf)); @@ -131,6 +131,8 @@ class I2CThread : public BaseThread { 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 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(); diff --git a/motormixer.cpp b/motormixer.cpp new file mode 100644 index 0000000..1951f29 --- /dev/null +++ b/motormixer.cpp @@ -0,0 +1,69 @@ +#include "motormixer.h" + +#include +#include + +namespace { + uint16_t motors[4]; + + void foo(PWMDriver*) { + pwmEnableChannel(&PWMD2, 0, 1000 + motors[0]); + pwmEnableChannel(&PWMD2, 1, 1000 + motors[1]); + pwmEnableChannel(&PWMD2, 2, 1000 + motors[2]); + pwmEnableChannel(&PWMD2, 3, 1000 + motors[3]); + } + + 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 + }; +}; + +void MotorMixer::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); +} + +void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t 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; + + motors[0] = thrust + pitch + roll - yaw; + motors[1] = thrust + pitch - roll + yaw; + motors[2] = thrust - pitch + roll + yaw; + motors[3] = thrust - pitch - roll - yaw; + + //sensordata[20] = motor_1; + //sensordata[21] = motor_2; + //sensordata[22] = motor_3; + //sensordata[23] = motor_4; +} diff --git a/motormixer.h b/motormixer.h new file mode 100644 index 0000000..5ec1869 --- /dev/null +++ b/motormixer.h @@ -0,0 +1,12 @@ +#ifndef MOTORMIXER_H +#define MOTORMIXER_H + +#include + +class MotorMixer { + public: + void start(); + void update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw); +}; + +#endif -- cgit v1.2.3