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.
Mixer/Mixer.cpp
- Committer:
- maetugr
- Date:
- 2013-06-12
- Revision:
- 37:34917f7c10ae
- Parent:
- 34:3aa1cbcde59d
- Child:
- 38:ff95fd524c9e
File content as of revision 37:34917f7c10ae:
#include "Mixer.h" Mixer::Mixer(int Configuration) { Mixer::Configuration = Configuration; for(int i=0; i<4; i++) Motor_speed[i]=0; } void Mixer::compute(int Throttle, const float * controller_value) { // Mixing tables for each configuration float mix_table[2][4][3] = { { { 0, 1, 1}, // + configuration { 1, 0, -1}, { 0, -1, 1}, { -1, 0, -1} }, { { RT, -RT, 1}, // X configuration { RT, RT, -1}, { -RT, RT, 1}, { -RT, -RT, -1} } }; // Calculate new motorspeeds for(int i=0; i<4; i++) { Motor_speed[i] = Throttle; for(int j = 0; j < 3; j++) Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j]; } for(int i = 0; i < 4; i++) { // make sure no motor stands still or gets a higher speed than 1000 Motor_speed[i] = Motor_speed[i] > 150 ? Motor_speed[i] : 150; Motor_speed[i] = Motor_speed[i] <= 1000 ? Motor_speed[i] : 1000; } }