Cuboid
Dependencies: mbed
Diff: main.cpp
- Revision:
- 8:d68e177e2571
- Parent:
- 5:d6c7ccbbce78
- Child:
- 9:30ee1d386a1d
--- a/main.cpp Sat Mar 17 08:43:07 2018 +0000 +++ b/main.cpp Thu Mar 22 17:32:37 2018 +0000 @@ -53,7 +53,7 @@ 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.0f; // time constant of complementary filter +float tau = 1.05f; // time constant of complementary filter float fg = 300.0f; // output and statemachine @@ -62,29 +62,36 @@ 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 +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 -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) +// 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,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V +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 // ----------------------------------------------------------------------------- @@ -103,6 +110,8 @@ 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))); @@ -114,7 +123,7 @@ void updateControllers(void) { - + // read encoder data short counts = MotorEncoder; // counts in 1 float omega = MotorDiff(counts); // angular velofity motor @@ -126,34 +135,42 @@ // 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){ + if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) { doesStand = 1; - } - else{ + } else { doesStand = 0; } // update controllers float desTorque = 0.0f; - if(shouldBalance){ + 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{ + 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/0.217f))); + 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 \r\n", actualAngleDegree, omega); - } - + // 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 @@ -176,14 +193,14 @@ // if the cube doesStand if(doesStand) { // in - or decrease speed - if(ButtonTime < 2.0f) { + if(ButtonTime < 4.0f) { // press Button long -> increase speed 5 rev/min - if(ButtonTime > 0.5f) { - n_soll -= 5.0f; + if(ButtonTime > 0.3f) { + n_soll -= 1.0f; } // press Button short -> decrease speed 5 rev/min else { - n_soll += 5.0f; + n_soll += 1.0f; } // constrain n_soll if(n_soll > v_max) @@ -198,8 +215,13 @@ pi_w2zero.reset(0.0f); } } else { - if(ButtonTime > 2.0f) + if(ButtonTime > 4.0f) shouldBalance = 1; pi_w.reset(0.0f); } } + +void printLine() +{ + printf("-----------------------------------------------------------------------------------------\r\n"); +}