altb_pmic
for Infotag
Dependencies: Library_Cntrl Library_Misc
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
--- /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);
+}
--- /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();
+
+};
--- 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
--- 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
--- 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
--- /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
--- 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
--- /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
--- /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