Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Mixer/Mixer.cpp@40:2ca410923691, 2014-02-14 (annotated)

Committer:
maetugr
Date:
Fri Feb 14 14:17:32 2014 +0000
Revision:
40:2ca410923691
Parent:
38:ff95fd524c9e 
now with MPU6050 before taking it too FlyBed2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 26:96a072233d7a 1 #include "Mixer.h"
maetugr 26:96a072233d7a 2
maetugr 30:021e13b62575 3 Mixer::Mixer(int Configuration)
maetugr 26:96a072233d7a 4 {
maetugr 30:021e13b62575 5 Mixer::Configuration = Configuration;
maetugr 30:021e13b62575 6
maetugr 26:96a072233d7a 7 for(int i=0; i<4; i++)
maetugr 26:96a072233d7a 8 Motor_speed[i]=0;
maetugr 26:96a072233d7a 9 }
maetugr 26:96a072233d7a 10
maetugr 33:fd98776b6cc7 11 void Mixer::compute(int Throttle, const float * controller_value)
maetugr 26:96a072233d7a 12 {
maetugr 30:021e13b62575 13 // Mixing tables for each configuration
maetugr 30:021e13b62575 14 float mix_table[2][4][3] = {
maetugr 30:021e13b62575 15 {
maetugr 30:021e13b62575 16 { 0, 1, 1}, // + configuration
maetugr 30:021e13b62575 17 { 1, 0, -1},
maetugr 30:021e13b62575 18 { 0, -1, 1},
maetugr 30:021e13b62575 19 { -1, 0, -1}
maetugr 30:021e13b62575 20 },
maetugr 30:021e13b62575 21 {
maetugr 34:3aa1cbcde59d 22 { RT, -RT, 1}, // X configuration
maetugr 34:3aa1cbcde59d 23 { RT, RT, -1},
maetugr 34:3aa1cbcde59d 24 { -RT, RT, 1},
maetugr 34:3aa1cbcde59d 25 { -RT, -RT, -1}
maetugr 30:021e13b62575 26 }
maetugr 30:021e13b62575 27 };
maetugr 29:8b7362a2ee14 28
maetugr 30:021e13b62575 29 // Calculate new motorspeeds
maetugr 30:021e13b62575 30 for(int i=0; i<4; i++) {
maetugr 30:021e13b62575 31 Motor_speed[i] = Throttle;
maetugr 30:021e13b62575 32 for(int j = 0; j < 3; j++)
maetugr 30:021e13b62575 33 Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
maetugr 30:021e13b62575 34 }
maetugr 26:96a072233d7a 35
maetugr 37:34917f7c10ae 36 for(int i = 0; i < 4; i++) { // make sure no motor stands still or gets a higher speed than 1000
maetugr 38:ff95fd524c9e 37 Motor_speed[i] = Motor_speed[i] > 70 ? Motor_speed[i] : 70;
maetugr 37:34917f7c10ae 38 Motor_speed[i] = Motor_speed[i] <= 1000 ? Motor_speed[i] : 1000;
maetugr 37:34917f7c10ae 39 }
maetugr 26:96a072233d7a 40 }