Cuboid
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:15be70d21d7c
- Child:
- 1:2e118d67eeae
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jan 10 16:08:07 2018 +0000 @@ -0,0 +1,145 @@ +#include "mbed.h" +#include "math.h" +//------------------------------------------ +#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) **** +hardware Connections: + + CN7 CN10 + : : + : : + .. .. + .. ENC CH A o. + o. GND .. 10. + o. ENC CH B .. + .. .. + .. .. + .o AIN acy (PA_0) .. + .o AIN acy (PA_1) .. 5. + .o i_soll(PA_4) .o Analog GND + .o AIN Gyro (PB_0) .. + .. .. + .. .. 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(PB_0); // Analog IN (gyr z) on PB_0 +AnalogOut out(PA_4); // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON) +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 ----------------------------------------- +LinearCharacteristics i2u(0.8f,-2.0f); // 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) +LinearCharacteristics u2ax(14.67f,1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2 +LinearCharacteristics u2ay(15.02f ,1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2 +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 ----------- +void updateControllers(void); // speed controller loop (via interrupt) +void pressed(void); // user button pressed +void released(void); // user button released +// ------ END User defined functions ------ + +//****************************************************************************** +//---------- main 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); + 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())); + ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz; + 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 + 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 + if(++k >= 99){ + k = 0; + pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel); + } +} +//****************************************************************************** +//********** 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; +}