Diff: main.cpp

Revision:

1:314e2e3727f2

Parent:

0:4ed9a8952ddc

Child:

2:0b64504024fe

diff -r 4ed9a8952ddc -r 314e2e3727f2 main.cpp
--- a/main.cpp	Tue Apr 30 15:34:47 2019 +0000
+++ b/main.cpp	Tue May 07 10:22:30 2019 +0000
@@ -1,31 +1,22 @@
 #include "mbed.h"
 #include "EncoderCounter.h"
+#include "PID_Cntrl.h"
 #include "LinearCharacteristics.h"
-#include "PID_Cntrl.h"
-
 //------------------------------------------
 #define PI 3.1415927f
 //------------------------------------------
  
-/* GRT, Control of voice-coil with PID-T1 - Controller
+/* GRT: Control of voice-coil with PID-T1 - Controller
  */
 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
-DigitalOut led1(LED1);
+AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 4A (see ESCON)
 
 bool key_was_pressed = false;
 bool controller_active = false;
-AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 2A (see ESCON)
-float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
-float Ts = 0.0005f;                     // sample time of main loops
 void pressed(void);                     // user Button pressed
 void released(void);                    // user Button released
-// dt1-Regler with Kp = 0, Kd = 0.0109, Tf = 0.000625
-PID_Cntrl dt1(0.0f,0.0f,17.3780,0.000625f,Ts,-3.0,3.0);
-PID_Cntrl pi1(1.35f,13.5f,0,1.0f,Ts,-3.0,3.0);
-PID_Cntrl pidt1(-0.0019,4.72,0.4,0.005,Ts,-3.0,3.0);
-uint16_t kk=0;
-float w=1.0f;
+  
 //------------------------------------------
 // ... here define variables like gains etc.
 //------------------------------------------
@@ -33,57 +24,44 @@
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
-Timer ti;                               // define global timer
-Timer t_but;                            // define global timer
-//------------------------------------------
+Timer t_but;                            // define timer for button
 // ----- User defined functions -----------
 void updateLoop(void);   // loop for State machine (via interrupt)
-// ------ END User defined functions ------
- 
+float Ts = 0.0005f;                     // sample time of main loop
+uint16_t k = 0;
+PID_Cntrl dt1(0.0f,0.0f,17.3780,0.000625f,Ts,-3.0,3.0);
+PID_Cntrl pi1(1.35f,13.5f,0,1.0f,Ts,-3.0,3.0);
+PID_Cntrl pidt1(-0.0019,4.72,0.4,0.005,Ts,-3.0,3.0);
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
 int main()
-{
+{   
     pc.baud(115200);   // for serial comm.
     counter1.reset();   // encoder reset
     out.write(i2u(0.0));
-    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
-    ti.reset();
-    ti.start();
     button.fall(&pressed);          // attach key pressed function
     button.rise(&released);         // attach key pressed function
-    
+    pc.printf("Start controller now...\r\n");
+    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
+    while(1);
 }   // END OF main
 //******************************************************************************
 //---------- main loop (called via interrupt) -------------
 //******************************************************************************
 void updateLoop(void){
-    if(key_was_pressed)
+    float x = (float)(counter1)/1000.0f;  // get counts from Encoder
+    float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
+    if(controller_active)
+        {   
+            i_des = pi1(w-x -dt1(x));
+                }
+    out.write(i2u(i_des));
+    if(++k>1000)
     {
-        controller_active = !controller_active;
-        if(controller_active)
-        {
-            dt1.reset(0.0f);
-            led1=true;
-            }
-        else
-            led1=false;
-        key_was_pressed = false;
-        }
-    float x = (float)(counter1)/1000.0;          // get counts from Encoder
-    float i_des = 0.0f;                  // set motor torque to zero (will be overwritten)
-    if(controller_active)
-        {
-            //i_des = pi1(w-x) - dt1(x);
-            i_des = pidt1(w-x);
-            }
-    if(++kk>1000){            
-        pc.printf("x/mm: %1.3f I/A: %1.3f\r\n",x,i_des);
-        kk=0;
-        w = -w;
-        }
-    out.write(i2u(i_des));
+        pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
+        k=0;
+        }    
 } // END OF updateLoop(void)
  
 //******************************************************************************
@@ -93,10 +71,8 @@
 void pressed()
 {
     t_but.start();
-    key_was_pressed = false;
 }
- 
-// evaluating statemachine
+// Falling edge of button: enable/disable controller 
 void released()
 {
     // readout, stop and reset timer
@@ -104,5 +80,14 @@
     t_but.stop();
     t_but.reset();
     if(ButtonTime > 0.05f) 
-        key_was_pressed = true;
+    {   
+        controller_active = !controller_active;
+        if(controller_active)
+            {
+            pc.printf("Controller active\r\n");
+            // reset controller here!!!        
+            }
+        else
+            pc.printf("Controller disabled\r\n");
+    }
 }