misc docs stuff

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 myController;
+ArduPID angleCtrl, speedCtrl;
 
 // Calculated with matlab, then adjusted by hand
 
@@ -18,16 +18,21 @@ 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;
@@ -52,11 +57,17 @@ void setup(void) {
     update_imu();
   }
 
-  myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
+  angleCtrl.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
-  myController.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
+  angleCtrl.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
-  // myController.setWindUpLimits(-0.2 , 0.02);
+  // angleCtrl.setWindUpLimits(-0.2 , 0.02);
-  myController.setSampleTime(1);
+  angleCtrl.setSampleTime(1);
-  myController.start();
+  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();
+
 
 
   digitalWrite(LED_BUILTIN, LOW);
@@ -71,11 +82,20 @@ 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;
 
-  input = pitch;
+  angleCtrl.compute();
-
-  myController.compute();
 
   double torque = abs(output);
 
@@ -85,4 +105,7 @@ 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

@@ -0,0 +1,50 @@
+% 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)
+%}