sim: nonlinear simulation

Demonstrates both the analytical solution and the current PID values work

firmware-arduino/selfbalance-madgwick/selfbalance-madgwick.ino

@@ -6,7 +6,7 @@
 constexpr float MOT_SX_MULT = 1.0;
 constexpr float MOT_DX_MULT = 1.0;
 
-ArduPID angleCtrl, speedCtrl;
+ArduPID myController;
 
 // Calculated with matlab, then adjusted by hand
 
@@ -18,21 +18,16 @@ Ki = KpTc/ti
 Kd = KpTd/Tc
 */ 
 
-constexpr double kponkd = 935.9697;
-constexpr double kionkd = 1.3189e+04;
-constexpr double KD = 0.1;
+
 constexpr double KP = 5.8;
 constexpr double KI = 0.2;
+constexpr double KD = 0.1;
 
 //double setpoint = -0.015;
 double setpoint = 0.0;
 double output = 0;
 double input = 0;
 
-double speed_setpoint = 0.0;
-double speed_output = 0.0;
-double speed_input = 0.0;
-
 double roll{ 0 }, pitch{ 0 }, yaw{ 0 };
 
 constexpr double max_torque_sx = 0.392;
@@ -57,17 +52,11 @@ void setup(void) {
     update_imu();
   }
 
-  angleCtrl.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
-  angleCtrl.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
-  // angleCtrl.setWindUpLimits(-0.2 , 0.02);
-  angleCtrl.setSampleTime(1);
-  angleCtrl.start();
-
-  speedCtrl.begin(&speed_input, &speed_output, &speed_setpoint, 0, 0.12, 0, P_ON_E, BACKWARD);
-  speedCtrl.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
-  speedCtrl.setSampleTime(1);
-  speedCtrl.start();
-
+  myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
+  myController.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
+  // myController.setWindUpLimits(-0.2 , 0.02);
+  myController.setSampleTime(1);
+  myController.start();
 
 
   digitalWrite(LED_BUILTIN, LOW);
@@ -82,20 +71,11 @@ void loop() {
 }
 
 void compute(){
-  static unsigned long last_time;
-  unsigned long t = millis();
-  unsigned long dt = millis() - t;
-  last_time = t;
-
-  speedCtrl.compute();
-  Serial.print(speed_input);
-  Serial.print(" ");
-  Serial.println(speed_output);
-
   update_imu();
-  input = speed_output-pitch;
 
-  angleCtrl.compute();
+  input = pitch;
+
+  myController.compute();
 
   double torque = abs(output);
 
@@ -105,7 +85,4 @@ void compute(){
 
   move_pwm(MOT_SX, pwm_sx);
   move_pwm(MOT_DX, pwm_dx);
-
-
-  speed_input = torque*0.5*s;
 }

matlab-sim/self_balance_tf.m

@@ -1,50 +0,0 @@
-% Model of self balancing robot for PID controller
-% See scheme anf formulas on notebook
-
-clear all
-clc
-
-load('params.mat')
-
-
-I_wheel = M_wheel*(r^2); %placeholder
-I_body = M_body*(l^2)/3; %placeholder
-
-nums1 = -(M_body + 2*(I_wheel/r^2) + 2*M_wheel + M_body*l/r);
-nums0 = -2*b/r;
-num = [nums1 nums0];
-
-dens3 = I_body * M_body + 2*I_wheel*(I_body*M_body*l^2)/(r^2)  + 2*I_body*M_wheel + 2*M_wheel*M_body*l^2;
-dens2 = (2*b/r)*(I_body+M_body*g);
-dens1 = -( ((M_body^2)*g*l) + 2*(I_wheel*M_body*g*l)/(r^2) + 2*M_body*M_wheel*g*l );
-dens0 = -2*(b*M_body*g*l) / r;
-den = [dens3 dens2 dens1 dens0];
-
-w = tf(num, den)
-
-sys = zpk(w);
-sys.DisplayFormat='roots'
-
-%sys.Z{1}
-% sys.P{1}(1)
-
-K = sys.K
-p1 = sys.P{1}(1)
-p2 = sys.P{1}(3)
-p3 = sys.P{1}(2)
-
-%display('Kd must be [' + num2str(p3/K) + ', 0]');
-
-% Scelgo Kd così
-kd = -0.001
-kponkd = (p1+p2);
-kionkd = (p1*p2);
-kp = kponkd*kd
-ki = kionkd*kd
-
-% rlocus(sys, -sys)
-%{
-bode(w)
-figure;
-nyquist(w)
-%}