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?
User | Revision | Line number | New 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 | } |