Revision 5:768e10f6d372, committed 2021-02-25

Comitter:

altb2

Date:

Thu Feb 25 20:28:45 2021 +0000

Parent:

4:3d8dd3d17564

Commit message:

first commit of Mirror actuator

Changed in this revision

diff -r 3d8dd3d17564 -r 768e10f6d372 ControllerLoop.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerLoop.cpp	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,52 @@
+#include "ControllerLoop.h"
+using namespace std;
+
+
+
+ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
+{
+    thread.start(callback(this, &ControllerLoop::loop));
+    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
+    diff1.reset(0.0f,0);
+    diff2.reset(0.0f,0);
+    }
+
+
+ControllerLoop::~ControllerLoop() {}
+
+
+void ControllerLoop::loop(void){
+    float Kp = 1500 * 4.89e-7/0.094;   // XX * J/km
+    float Tn = .0035;
+    float Kv=50;
+    float phi_des = 0;
+    float v_des = 0;
+    PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95);
+    Unwrapper_2pi uw2pi;
+    uint8_t k = 0;
+    while(1)
+        {
+        ThisThread::flags_wait_any(threadFlag);
+        // THE LOOP ------------------------------------------------------------
+        if(++k%64==0)
+            {
+                printf("%f %f\r\n",phi_des,v_des);
+            }
+        short c1 = counter1;            // get counts from Encoder
+        short c2 = counter2;            // get counts from Encoder
+        float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1);
+        float vel1 = diff1(c1);                 // motor velocity 
+        float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2);
+        float vel2 = diff2(c2);                 // motor velocity 
+        float w0=2*3.1415927 * 3;
+        //phi_des = 1.0*sinf(w0*ti.read());
+        current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
+        float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 );
+        //float i1 = v_cntrl(v_des-vel ) + exc;
+        i_des2.write(i2pwm(-i1));
+        }
+}
+
+void ControllerLoop::sendSignal() {
+    thread.flags_set(threadFlag);
+}

diff -r 3d8dd3d17564 -r 768e10f6d372 ControllerLoop.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerLoop.h	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "EncoderCounter.h"
+#include "EncoderCounterIndex.h"
+#include "DiffCounter.h"
+#include "LinearCharacteristics.h"
+#include "ThreadFlag.h"
+#include "path_1d.h"
+#include "PID_Cntrl.h"
+#include "Unwrapper_2pi.h"
+
+
+
+extern EncoderCounter counter1,counter2;
+extern EncoderCounterIndex index1,index2;
+extern DiffCounter diff1,diff2;
+extern path_1d *current_path;
+extern LinearCharacteristics i2pwm;
+extern PwmOut i_des1, i_des2;
+extern DigitalOut i_enable;
+extern Timer glob_ti;
+
+// This is the loop class, it is not a controller at first hand, it guarantees a cyclic call
+class ControllerLoop
+{
+public:
+    ControllerLoop(float Ts);
+    virtual     ~ControllerLoop();
+
+
+private:
+    void loop(void);
+    Thread thread;
+    Ticker ticker;
+    ThreadFlag threadFlag;
+    Timer ti;
+    float Ts;
+    void sendSignal();
+
+};

diff -r 3d8dd3d17564 -r 768e10f6d372 Library_Cntrl.lib
--- a/Library_Cntrl.lib	Wed Nov 06 08:00:26 2019 +0000
+++ b/Library_Cntrl.lib	Thu Feb 25 20:28:45 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/altb2/code/Library_Cntrl/#a201a6cd4c0c
+https://os.mbed.com/users/altb2/code/Library_Cntrl/#b54eb3e24d2d

diff -r 3d8dd3d17564 -r 768e10f6d372 Library_Misc.lib
--- a/Library_Misc.lib	Wed Nov 06 08:00:26 2019 +0000
+++ b/Library_Misc.lib	Thu Feb 25 20:28:45 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/altb2/code/Library_Misc/#dd5d116ace8f
+https://os.mbed.com/users/altb2/code/Library_Misc/#29602f4ade5c

