Diff: main.cpp

Revision:

8:d68e177e2571

Parent:

5:d6c7ccbbce78

Child:

9:30ee1d386a1d

--- a/main.cpp	Sat Mar 17 08:43:07 2018 +0000
+++ b/main.cpp	Thu Mar 22 17:32:37 2018 +0000
@@ -53,7 +53,7 @@
 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 tau = 1.0f;            // time constant of complementary filter
+float tau = 1.05f;            // time constant of complementary filter
 float fg = 300.0f;
 
 // output and statemachine
@@ -62,29 +62,36 @@
 bool shouldBalance = 0;                 // state if the controller is active
 
 // set up encoder
-EncoderCounter MotorEncoder(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
-DiffCounter MotorDiff(0.01, Ts);             // discrete differentiate, based on encoder data
+EncoderCounter MotorEncoder(PB_6, PB_7);         // initialize counter on PB_6 and PB_7
+DiffCounter MotorDiff(1/(2.0f*pi*80.0f), 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
-PI_Cntrl pi_w(1.0f, 0.1f, 2.0f);              // PI controller for test purposes motor speed (no balance)
+// 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A
+float maxCurrent = 15.0f;
+float Km = 1/0.217f;   // Motorgain: Torque -> km -> Current in A/Nm
+float maxTorque = maxCurrent/Km;
+PI_Cntrl pi_w2zero(-.01f, 0.8f, maxTorque);  // controller to bring motor speed to zero while balancing
+PI_Cntrl pi_w(0.5f, 0.2f, maxTorque);              // PI controller for test purposes motor speed (no balance)
 
 IIR_filter FilterACCx(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_x
 IIR_filter FilterACCy(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_y
 IIR_filter FilterGYRZ(tau, Ts, tau);     // 1st order LP for complementary filter gyro
 
+// IIR_filter FilterDiffANG(1.0f/(2.0f*pi*180.0f), Ts);
+
 // 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)
-LinearCharacteristics u2ax(14.67f,1.6378f);         // convert input voltage (0..3.3V) -> acc_x m/s^2
-LinearCharacteristics u2ay(15.02f ,1.6673f);        // convert input voltage (0..3.3V) -> acc_y m/s^2
-LinearCharacteristics u2gz(-4.652f,1.4949f);        // convert input voltage (0..3.3V) -> w_x rad/s
-LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
+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)
+LinearCharacteristics u2ax(14.67f, 1.6378f);         // convert input voltage (0..3.3V) -> acc_x m/s^2
+LinearCharacteristics u2ay(15.02f, 1.6673f);        // convert input voltage (0..3.3V) -> acc_y m/s^2
+LinearCharacteristics u2gz(-4.652f, 1.4949f);        // convert input voltage (0..3.3V) -> w_x rad/s
+LinearCharacteristics u3k3_TO_1V(0.303030303f, 0.0f, 3.3f, 0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
 
 // user defined functions
 void updateControllers(void);   // speed controller loop (via interrupt)
 void pressed(void);             // user Button pressed
 void released(void);            // user Button released
+void printLine();
 
 // main program and control loop
 // -----------------------------------------------------------------------------
@@ -103,6 +110,8 @@
     FilterACCy.reset(u2ay(3.3f*ay.read()));
     FilterGYRZ.reset(u2gz(3.3f*gz.read()));
     
+    FilterDiffANG.reset(u2gz(0.0f));
+
     // reset output
     out.write(u3k3_TO_1V(i2u(0.0f)));
 
@@ -114,7 +123,7 @@
 
 void updateControllers(void)
 {
-       
+
     // read encoder data
     short counts = MotorEncoder;            // counts in 1
     float omega = MotorDiff(counts);        // angular velofity motor
@@ -126,34 +135,42 @@
 
     // perform complementary filter
     float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;
+    // float dang = FilterDiffANG(ang);
 
     // get current state of the cube
     float actualAngleDegree = ang*180.0f/pi;
-    if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
+    if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) {
         doesStand = 1;
-    }
-    else{
+    } else {
         doesStand = 0;
     }
 
     // update controllers
     float desTorque = 0.0f;
-    if(shouldBalance){
+    if(shouldBalance) {
         // K matrix: -5.2142   -0.6247  // from Matlab
-        desTorque = pi_w2zero(n_soll-omega)-(-5.2142f*ang-0.6247f*gyrz);     // state space controller for balance, calc desired Torque
-    }
-    else{
+        float uPiC = pi_w2zero(n_soll - omega - 24.9f); // needs further inverstigation
+        float uSsC = (-5.2142f*ang - 0.6247f*gyrz);
+        desTorque = uPiC - uSsC;     // state space controller for balance, calc desired Torque
+        if(abs(desTorque) > maxTorque) {
+            desTorque = copysign(maxTorque, desTorque);
+        }
+        if(k == 0) printLine();
+        if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPiC, uSsC, ang, omega);
+    } else {
         desTorque = pi_w(n_soll-omega);     // state space controller for balance, calc desired Torque
     }
     // convert Nm -> A and write to AOUT
-    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));
+    out.write(u3k3_TO_1V(i2u(desTorque*Km)));
 
+    // if(k == 0) printLine();
+    // if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", accx, accy, gyrz);
     //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-omega))));          // test speed controller
-    if(++k >= 199){
-        k = 0;
-        pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega);
-    }
-    
+    // if(++k >= 199){
+    //    k = 0;
+    //    pc.printf("phi=%3.2f omega=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega, n_soll);
+    //}
+
 }
 
 // Buttonhandling and statemachine
@@ -176,14 +193,14 @@
     // if the cube doesStand
     if(doesStand) {
         // in - or decrease speed
-        if(ButtonTime < 2.0f) {
+        if(ButtonTime < 4.0f) {
             // press Button long -> increase speed 5 rev/min
-            if(ButtonTime > 0.5f) {
-                n_soll -= 5.0f;
+            if(ButtonTime > 0.3f) {
+                n_soll -= 1.0f;
             }
             // press Button short -> decrease speed 5 rev/min
             else {
-                n_soll += 5.0f;
+                n_soll += 1.0f;
             }
             // constrain n_soll
             if(n_soll >  v_max)
@@ -198,8 +215,13 @@
             pi_w2zero.reset(0.0f);
         }
     } else {
-        if(ButtonTime > 2.0f)
+        if(ButtonTime > 4.0f)
             shouldBalance = 1;
         pi_w.reset(0.0f);
     }
 }
+
+void printLine()
+{
+    printf("-----------------------------------------------------------------------------------------\r\n");
+}