RT2_Cuboid_demo
Cuboid
Dependencies: mbed
Diff: main.cpp
- Revision:
- 15:ed33be6f040e
- Parent:
- 13:a308f5e6c306
- Child:
- 17:802aede7b90e
diff -r 164daac9c9d2 -r ed33be6f040e main.cpp
--- a/main.cpp Mon Apr 09 07:04:02 2018 +0000
+++ b/main.cpp Mon Apr 09 08:02:04 2018 +0000
@@ -50,12 +50,10 @@
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.0025f; // sample time
-float v_max = 200.0f; // 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;
+float Ts = 0.0025f; // sample time
+float v_max = 200.0f; // maximum speed rad/s
+float n_soll = 15.0f; // nominal speed for speed control tests
+float tau = 1.00f; // time constant of complementary filter
// output and statemachine
unsigned int k = 0; // counter for serial output
@@ -64,17 +62,18 @@
bool shouldGoDown = 0; // state if the controller should swing down
// set up encoder
-EncoderCounter MotorEncoder(PB_6, PB_7); // initialize counter on PB_6 and PB_7
+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
+// set up controllers
+float maxCurrent = 15.0f; // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A
+float Km = 1/0.217f; // Motorgain: Torque -> km -> Current in A/Nm
float maxTorque = maxCurrent/Km;
PI_Cntrl pi_w2zero(-.012f, 0.8f, Ts, maxTorque); // controller to bring motor speed to zero while balancing
-PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque); // PI controller for test purposes motor speed (no balance)
+PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque); // PI controller for test purposes motor speed (no balance)
float desTorque = 0.0f;
+// set up gpa
float fMin = 1.0f;
float fMax = 1.0f/2.0f/Ts*0.9f;
int NfexcDes = 150;
@@ -88,15 +87,15 @@
float outWobble = 0.0f;
float excWobble = 0.0f;
+// set up filters for complementary filter
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
+// set up filter for swing down process
IIR_filter FilterTrajectory(10.0f, 1.0f, Ts, 1.0f);
float V = -2.6080f;
-// 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)
@@ -131,8 +130,6 @@
FilterTrajectory.reset(0.0f);
- // FilterDiffANG.reset(u2gz(0.0f));
-
Wobble.reset();
Wobble.printGPAmeasPara();
inpWobble = 0.0f;
@@ -143,6 +140,7 @@
desTorque = 0.0f;
out.write(u3k3_TO_1V(i2u(desTorque*Km)));
+ // reset statemachine
shouldGoDown = 0;
shouldBalance = 0;
@@ -166,7 +164,6 @@
// 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;
@@ -178,8 +175,10 @@
// update controllers
if(shouldBalance) {
- ///*
+
+ /*
// balance, set n_soll = 0.0f
+ // ---------------------------------------------------------------------
// K matrix: -5.2142 -0.6247 // from Matlab
float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation
float uSS = (-5.2142f*ang - 0.6247f*gyrz);
@@ -189,15 +188,19 @@
}
if(k == 0) printLine();
if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPI, uSS, ang, omega);
- //*/
+ */
+
/*
- // step response, set n_soll = 0.0f
+ // step response velocity controller, set n_soll = 0.0f
+ // ---------------------------------------------------------------------
desTorque = pi_w(10.0f - omega);
if(k == 0) printLine();
if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", 10.0f, omega, desTorque);
*/
- /*
- // wobble, set n_soll = 15.0f
+
+ ///*
+ // gpa measurement, set n_soll = 15.0f
+ // ---------------------------------------------------------------------
// measuring the plant P and the closed loop tf T = PC/(1 + PC)
desTorque = pi_w(n_soll + excWobble - omega);
inpWobble = desTorque;
@@ -207,40 +210,30 @@
// desTorque = pi_w(n_soll + excWobble - omega);
// inpWobble = n_soll + excWobble - omega;
// outWobble = desTorque;
- //excWobble = Wobble(inpWobble, outWobble);
+ // excWobble = Wobble(inpWobble, outWobble);
if(++k == 73000) Wobble.printGPAmeasTime();
- */
+ //*/
+
} else if(shouldGoDown) {
// swing down
// K matrix: -5.2142 -0.6247 // from Matlab
// V gain: -2.6080 // from Matlab
- float ref = FilterTrajectory(pi/4.0f);; // needs further inverstigation
+ float ref = FilterTrajectory(pi/4.0f);
float uV = V*ref;
float uSS = (-5.2142f*ang - 0.6247f*gyrz);
- desTorque = uV - uSS; // state space controller for balance, calc desired Torque
+ desTorque = uV - uSS; // state space controller for balance
if(abs(desTorque) > maxTorque) {
desTorque = copysign(maxTorque, desTorque);
}
if(abs(ref - ang)/abs(ref) < 0.10f) {
shouldGoDown = 0;
FilterTrajectory.reset(omega);
- // printLine();
- // pc.printf("%6f %6f %6f \r\n", shouldBalance, shouldGoDown, doesStand);
}
} else {
- desTorque = pi_w(FilterTrajectory(0.0f) - omega); // state space controller for balance, calc desired Torque
+ desTorque = pi_w(FilterTrajectory(0.0f) - omega); // state space controller for balance
}
// 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