diff -r 3d8dd3d17564 -r 768e10f6d372 main.cpp
--- a/main.cpp	Wed Nov 06 08:00:26 2019 +0000
+++ b/main.cpp	Thu Feb 25 20:28:45 2021 +0000
@@ -4,259 +4,107 @@
 #define PI 3.1415927f
 //------------------------------------------
 #include "EncoderCounter.h"
+#include "EncoderCounterIndex.h"
 #include "DiffCounter.h"
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
-#include "PI_Cntrl.h"
-// #include "GPA.h"
-// define STATES:
-#define INIT 0              // at very beginning
-#define FLAT 10             // cuboid is flat, motor is controlled to zero
-#define BALANCE 20          // balancing
-#define SWING_DOWN 30       // move cuboid down
- 
- 
-/* Cuboid balance on one edge on Nucleo F446RE
- 
- **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
-                 settings for Maxon ESCON controller (upload via ESCON Studio) ****
-hardware Connections:
+#include "PID_Cntrl.h"
+#include "Unwrapper_2pi.h"
+#include "path_1d.h"
+#include "GPA.h"
+#include "ControllerLoop.h"
+
+
  
- CN7                    CN10
-  :                         :
-  :                         :
- ..                        ..
- ..                        ..                  15.
- ..    AOUT i_des on (PA_5)o.
- ..                        ..
- ..                        ..
- ..               ENC CH A o.
- o. GND                    ..                  10.
- o. ENC CH B               ..
- ..                        ..
- ..                        ..
- .o AIN acx (PA_0)         ..
- .o AIN acy (PA_1)         ..                  5.
- .o AIN Gyro(PA_4)         .o Analog GND
- ..                        ..
- ..                        ..
- ..                        ..                  1.
- ----------------------------
- CN7               CN10
- */
-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
+Serial pc(USBTX, USBRX,115200);
+InterruptIn button(USER_BUTTON);        // User Button, short and long presses!
 bool key_was_pressed = false;
