Diff: main.cpp

Revision:

0:4c04e4fd1310

Child:

1:b87e95907a18

--- /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);
+}