RT2_Cuboid_demo
Test of pmic GPA with filter
Dependencies: mbed
Fork of
nucf446-cuboid-balance1_strong
by 
RT2_Cuboid_demo
Diff: main.cpp
- Revision:
- 5:d6c7ccbbce78
- Parent:
- 4:6457d61fd234
- Child:
- 8:d68e177e2571
diff -r 6457d61fd234 -r d6c7ccbbce78 main.cpp
--- a/main.cpp Thu Mar 01 13:48:32 2018 +0000
+++ b/main.cpp Fri Mar 09 12:53:45 2018 +0000
@@ -1,20 +1,21 @@
#include "mbed.h"
#include "math.h"
-//------------------------------------------
-#define PI 3.1415927f
-//------------------------------------------
+#define pi 3.1415927f
+
#include "EncoderCounter.h"
#include "DiffCounter.h"
#include "PI_Cntrl.h"
#include "IIR_filter.h"
#include "LinearCharacteristics.h"
+
/* 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) ****
+IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc
+ settings for Maxon ESCON controller (upload via ESCON Studio)
+
hardware Connections:
-
+
CN7 CN10
: :
: :
@@ -28,46 +29,50 @@
o. ENC CH B ..
.. ..
.. ..
- .o AIN acx (PA_0) ..
+ .o AIN acx (PA_0) ..
.o AIN acy (PA_1) .. 5.
.o AIN Gyro(PA_4) .o Analog GND
.. ..
- .. ..
- .. .. 1.
+ .. ..
+ .. .. 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
-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 PB_0
-AnalogOut out(PA_5); // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
+
+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
+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 PB_0
+AnalogOut out(PA_5); // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
+Ticker ControllerLoopTimer; // interrupt for control loop
+Timer t; // timer to analyse Button
+
+// controller parameters etc.
float out_value = 1.6f; // set voltage on 1.6 V (0 A current)
-float kp = 4.0f; // speed control gain for motor speed cntrl.
-float Tn = 0.05f; // Integral time " " "
float Ts = 0.0025; // sample time
float v_max = 200; // maximum speed rad/s
-//------------------------------------------
-float n_soll = 10.0f; // nominal speed for speed control tests
-float data[1000][2]; // logging data
-unsigned int k = 0; // standart counter for output
-unsigned int n = 0; // standart counter for output
-//------------------------------------------
-Ticker ControllerLoopTimer; // interrupt for control loop
-Timer t; // Timer to analyse Button
-float TotalTime = 0.0f;
-float comp_filter_tau =1.0f; // time constant of complementary filter
-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
-PI_Cntrl pi_w(kp,Tn,2.0f); // PI controller for test purposes motor speed (no balance)
-PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f); // PI controller to bring motor speed to zero while balancing
-//------------------------------------------
-IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x
-IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y
-IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
-// define some linear characteristics -----------------------------------------
-//9LinearCharacteristics i2u(0.8f,-2.0f); // max. 2 A, convert desired current (Amps) -> voltage 0..3.3V
+float n_soll = 0.0f; // nominal speed for speed control tests
+float tau = 1.0f; // time constant of complementary filter
+float fg = 300.0f;
+
+// output and statemachine
+unsigned int k = 0; // counter for serial output
+bool doesStand = 0; // state if the cube is standing or not
+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
+
+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)
+
+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
+
+// 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)
@@ -76,77 +81,125 @@
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
-// ----- User defined functions -----------
+// user defined functions
void updateControllers(void); // speed controller loop (via interrupt)
-void pressed(void); // user button pressed
-void released(void); // user button released
-// ------ END User defined functions ------
+void pressed(void); // user Button pressed
+void released(void); // user Button released
-//******************************************************************************
-//---------- main loop -------------
-//******************************************************************************
+// main program and control loop
+// -----------------------------------------------------------------------------
int main()
{
- //attach controller loop to timer interrupt
- pc.baud(2000000); // for serial comm.
- counter1.reset(); // encoder reset
- diff.reset(0.0f,0.0f);
- pi_w.reset(0.0f);
+ // for serial comm.
+ pc.baud(2000000);
+
+ // reset encoder, controller and filters
+ MotorEncoder.reset();
+ MotorDiff.reset(0.0f,0.0f);
pi_w2zero.reset(0.0f);
- f_ax.reset(u2ax(3.3f*ax.read()));
- f_ay.reset(u2ay(3.3f*ay.read()));
- f_gz.reset(u2gz(3.3f*gz.read()));
+ pi_w.reset(0.0f);
+
+ FilterACCx.reset(u2ax(3.3f*ax.read()));
+ FilterACCy.reset(u2ay(3.3f*ay.read()));
+ FilterGYRZ.reset(u2gz(3.3f*gz.read()));
+
+ // reset output
+ out.write(u3k3_TO_1V(i2u(0.0f)));
+
+ // attach controller loop to timer interrupt
ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
- button.fall(&pressed); // attach key pressed function
- button.rise(&released); // attach key pressed function
+ Button.fall(&pressed); // attach key pressed function
+ Button.rise(&released); // attach key pressed function
}
-//******************************************************************************
-//---------- control loop -------------
-//******************************************************************************
-void updateControllers(void){
- short counts = counter1; // get counts from Encoder
- float vel = diff(counts); // motor velocity
- float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity
- float ang = atan2(-f_ax(u2ax(3.3f*ax.read())),f_ay(u2ay(3.3f*ay.read())))+f_gz(wz)+PI/4.0f; // compl. filter
- // K matrix: -5.2142 -0.6247 // from Matlab
- float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance, calc desired Torque
- out.write(u3k3_TO_1V(i2u(desTorque/0.217f))); // convert Nm -> A and write to AOUT
- //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller
+
+void updateControllers(void)
+{
+
+ // read encoder data
+ short counts = MotorEncoder; // counts in 1
+ float omega = MotorDiff(counts); // angular velofity motor
+
+ // read imu data
+ float accx = u2ax(3.3f*ax.read());
+ float accy = u2ay(3.3f*ay.read());
+ float gyrz = u2gz(3.3f*gz.read());
+
+ // perform complementary filter
+ float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;
+
+ // get current state of the cube
+ float actualAngleDegree = ang*180.0f/pi;
+ if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
+ doesStand = 1;
+ }
+ else{
+ doesStand = 0;
+ }
+
+ // update controllers
+ float desTorque = 0.0f;
+ 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{
+ 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(pi_w(n_soll-omega)))); // test speed controller
if(++k >= 199){
k = 0;
- pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel);
- //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
+ pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega);
+ }
+
+}
+
+// Buttonhandling and statemachine
+// -----------------------------------------------------------------------------
+// start timer as soon as Button is pressed
+void pressed()
+{
+ t.start();
+}
+
+// evaluating statemachine
+void released()
+{
+
+ // readout, stop and reset timer
+ float ButtonTime = t.read();
+ t.stop();
+ t.reset();
+
+ // if the cube doesStand
+ if(doesStand) {
+ // in - or decrease speed
+ if(ButtonTime < 2.0f) {
+ // press Button long -> increase speed 5 rev/min
+ if(ButtonTime > 0.5f) {
+ n_soll -= 5.0f;
+ }
+ // press Button short -> decrease speed 5 rev/min
+ else {
+ n_soll += 5.0f;
+ }
+ // constrain n_soll
+ if(n_soll > v_max)
+ n_soll = v_max;
+ if(n_soll < -v_max)
+ n_soll = -v_max;
+ }
+ // stop balancing
+ else {
+ n_soll = 0.0f;
+ shouldBalance = 0;
+ pi_w2zero.reset(0.0f);
+ }
+ } else {
+ if(ButtonTime > 2.0f)
+ shouldBalance = 1;
+ pi_w.reset(0.0f);
}
}
-//******************************************************************************
-//********** User functions like buttens handle etc. **************
-//******************************************************************************
-// pressed button
-//******************************************************************************
-void pressed()
-{
-t.start();
-}
-//******************************************************************************
-// analyse pressed button
-//******************************************************************************
-void released()
-{
- TotalTime = t.read();
- t.stop();
- t.reset();
- if(TotalTime<0.1f) // short button presses means decrease speed
- {
- n_soll -=5; // decrease nominal speed
- TotalTime = 0.0f; // reset Time
- }
- else
- {
- n_soll +=5; // otherwise increase n_soll is in rev/min
- TotalTime = 0.0f;
- }
- if(n_soll>v_max) // limit nominal speed
- n_soll=v_max;
- if(n_soll<-v_max)
- n_soll=-v_max;
-}