-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 PA_4
-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 w_soll = 10.0f;                   // desired velocity
-float Ts = 0.002f;                      // sample time of main loops
-int k = 0;
-float phi1_des = 0.0f;
-int CS = INIT;
-void pressed(void);             // user Button pressed
-void released(void);            // user Button released
- 
- 
-//------------------------------------------
-// ... here define variables like gains etc.
-//------------------------------------------
-LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f);       // output is normalized output
-LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
-LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f);      // use normalized input
-LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f);           // use normalized input
-                // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
- 
-//-------------DEFINE FILTERS----------------
-float tau = 1.0f;
-IIR_filter f_ax(tau,Ts,1.0f);        // filter ax signals
-IIR_filter f_ay(tau,Ts,1.0f);        // filter ay signals
-IIR_filter f_gz(tau,Ts,tau);    // filter gz signals
-//------------------------------------------
-float vel = 0.0f;                  // velocity of motor
-float gyro = 0.0f;
-float phi1 = 0.0f;
-//------------------------------------------
-Ticker  ControllerLoopTimer;            // interrupt for control loop
-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
-Timer ti;                               // define global timer
-Timer t_but;                            // define global timer
-PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f);  // velocity controller for motor
-PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f);  // slow vel. contrl. to bring motor to zero
+float Ts=.001;                         // sampling time
+void pressed(void);
+void released(void); 
+float vel;
+//------------- DEFINE FILTERS ----------------
+// missing
+//-----------------------------------
+Timer glob_ti;
+EncoderCounter counter1(PA_6, PC_7);    // initialize counter on PA_6 and PC_7
+InterruptIn indexpulse1(PA_8);
+EncoderCounterIndex index1(counter1,indexpulse1);   
+//
+EncoderCounter counter2(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
+InterruptIn indexpulse2(PB_4);
+EncoderCounterIndex index2(counter2,indexpulse2);    // initialize counter on PA_6 and PC_7
+//
+DiffCounter diff1(0.0008,Ts);              // discrete differentiate, based on encoder1 data
+DiffCounter diff2(0.0008,Ts);              // discrete differentiate, based on encoder2 data
+LinearCharacteristics i2pwm(-1.0,1.0,0.01,0.99,.01,.99);
+//
+PwmOut i_des1(PB_10);
+PwmOut i_des2(PA_15);
+DigitalOut i_enable(PC_4);
+float data[2000][4];
+Unwrapper_2pi uw2pi;
 //------------------------------------------
 // ----- User defined functions -----------
-void updateLoop(void);   // loop for State machine (via interrupt)
-float cuboid_stab_cntrl(int);   // stabalizer 
-void calc_angle_phi1(int);
-// ------ END User defined functions ------
- 
+ControllerLoop loop(Ts);
+path_1d p1;
+path_1d p2;
+path_1d *current_path;
+float A = 2.7;
+float dc=0.0;
+//GPA myGPA(1, 2500, 100, 30, 20, Ts);
+//GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts);
+float exc=0.0;              // excitation GPA
+//        f1    f2  N  A1  A2  Ts
+
+// *****************************************************************************
+void rise_edge(void)
+    {
+     //glob_ti.reset();
+     }
+// *****************************************************************************
+void fall_edge(void)
+   {
+        uint32_t time_us = glob_ti.read_us();
+        dc = (float)time_us/200.0f;
+    }
+//InterruptIn i_des2(PA_6);
+
+
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
 int main()
 {
-    pc.baud(2000000);   // for serial comm.
+    i_des1.period_us(200);
+    i_des2.period_us(200);      // PWM Frequency of i_desired
+    i_enable = 0;               // disable current first
     counter1.reset();   // encoder reset
-    diff.reset(0.0f,0);  
-    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
-    ti.reset();
-    ti.start();
-    calc_angle_phi1(1);
-    button.fall(&pressed);          // attach key pressed function
-    button.rise(&released);         // attach key pressed function
+    counter2.reset();   // encoder reset
+    ThisThread::sleep_for(100);
+    i_enable = 1;
+    
+    glob_ti.start();
+    glob_ti.reset();
+    printf("Start Mirror\r\n");
+    p1.initialize(300,10,A,0,0,0);
+    p2.initialize(300,10,-A,0,0,A);
+    for(int wk =0;wk<10;wk++)
+        {
+        current_path = &p1;
+        current_path->start(glob_ti.read());
+        while(!current_path->finished)
+            wait(.1);
+        current_path = &p2;
+        current_path->start(glob_ti.read());
+        while(!current_path->finished)
+            wait(.1);
+        ThisThread::sleep_for(100);
+        }
+    i_enable = 0;
+    while(1)
+        wait(1);
+
+
     
 }   // END OF main
-//******************************************************************************
-//---------- main loop (called via interrupt) -------------
-//******************************************************************************
-void updateLoop(void){
-    short counts = counter1;            // get counts from Encoder
-    vel = diff(counts);                 // motor velocity 
-    float torq = 0.0f;                  // set motor torque to zero (will be overwritten)
-    float dt  = ti.read();              // time elapsed in current state
-    calc_angle_phi1(0);                 // angle phi1 is calculated in every loop
-    // ****************** STATE  MACHINE ***************************************
-    switch(CS)  { 
-        case INIT:                      // at very beginning
-            if (dt > 2.0f){
-                CS = FLAT;              // switch to FLAT state
-                ti.reset(); 
-                }
-            break;
-        case FLAT:                      // cuboid is flat, keep motor velocity to zero
-            torq = vel_cntrl(0.0f - vel); 
-            if (key_was_pressed && dt > 1.0f){
-                CS = BALANCE;
-                torq = cuboid_stab_cntrl(1);
-                ti.reset();
-                key_was_pressed = false;
-                phi1_des = 0.0f;
-                vel_cntrl.reset(0.0f);  // reset velocity controller for the next time
-                }
-            break;
-        case BALANCE:                   // balance the cube
-            torq = cuboid_stab_cntrl(0);
-            if (key_was_pressed && dt > 1.0f){
-                CS = SWING_DOWN;
-                phi1_des = 0.0f; 
-                ti.reset();
-                key_was_pressed = false;
-                }
-            break;  
-        case SWING_DOWN: 
-            phi1_des = dt;              // ramp the desired angle up to pi/4
-            torq = cuboid_stab_cntrl(0);// call balance routine
-            if (dt > 1.0f){             // good value for smooth moving down
-                CS = FLAT;              // move to flat
-                phi1_des = 0.0f; 
-                ti.reset(); 
-                }
-            break;  
-    default: break;
-    }
-    out.write( i2u(torq/0.217f)); // motor const. is 0.217, 
-    if(++k >= 249){
-        k = 0;
-        //pc.printf("phi: %1.5f, Torq: %1.2f  \r\n",phi1,torq);
-        pc.printf("ax %1.5f, ay: %1.5f, gyro: %1.5f, ph1: %1.5f\r\n",u2ax(ax.read()),u2ay(ay.read()),u2gz(gz.read()),phi1);
-        }
-
-} // END OF updateLoop(void)
- 
-//******************************************************************************
-//        ************ stabalizing controller *****************
-//******************************************************************************
- 
-float cuboid_stab_cntrl(int do_reset){
-/*                   | phi_des
-                     v 
-                    ---
-                   | V |  feed forward gain
-                    ---
-                     |
-          -------    v      --------
-0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
-     ^-   -------    ^-     --------   |
-     |               |                 |
-    vel              |       ---       |
-                      ---  -| K |<-----
-                             ---
-*/
-    if(do_reset == 1)       // reset controller
-        om2zero.reset(0.0f);
-        
-    float M_des = om2zero(0.0f-vel);    // outer controller to bring motor to zero
-    float torq = M_des    +  (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro);  // calculationof gains are based on the MATLAB script at the end of this file!!!
-    //          output PI       V               K(1)              K(2)
-    return torq;
-}
-// ************ calculate angle *****************
-void calc_angle_phi1(int do_reset){
-    gyro = u2gz(gz.read());
-    if(do_reset == 1){
-        f_ax.reset(u2ax(ax.read()));
-        f_ay.reset(u2ax(ay.read()));
-        f_gz.reset(gyro);
-        }
-    phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
-    }
- 
-// start timer as soon as Button is pressed
-void pressed()
-{
-    t_but.start();
-    key_was_pressed = false;
-}
- 
-// evaluating statemachine
-void released()
-{
- 
-    // readout, stop and reset timer
-    float ButtonTime = t_but.read();
-    t_but.stop();
-    t_but.reset();
-    if(ButtonTime > 0.05f) 
-        key_was_pressed = true;
-}
- 
- 
- 
-/* MATLAB CODE FOR CUBOID:
-%% Skript fuer cuboid Praktikum 
-    m_geh=.938;     % Masse des Gehaeses 
-    m_sb=1.243;     % Masse Scheibe 
-    m_g=m_geh+m_sb; % Gesamtmasse
-    J_geh=.00408;   % Traegheit Gehaeuse (CM)
-    J_sb=.00531;    % Traegheit Scheibe (CM)
-    J1=diag([0 0 J_geh]);
-    J2=diag([0 0 J_sb]);
-    l=.17;          % Kantenlaenge
-    sy=.085;
-    g=9.81;
-    phi0=0;
-    J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2;
- 
-    %% Linearisiertes Modell
-    A2=[0 1;m_g*g*l/sqrt(2)/J_g 0];
-    B2=[0;-1/J_g];
-    K=place(A2,B2,10*[-1+1j -1-1j]);
-    s_cl=ss(A2-B2*K,B2,[1 0],0);
-    V=1/dcgain(s_cl);
-    
-    %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten)
-    A3=[A2-B2*K [0;0];...
-         -1/J_sb*K 0];  % Auf Scheibenbeschleunigung wirken ganz 
-                        % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung
-    B3=[B2;1/J_sb];
-    C3=[0 -1 1];        % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse
-    s3=ss(A3,B3,C3,0);
-    s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]);  % Transformiere 3ten Zustand (eigentlich unnoetig)
-    Tn=4;
-    PI=-tf([Tn 1],[Tn 0]);  % Langsamer PI Regler, negatives gain erforderlich!!
-    rlocus(s3*PI);grid
-    */
\ No newline at end of file

diff -r 3d8dd3d17564 -r 768e10f6d372 mbed-os.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,1 @@
+https://github.com/armmbed/mbed-os/#c53d51fe9220728bf8ed27afe7afc1ecc3f6f5d7

diff -r 3d8dd3d17564 -r 768e10f6d372 mbed.bld
--- a/mbed.bld	Wed Nov 06 08:00:26 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 3d8dd3d17564 -r 768e10f6d372 path_1d.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/path_1d.cpp	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,93 @@
+#include "path_1d.h"
+using namespace std;
+
+path_1d::path_1d(void)
+{
+    this->finished = false;
+    this->started  = false;
+    this->initialized = false;
+    
+}
+
+
+path_1d::~path_1d() {}
+
+
+void path_1d::initialize(float a,float v, float s,float v_0,float v_end, float s_start){
+    this->a_max = a;
+    this->v_max = v;
+    this->s_start = s_start;
+    this->sig = (float)(((s-s_start)>=0) -((s-s_start)<0));
+    this->v_0 = abs(v_0);
+    this->dT=-this->v_0/this->a_max;
+    v_end = abs(v_end);
+    this->s_0 = abs(s-s_start);
+    this->s_end = this->s_0+.5f*v_end*v_end/this->a_max+.5f*this->v_0*this->v_0/this->a_max;
+    this->T_end=2*sqrt(this->s_end/this->a_max);
+    float VT= a * this->T_end/2.0f;
+    if(VT<=this->v_max)
+        {
+        this->T1=this->T_end/2.0f;
+        this->T2=this->T_end/2.0f;
+        }
+    else
+        {
+        float V_ = VT-this->v_max;
+        float TX = 2.0f*V_ / this->a_max;
+        float DS = .5f*V_ * TX;
+        float T_ = TX + DS / this->v_max;
+        this->T_end = this->T_end+DS / this->v_max;
+        this->T1=(this->T_end-T_)/2.0f;          // beschleunige bis hier
+        this->T2=(this->T_end+T_)/2.0f;          // bis hier konstant
+        this->X1=0.5f*this->a_max*this->T1*this->T1;
+        this->X2=this->X1 + this->v_max*(this->T2-this->T1);
+        this->X3=this->X2 + .5f*a_max*(this->T_end - this->T2)*(this->T_end-this->T2);
+        }
+    finished = false;
+    started  = false;
+    initialized = true;
+}
+void path_1d::get_x_v(float t, float *x,float *v)
+{
+ t = t-t_offset-dT;
+ if(t <=T1)
+    {
+    *x = 0.5f*a_max*t*t;
+    *v = a_max * t;
+    }
+else if(t<=T2)
+    {
+     *x = X1+v_max*(t-T1);
+     *v = v_max;
+    }
+else if(t<=T_end)
+    {
+    *x = X3-.5*a_max*(t-T_end)*(t-T_end);
+    *v = -a_max*(t-T_end);
+    }
+else
+    {
+    *x = s_end;
+    *v = 0;
+    }
+if(t>=T_end)
+    {
+    finished = true;
+    started = false;
+    }
+*x -= .5*v_0*v_0/a_max;
+if((*x) >= s_0)
+    {
+    finished = true;    
+    started = false;
+    }
+*x *= sig;
+*x += s_start; 
+*v *= sig;
+}
+void path_1d::start(float t)
+{   
+    t_offset = t;
+    finished = false;
+    started = true;
+    }
\ No newline at end of file

diff -r 3d8dd3d17564 -r 768e10f6d372 path_1d.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/path_1d.h	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,29 @@
+#ifndef PATH_1D_H_
+#define PATH_1D_H_
+
+
+#include "mbed.h"
+class path_1d{
+public:
+    path_1d(void);
+    virtual ~path_1d();
+    void initialize(float,float,float,float,float,float);
+    void get_x_v(float,float *, float *);
+    void start(float);
+    bool finished;
+    bool started;
+    bool initialized;
+    
+
+private:
+    float dT;
+    float sig;
+    float T_end,T1,T2;
+    float X1,X2,X3;
+    float t_offset;
+    float a_max,v_max,s_end,v_0,s_0,s_start;
+    //-----------------------------------
+    
+};
+
+#endif
\ No newline at end of file