Mixer.cpp

00001 #include "Mixer.h"
00002 
00003 Mixer::Mixer(int Configuration)
00004 {
00005     Mixer::Configuration = Configuration;
00006     
00007     for(int i=0; i<4; i++)
00008         Motor_speed[i]=0;
00009 }
00010 
00011 void Mixer::compute(int Throttle, const float * controller_value)
00012 {
00013     // Mixing tables for each configuration
00014     float mix_table[2][4][3] = {
00015     {
00016         {   0,  1,  1},       // + configuration
00017         {   1,  0, -1},
00018         {   0, -1,  1},
00019         {  -1,  0, -1}
00020     },
00021     {
00022         {  RT, -RT,  1},       // X configuration
00023         {  RT,  RT, -1},
00024         { -RT,  RT,  1},
00025         { -RT, -RT, -1}
00026     }
00027     };
00028     
00029     // Calculate new motorspeeds
00030     for(int i=0; i<4; i++) {
00031         Motor_speed[i] = Throttle;
00032         for(int j = 0; j < 3; j++)
00033             Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
00034     }
00035     
00036     for(int i = 0; i < 4; i++) { // make sure no motor stands still or gets a higher speed than 1000
00037         Motor_speed[i] = Motor_speed[i] > 70 ? Motor_speed[i] : 70;
00038         Motor_speed[i] = Motor_speed[i] <= 1000 ? Motor_speed[i] : 1000;
00039     }
00040 }