ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:4c04e4fd1310
- Child:
- 1:b87e95907a18
diff -r 000000000000 -r 4c04e4fd1310 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 30 16:05:30 2016 +0000 @@ -0,0 +1,102 @@ +#include "mbed.h" + +Serial pc(USBTX, USBRX); + +double controller(double phi) +{ + double g=9.81; // gravity [m/s^2] + double m=1; // mass [kg] + + // proportional attitude control gains + double kp_phi = 1; + double kp_theta = 1; + double kp_psi = 1; + + // derivative attitude control gains + double kd_phi = 0; + double kd_theta = 0; + double kd_psi = 0; + + // measured values (will come from IMU/parameter class/Input to function later) + // double phi = 0; // roll // commented out, because input to function + double theta = 0; // pitch + double psi = 0; // yaw + + // angular velocities in body coordinate system + double p = 0; // omega_x + double q = 0; // omega_y + double r = 0; // omega_z + + // desired values + double F_des = 0; // desired thrust force (excluding weight compensation) + double phi_des = 0; // roll + double theta_des = 0; // pitch + double psi_des = 0; // yaw + // these will probably not be changed -> we could use const keyword in case of storage shortage + double p_des = 0; // omega_x + double q_des = 0; // omega_y + double r_des = 0; // omega_z + + // total thrust force + double F=0; + // control moments + double M_x=0; + double M_y=0; + double M_z=0; + + // get IMU data + // phi = 1; + theta =0; + psi =0; + p = 0; + q = 0; + r = 0; + + // compute desired angles (in the case we decide not to set + // the angles, but for instance the velocity with the Joystick + + // desired values + // dirctly from Joystick, or compute from desired velocities. + F_des=1; + phi_des = 0; + theta_des =1; + psi_des =0; + + // PD controller + F = m*g + F_des; + M_x = kp_phi*(phi_des-phi)+kd_phi*(p_des-p); + M_y = kp_theta*(theta_des-theta)+kd_theta*(q_des-q); + M_z = kp_psi*(psi_des-psi)+kd_psi*(r_des-r); + + //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z); + // pc.printf("F: %f\n\r", F); + + // pc.printf("Mx: %f\n\r", F); + //pc.printf("F: %f\n\r", F); + + return M_x; +} + +int main() +{ + double F=0; + double M_x=0; + double M_y=0; + double M_z=0; + + double *orientation = (double *) malloc(6 * sizeof(double)); + + while (1) { + // get sensor values + double phi=1; + + // call controller + M_x= controller(phi); + + // plot + + //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z); + pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z); + } + free(orientation); +}