David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@0:4c04e4fd1310, 2016-03-30 (annotated)

Committer:
ivo_david_michelle
Date:
Wed Mar 30 16:05:30 2016 +0000
Revision:
0:4c04e4fd1310
Child:
1:b87e95907a18
right after ivo stopped working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 0:4c04e4fd1310 1 #include "mbed.h"
ivo_david_michelle 0:4c04e4fd1310 2
ivo_david_michelle 0:4c04e4fd1310 3 Serial pc(USBTX, USBRX);
ivo_david_michelle 0:4c04e4fd1310 4
ivo_david_michelle 0:4c04e4fd1310 5 double controller(double phi)
ivo_david_michelle 0:4c04e4fd1310 6 {
ivo_david_michelle 0:4c04e4fd1310 7 double g=9.81; // gravity [m/s^2]
ivo_david_michelle 0:4c04e4fd1310 8 double m=1; // mass [kg]
ivo_david_michelle 0:4c04e4fd1310 9
ivo_david_michelle 0:4c04e4fd1310 10 // proportional attitude control gains
ivo_david_michelle 0:4c04e4fd1310 11 double kp_phi = 1;
ivo_david_michelle 0:4c04e4fd1310 12 double kp_theta = 1;
ivo_david_michelle 0:4c04e4fd1310 13 double kp_psi = 1;
ivo_david_michelle 0:4c04e4fd1310 14
ivo_david_michelle 0:4c04e4fd1310 15 // derivative attitude control gains
ivo_david_michelle 0:4c04e4fd1310 16 double kd_phi = 0;
ivo_david_michelle 0:4c04e4fd1310 17 double kd_theta = 0;
ivo_david_michelle 0:4c04e4fd1310 18 double kd_psi = 0;
ivo_david_michelle 0:4c04e4fd1310 19
ivo_david_michelle 0:4c04e4fd1310 20 // measured values (will come from IMU/parameter class/Input to function later)
ivo_david_michelle 0:4c04e4fd1310 21 // double phi = 0; // roll // commented out, because input to function
ivo_david_michelle 0:4c04e4fd1310 22 double theta = 0; // pitch
ivo_david_michelle 0:4c04e4fd1310 23 double psi = 0; // yaw
ivo_david_michelle 0:4c04e4fd1310 24
ivo_david_michelle 0:4c04e4fd1310 25 // angular velocities in body coordinate system
ivo_david_michelle 0:4c04e4fd1310 26 double p = 0; // omega_x
ivo_david_michelle 0:4c04e4fd1310 27 double q = 0; // omega_y
ivo_david_michelle 0:4c04e4fd1310 28 double r = 0; // omega_z
ivo_david_michelle 0:4c04e4fd1310 29
ivo_david_michelle 0:4c04e4fd1310 30 // desired values
ivo_david_michelle 0:4c04e4fd1310 31 double F_des = 0; // desired thrust force (excluding weight compensation)
ivo_david_michelle 0:4c04e4fd1310 32 double phi_des = 0; // roll
ivo_david_michelle 0:4c04e4fd1310 33 double theta_des = 0; // pitch
ivo_david_michelle 0:4c04e4fd1310 34 double psi_des = 0; // yaw
ivo_david_michelle 0:4c04e4fd1310 35 // these will probably not be changed -> we could use const keyword in case of storage shortage
ivo_david_michelle 0:4c04e4fd1310 36 double p_des = 0; // omega_x
ivo_david_michelle 0:4c04e4fd1310 37 double q_des = 0; // omega_y
ivo_david_michelle 0:4c04e4fd1310 38 double r_des = 0; // omega_z
ivo_david_michelle 0:4c04e4fd1310 39
ivo_david_michelle 0:4c04e4fd1310 40 // total thrust force
ivo_david_michelle 0:4c04e4fd1310 41 double F=0;
ivo_david_michelle 0:4c04e4fd1310 42 // control moments
ivo_david_michelle 0:4c04e4fd1310 43 double M_x=0;
ivo_david_michelle 0:4c04e4fd1310 44 double M_y=0;
ivo_david_michelle 0:4c04e4fd1310 45 double M_z=0;
ivo_david_michelle 0:4c04e4fd1310 46
ivo_david_michelle 0:4c04e4fd1310 47 // get IMU data
ivo_david_michelle 0:4c04e4fd1310 48 // phi = 1;
ivo_david_michelle 0:4c04e4fd1310 49 theta =0;
ivo_david_michelle 0:4c04e4fd1310 50 psi =0;
ivo_david_michelle 0:4c04e4fd1310 51 p = 0;
ivo_david_michelle 0:4c04e4fd1310 52 q = 0;
ivo_david_michelle 0:4c04e4fd1310 53 r = 0;
ivo_david_michelle 0:4c04e4fd1310 54
ivo_david_michelle 0:4c04e4fd1310 55 // compute desired angles (in the case we decide not to set
ivo_david_michelle 0:4c04e4fd1310 56 // the angles, but for instance the velocity with the Joystick
ivo_david_michelle 0:4c04e4fd1310 57
ivo_david_michelle 0:4c04e4fd1310 58 // desired values
ivo_david_michelle 0:4c04e4fd1310 59 // dirctly from Joystick, or compute from desired velocities.
ivo_david_michelle 0:4c04e4fd1310 60 F_des=1;
ivo_david_michelle 0:4c04e4fd1310 61 phi_des = 0;
ivo_david_michelle 0:4c04e4fd1310 62 theta_des =1;
ivo_david_michelle 0:4c04e4fd1310 63 psi_des =0;
ivo_david_michelle 0:4c04e4fd1310 64
ivo_david_michelle 0:4c04e4fd1310 65 // PD controller
ivo_david_michelle 0:4c04e4fd1310 66 F = m*g + F_des;
ivo_david_michelle 0:4c04e4fd1310 67 M_x = kp_phi*(phi_des-phi)+kd_phi*(p_des-p);
ivo_david_michelle 0:4c04e4fd1310 68 M_y = kp_theta*(theta_des-theta)+kd_theta*(q_des-q);
ivo_david_michelle 0:4c04e4fd1310 69 M_z = kp_psi*(psi_des-psi)+kd_psi*(r_des-r);
ivo_david_michelle 0:4c04e4fd1310 70
ivo_david_michelle 0:4c04e4fd1310 71 //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z);
ivo_david_michelle 0:4c04e4fd1310 72 // pc.printf("F: %f\n\r", F);
ivo_david_michelle 0:4c04e4fd1310 73
ivo_david_michelle 0:4c04e4fd1310 74 // pc.printf("Mx: %f\n\r", F);
ivo_david_michelle 0:4c04e4fd1310 75 //pc.printf("F: %f\n\r", F);
ivo_david_michelle 0:4c04e4fd1310 76
ivo_david_michelle 0:4c04e4fd1310 77 return M_x;
ivo_david_michelle 0:4c04e4fd1310 78 }
ivo_david_michelle 0:4c04e4fd1310 79
ivo_david_michelle 0:4c04e4fd1310 80 int main()
ivo_david_michelle 0:4c04e4fd1310 81 {
ivo_david_michelle 0:4c04e4fd1310 82 double F=0;
ivo_david_michelle 0:4c04e4fd1310 83 double M_x=0;
ivo_david_michelle 0:4c04e4fd1310 84 double M_y=0;
ivo_david_michelle 0:4c04e4fd1310 85 double M_z=0;
ivo_david_michelle 0:4c04e4fd1310 86
ivo_david_michelle 0:4c04e4fd1310 87 double *orientation = (double *) malloc(6 * sizeof(double));
ivo_david_michelle 0:4c04e4fd1310 88
ivo_david_michelle 0:4c04e4fd1310 89 while (1) {
ivo_david_michelle 0:4c04e4fd1310 90 // get sensor values
ivo_david_michelle 0:4c04e4fd1310 91 double phi=1;
ivo_david_michelle 0:4c04e4fd1310 92
ivo_david_michelle 0:4c04e4fd1310 93 // call controller
ivo_david_michelle 0:4c04e4fd1310 94 M_x= controller(phi);
ivo_david_michelle 0:4c04e4fd1310 95
ivo_david_michelle 0:4c04e4fd1310 96 // plot
ivo_david_michelle 0:4c04e4fd1310 97
ivo_david_michelle 0:4c04e4fd1310 98 //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z);
ivo_david_michelle 0:4c04e4fd1310 99 pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z);
ivo_david_michelle 0:4c04e4fd1310 100 }
ivo_david_michelle 0:4c04e4fd1310 101 free(orientation);
ivo_david_michelle 0:4c04e4fd1310 102 }