Cuboid
Dependencies: mbed
main.cpp
- Committer:
- pmic
- Date:
- 2018-03-22
- Revision:
- 8:d68e177e2571
- Parent:
- 5:d6c7ccbbce78
- Child:
- 9:30ee1d386a1d
File content as of revision 8:d68e177e2571:
#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 ..\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 : : : : .. .. .. .. 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 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 Ts = 0.0025; // sample time float v_max = 200; // maximum speed rad/s float n_soll = 0.0f; // nominal speed for speed control tests float tau = 1.05f; // 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(1/(2.0f*pi*80.0f), Ts); // discrete differentiate, based on encoder data // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A float maxCurrent = 15.0f; float Km = 1/0.217f; // Motorgain: Torque -> km -> Current in A/Nm float maxTorque = maxCurrent/Km; PI_Cntrl pi_w2zero(-.01f, 0.8f, maxTorque); // controller to bring motor speed to zero while balancing PI_Cntrl pi_w(0.5f, 0.2f, maxTorque); // 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 // IIR_filter FilterDiffANG(1.0f/(2.0f*pi*180.0f), Ts); // 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) 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.0f, 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 void printLine(); // main program and control loop // ----------------------------------------------------------------------------- int main() { // for serial comm. pc.baud(2000000); // reset encoder, controller and filters MotorEncoder.reset(); MotorDiff.reset(0.0f,0.0f); pi_w2zero.reset(0.0f); 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())); FilterDiffANG.reset(u2gz(0.0f)); // 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 } 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; // float dang = FilterDiffANG(ang); // 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 float uPiC = pi_w2zero(n_soll - omega - 24.9f); // needs further inverstigation float uSsC = (-5.2142f*ang - 0.6247f*gyrz); desTorque = uPiC - uSsC; // state space controller for balance, calc desired Torque if(abs(desTorque) > maxTorque) { desTorque = copysign(maxTorque, desTorque); } if(k == 0) printLine(); if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPiC, uSsC, ang, omega); } 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*Km))); // if(k == 0) printLine(); // if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", accx, accy, gyrz); //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 omega=%3.2f \r\n", actualAngleDegree, omega, n_soll); //} } // 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 < 4.0f) { // press Button long -> increase speed 5 rev/min if(ButtonTime > 0.3f) { n_soll -= 1.0f; } // press Button short -> decrease speed 5 rev/min else { n_soll += 1.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 > 4.0f) shouldBalance = 1; pi_w.reset(0.0f); } } void printLine() { printf("-----------------------------------------------------------------------------------------\r\n"); }