Diff: main.cpp

Revision:

3:477db0d9895e

Parent:

2:0b64504024fe

Child:

4:37df0f6a1bc3

--- a/main.cpp	Tue May 07 11:04:54 2019 +0000
+++ b/main.cpp	Fri May 10 14:24:29 2019 +0000
@@ -5,7 +5,7 @@
 //------------------------------------------
 #define PI 3.1415927f
 //------------------------------------------
- 
+
 /* GRT: Control of voice-coil with PID-T1 - Controller
  */
 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
@@ -16,11 +16,11 @@
 bool controller_active = false;
 void pressed(void);                     // user Button pressed
 void released(void);                    // user Button released
-  
+
 //------------------------------------------
 // ... here define variables like gains etc.
 //------------------------------------------
-LinearCharacteristics i2u(-4.0f,4.0f,0.0f,3.2f / 3.3f);         // output is normalized output
+LinearCharacteristics i2u(-4.0f, 4.0f, 0.0f, 3.2f/3.3f);         // output is normalized output
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
@@ -29,69 +29,68 @@
 void updateLoop(void);   // loop for State machine (via interrupt)
 float Ts = 0.0005f;                     // sample time of main loop
 uint16_t k = 0;
-float w=0.5;
-PID_Cntrl dt1(0.0f,0.0f,17.3780/1600.0,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);
+// initiate PIDT1 controller objects
+// PID_Cntrl dt1(0.0f, 0.0f, 0.0109f, 0.000625f, Ts, -3.0f, 3.0f);
+// PID_Cntrl pi1(1.35f, 13.5f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
+PID_Cntrl dt1(0.0f, 0.0f, 0.0126f, 0.00067f, Ts, -3.0f, 3.0f);
+PID_Cntrl pi1(3.16f, 15.1f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
+float w = 0.5f;
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
 int main()
-{   
+{
     pc.baud(115200);   // for serial comm.
     counter1.reset();   // encoder reset
     out.write(i2u(0.0));
     button.fall(&pressed);          // attach key pressed function
     button.rise(&released);         // attach key pressed function
-    pc.printf("Start controller now...\r\n");
+    pc.printf("Start controller now or reset...\r\n");
     ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
     while(1);
 }   // END OF main
 //******************************************************************************
 //---------- main loop (called via interrupt) -------------
 //******************************************************************************
-void updateLoop(void){
+void updateLoop(void)
+{
     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);
-                }
+    if(controller_active) {
+        // controller update
+        i_des = pi1(w - x) - dt1(x);
+    }
     out.write(i2u(i_des));
-    if(++k>1000)
-    {
+    if(++k>1000) {
         pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
-        k=0;
-        w=-w;
-        }    
+        k = 0;
+        w = -w;
+    }
 } // END OF updateLoop(void)
- 
+
 //******************************************************************************
- 
- 
+
+
 // start timer as soon as Button is pressed
 void pressed()
 {
     t_but.start();
 }
-// Falling edge of button: enable/disable controller 
+// Falling edge of button: enable/disable controller
 void released()
 {
     // readout, stop and reset timer
     float ButtonTime = t_but.read();
     t_but.stop();
     t_but.reset();
-    if(ButtonTime > 0.05f) 
-    {   
+    if(ButtonTime > 0.05f) {
         controller_active = !controller_active;
-        if(controller_active)
-            {
-            pc.printf("Controller active\r\n");
-            // reset controller here!!!        
+        if(controller_active) {
+            pc.printf("Controller actived\r\n");
+            // reset controller here!!!
+            dt1.reset(0.0);
             pi1.reset(0.0);
-            dt1.reset(0.0);
-            }
-        else
+        } else
             pc.printf("Controller disabled\r\n");
     }
 }