Danielle Kruijver / Mbed 2 deprecated Controll_SBT_main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos

Force_to_wing_angle.cpp@1:3d46a0660d05, 2019-04-12 (annotated)

Committer:
DanielleKruijver
Date:
Fri Apr 12 09:55:53 2019 +0000
Revision:
1:3d46a0660d05
Parent:
0:3fbb3fa4ff64 
Main SBT controllll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Awiegmink 0:3fbb3fa4ff64 1 /*
Awiegmink 0:3fbb3fa4ff64 2 * File: Force_to_wing_angle.cpp
Awiegmink 0:3fbb3fa4ff64 3 * Author: User
Awiegmink 0:3fbb3fa4ff64 4 *
Awiegmink 0:3fbb3fa4ff64 5 * Created on 28 februari 2019, 10:05
Awiegmink 0:3fbb3fa4ff64 6 */
Awiegmink 0:3fbb3fa4ff64 7 #include "mbed.h"
Awiegmink 0:3fbb3fa4ff64 8 #include "Force_to_wing_angle.h"
Awiegmink 0:3fbb3fa4ff64 9 #include "DataStore.h"
Awiegmink 0:3fbb3fa4ff64 10 #include <iostream>
Awiegmink 0:3fbb3fa4ff64 11 #include <cmath>
Awiegmink 0:3fbb3fa4ff64 12 #include "math.h"
DanielleKruijver 1:3d46a0660d05 13
Awiegmink 0:3fbb3fa4ff64 14 control::ForceToWingAngle::ForceToWingAngle(){
Awiegmink 0:3fbb3fa4ff64 15 }
Awiegmink 0:3fbb3fa4ff64 16
Awiegmink 0:3fbb3fa4ff64 17 control::ForceToWingAngle::ForceToWingAngle(const ForceToWingAngle& orig) {
Awiegmink 0:3fbb3fa4ff64 18 }
Awiegmink 0:3fbb3fa4ff64 19
Awiegmink 0:3fbb3fa4ff64 20 control::ForceToWingAngle::~ForceToWingAngle() {
Awiegmink 0:3fbb3fa4ff64 21 }
Awiegmink 0:3fbb3fa4ff64 22
Awiegmink 0:3fbb3fa4ff64 23 void control::ForceToWingAngle::MMA() {
DanielleKruijver 1:3d46a0660d05 24
Awiegmink 0:3fbb3fa4ff64 25 /* First we will start with the MMA. This is the motor mixing algorithm that
Awiegmink 0:3fbb3fa4ff64 26 * divides the incoming forces from the 3 PID controllers in 3 different
Awiegmink 0:3fbb3fa4ff64 27 * forces that the wings should deliver. The MMA consists of a matrix, that
Awiegmink 0:3fbb3fa4ff64 28 * has to be inversed to calculate the right forces per wing.
Awiegmink 0:3fbb3fa4ff64 29 */
Awiegmink 0:3fbb3fa4ff64 30 DataStore::PIDDataTotal input =m_PID_data-> GetPIDData();
Awiegmink 0:3fbb3fa4ff64 31 DataStore::RealData input2 =m_complementary_data-> GetComplementaryData();
Awiegmink 0:3fbb3fa4ff64 32 DataStore::AngleWings output;
Awiegmink 0:3fbb3fa4ff64 33
Awiegmink 0:3fbb3fa4ff64 34 float velocity = 5;
Awiegmink 0:3fbb3fa4ff64 35 int i;
Awiegmink 0:3fbb3fa4ff64 36 float matrix_MMA[3][3] = {
Awiegmink 0:3fbb3fa4ff64 37 {1, 1, 1},
Awiegmink 0:3fbb3fa4ff64 38 {1.54, 1.54, -2.83},
Awiegmink 0:3fbb3fa4ff64 39 {-0.37, 0.37, 0},
Awiegmink 0:3fbb3fa4ff64 40 };
Awiegmink 0:3fbb3fa4ff64 41
Awiegmink 0:3fbb3fa4ff64 42 float inverse_matrix_MMA[3][3];
Awiegmink 0:3fbb3fa4ff64 43 float determinant = 0;
Awiegmink 0:3fbb3fa4ff64 44
Awiegmink 0:3fbb3fa4ff64 45 // finding determinant:
Awiegmink 0:3fbb3fa4ff64 46 for(i = 0; i < 3; i++){
Awiegmink 0:3fbb3fa4ff64 47 determinant = determinant + (matrix_MMA[0][i] * (matrix_MMA[1][(i+1)%3] * matrix_MMA[2][(i+2)%3] - matrix_MMA[1][(i+2)%3] * matrix_MMA[2][(i+1)%3]));
Awiegmink 0:3fbb3fa4ff64 48 }
Awiegmink 0:3fbb3fa4ff64 49 // std::cout<<"\n\nInverse of matrix is: \n";
Awiegmink 0:3fbb3fa4ff64 50 //for(i = 0; i < 3; i++){
Awiegmink 0:3fbb3fa4ff64 51 //for(j = 0; j < 3; j++)
Awiegmink 0:3fbb3fa4ff64 52 //std::cout<<((matrix_MMA[(j+1)%3][(i+1)%3] * matrix_MMA[(j+2)%3][(i+2)%3]) - (matrix_MMA[(j+1)%3][(i+2)%3] * matrix_MMA[(j+2)%3][(i+1)%3]))/ determinant<<"\t";
Awiegmink 0:3fbb3fa4ff64 53 //std::cout<<"\n";
Awiegmink 0:3fbb3fa4ff64 54 //}
Awiegmink 0:3fbb3fa4ff64 55 //calculate inverse matrix and save values []][] = [row][colomn]
Awiegmink 0:3fbb3fa4ff64 56 inverse_matrix_MMA[0][0] = ((matrix_MMA[(0+1)%3][(0+1)%3] * matrix_MMA[(0+2)%3][(0+2)%3]) - (matrix_MMA[(0+1)%3][(0+2)%3] * matrix_MMA[(0+2)%3][(0+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 57 //std::cout << (inverse_matrix_MMA[0][0]) << "\t";
Awiegmink 0:3fbb3fa4ff64 58 inverse_matrix_MMA[0][1] = ((matrix_MMA[(1+1)%3][(0+1)%3] * matrix_MMA[(1+2)%3][(0+2)%3]) - (matrix_MMA[(1+1)%3][(0+2)%3] * matrix_MMA[(1+2)%3][(0+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 59 inverse_matrix_MMA[0][2] = ((matrix_MMA[(2+1)%3][(0+1)%3] * matrix_MMA[(2+2)%3][(0+2)%3]) - (matrix_MMA[(2+1)%3][(0+2)%3] * matrix_MMA[(2+2)%3][(0+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 60 inverse_matrix_MMA[1][0] = ((matrix_MMA[(0+1)%3][(1+1)%3] * matrix_MMA[(0+2)%3][(1+2)%3]) - (matrix_MMA[(0+1)%3][(1+2)%3] * matrix_MMA[(0+2)%3][(1+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 61 inverse_matrix_MMA[1][1] = ((matrix_MMA[(1+1)%3][(1+1)%3] * matrix_MMA[(1+2)%3][(1+2)%3]) - (matrix_MMA[(1+1)%3][(1+2)%3] * matrix_MMA[(1+2)%3][(1+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 62 inverse_matrix_MMA[1][2] = ((matrix_MMA[(2+1)%3][(1+1)%3] * matrix_MMA[(2+2)%3][(1+2)%3]) - (matrix_MMA[(2+1)%3][(1+2)%3] * matrix_MMA[(2+2)%3][(1+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 63 inverse_matrix_MMA[2][0] = ((matrix_MMA[(0+1)%3][(2+1)%3] * matrix_MMA[(0+2)%3][(2+2)%3]) - (matrix_MMA[(0+1)%3][(2+2)%3] * matrix_MMA[(0+2)%3][(2+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 64 inverse_matrix_MMA[2][1] = ((matrix_MMA[(1+1)%3][(2+1)%3] * matrix_MMA[(1+2)%3][(2+2)%3]) - (matrix_MMA[(1+1)%3][(2+2)%3] * matrix_MMA[(1+2)%3][(2+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 65 inverse_matrix_MMA[2][2] = ((matrix_MMA[(2+1)%3][(2+1)%3] * matrix_MMA[(2+2)%3][(2+2)%3]) - (matrix_MMA[(2+1)%3][(2+2)%3] * matrix_MMA[(2+2)%3][(2+1)%3]))/ determinant;
Awiegmink 0:3fbb3fa4ff64 66 //std::cout << (inverse_matrix_MMA[1][1]) << "\t";
Awiegmink 0:3fbb3fa4ff64 67
Awiegmink 0:3fbb3fa4ff64 68
Awiegmink 0:3fbb3fa4ff64 69 float left_force = inverse_matrix_MMA[0][0] * input.Force_height + inverse_matrix_MMA[0][1] * input.Force_pitch + inverse_matrix_MMA[0][2] * input.Force_roll;
Awiegmink 0:3fbb3fa4ff64 70 float right_force = inverse_matrix_MMA[1][0] * input.Force_height + inverse_matrix_MMA[1][1] * input.Force_pitch + inverse_matrix_MMA[1][2] * input.Force_roll;
Awiegmink 0:3fbb3fa4ff64 71 float back_force = inverse_matrix_MMA[2][0] * input.Force_height + inverse_matrix_MMA[2][1] * input.Force_pitch + inverse_matrix_MMA[2][2] * input.Force_roll;
Awiegmink 0:3fbb3fa4ff64 72
Awiegmink 0:3fbb3fa4ff64 73 /* Now we know the forces that each wing should generate to fly, we should
Awiegmink 0:3fbb3fa4ff64 74 * also calculate the angle that the wings should have to generate this
Awiegmink 0:3fbb3fa4ff64 75 * force. This is done with the equation for lift: L=0.5*rho*v^2*Cl;
Awiegmink 0:3fbb3fa4ff64 76 */
Awiegmink 0:3fbb3fa4ff64 77 const float kLeftSurface = 0.5*(35832.62+9379.39+15937.11+43774.74) * 0.000006; //m2
Awiegmink 0:3fbb3fa4ff64 78 const float kRightSurface = 0.5*(35832.62+9379.39+15937.11+43774.74) * 0.000006;
Awiegmink 0:3fbb3fa4ff64 79 const float kBackSurface = 0.5*(37297+37930+10794+10794) * 0.000006;
Awiegmink 0:3fbb3fa4ff64 80 const float kDensity = 1025; //kg/m3 density salt water
Awiegmink 0:3fbb3fa4ff64 81 const int kLiftSlope = 5;
Awiegmink 0:3fbb3fa4ff64 82 const float kZeroLiftAngle = -0.05236; //radians
Awiegmink 0:3fbb3fa4ff64 83
Awiegmink 0:3fbb3fa4ff64 84 //test to check the surface std::cout << (kLeftSurface) << '\t';
Awiegmink 0:3fbb3fa4ff64 85
Awiegmink 0:3fbb3fa4ff64 86
Awiegmink 0:3fbb3fa4ff64 87 //devide the force by (0.5*density*surface*velocity) to get the lift coefficient
Awiegmink 0:3fbb3fa4ff64 88 float left_lift_coefficient = left_force / (0.5 * kDensity * kLeftSurface * velocity) ;
Awiegmink 0:3fbb3fa4ff64 89 float right_lift_coefficient = right_force / (0.5 * kDensity * kRightSurface * velocity) ;
Awiegmink 0:3fbb3fa4ff64 90 float back_lift_coefficient = back_force / (0.5 * kDensity * kBackSurface * velocity) ;
Awiegmink 0:3fbb3fa4ff64 91
Awiegmink 0:3fbb3fa4ff64 92 float left_angle_total = left_lift_coefficient / kLiftSlope;
Awiegmink 0:3fbb3fa4ff64 93 float right_angle_total = right_lift_coefficient / kLiftSlope;
Awiegmink 0:3fbb3fa4ff64 94 float back_angle_total = back_lift_coefficient / kLiftSlope;
Awiegmink 0:3fbb3fa4ff64 95
Awiegmink 0:3fbb3fa4ff64 96 //calculate the final wing angles
Awiegmink 0:3fbb3fa4ff64 97 output.Wing_left = left_angle_total - input2.Real_pitch + kZeroLiftAngle;
Awiegmink 0:3fbb3fa4ff64 98 output.Wing_right = right_angle_total - input2.Real_pitch + kZeroLiftAngle;
Awiegmink 0:3fbb3fa4ff64 99 output.Wing_back = back_angle_total - input2.Real_pitch + kZeroLiftAngle;
Awiegmink 0:3fbb3fa4ff64 100
Awiegmink 0:3fbb3fa4ff64 101 m_FtoW_data->PutWingData(&output);
Awiegmink 0:3fbb3fa4ff64 102 return ;
Awiegmink 0:3fbb3fa4ff64 103 /*Now we can calculate the angle that the maxon motor should make in such
Awiegmink 0:3fbb3fa4ff64 104 * a way that the wing is making the right angle.
Awiegmink 0:3fbb3fa4ff64 105 */
Awiegmink 0:3fbb3fa4ff64 106
Awiegmink 0:3fbb3fa4ff64 107
Awiegmink 0:3fbb3fa4ff64 108 }
Awiegmink 0:3fbb3fa4ff64 109