Cuboid
Dependencies: mbed
Diff: main.cpp
- Revision:
- 5:d6c7ccbbce78
- Parent:
- 4:6457d61fd234
- Child:
- 8:d68e177e2571
--- 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; -}