gpa in double prescision

Dependencies:   mbed

Revision:
6:7ea1a4bac3c2
Parent:
5:e443d5ad409f
Child:
7:b3c5116e9fab
--- a/main.cpp	Thu Mar 01 17:36:46 2018 +0000
+++ b/main.cpp	Thu Mar 01 17:47:55 2018 +0000
@@ -42,36 +42,36 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
 InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
-
 AnalogIn ax(PA_0);                      // analog IN (acc x) on PA_0
 AnalogIn ay(PA_1);                      // analog IN (acc y) on PA_1
 AnalogIn gz(PA_4);                      // analog IN (gyr z) on PB_0
 AnalogOut out(PA_5);                    // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
-
 Ticker  ControllerLoopTimer;            // interrupt for control loop
 Timer t;                                // timer to analyse button
 
+// controller parameters etc.
 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
 float kp = 4.0f;                        // speed control gain for motor speed cntrl.
 float Tn = 0.05f;                       // integral time       "     "        "
 float Ts = 0.0025;                      // sample time
 float v_max = 200;                      // maximum speed rad/s
 float n_soll = 0.0f;                    // nominal speed for speed control tests
+float comp_filter_tau =1.0f;            // time constant of complementary filter
 
-unsigned int k = 0;                     // standart counter for output 
-bool isStanding = 0;                    // state if the cube is standing or not
+// output and statemachine
+unsigned int k = 0;                     // counter for serial output
+bool doesStand = 0;                     // state if the cube is standing or not
 bool shouldBalance = 0;                 // state if the controller is active
 
-float comp_filter_tau =1.0f;            // time constant of complementary filter  
-            
+// set up encoder, controller and filter instanzes           
 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
 DiffCounter diff(0.01, Ts);             // discrete differentiate, based on encoder data
 PI_Cntrl pi_w2zero(-.01f, 1.0f, 0.4f);  // controller to bring motor speed to zero while balancing
-
 IIR_filter f_ax(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_x
 IIR_filter f_ay(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_y
 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
 
+// linear characteristics
 LinearCharacteristics i2u(0.1067f,-15.0f);          // full range, convert desired current (Amps)  -> voltage 0..3.3V
 LinearCharacteristics u2n(312.5f,1.6f);             // convert input voltage (0..3.3V) -> speed (1/min)
 LinearCharacteristics u2w(32.725,1.6f);             // convert input voltage (0..3.3V) -> speed (rad/sec)
@@ -85,8 +85,8 @@
 void pressed(void);             // user button pressed
 void released(void);            // user button released
 
+// main program and control loop
 // -----------------------------------------------------------------------------
-
 int main()
 {
     // for serial comm.
@@ -120,10 +120,10 @@
     // get current state of the cube
     float actualAngleDegree = ang*180.0f/PI;
     if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
-        isStanding = 1;
+        doesStand = 1;
     }
     else{
-        isStanding = 0;
+        doesStand = 0;
     }
     
     // update controller
@@ -160,8 +160,8 @@
     t.stop();
     t.reset();
     
-    // if the cube isStanding
-    if(isStanding)
+    // if the cube doesStand
+    if(doesStand)
     {
         // in - or decrease speed 
         if(buttonTime < 2.0f){