misc docs stuff

Demonstrates both the analytical solution and the current PID values work

.gitignore

@@ -1,2 +1,5 @@
 matlab-sim/slprj/
 matlab-sim/*.slxc
+matlab-sim-v2/slprj/
+matlab-sim-v2/*.slxc
+**/*.autosave

README.md

@@ -4,15 +4,17 @@ My attempt at building a two-wheeled self-balancing robot. The project started a
 later evolved as a University project. I'll upload the university report to this repo as soon as it is complete. This repo will also be
 supported by a blog post on [emamaker.com](https://emamaker.com)
 
-The physical model of the robot has been derived by calculatin the Lagrangian function. This model
-is heavily non-linear and has been linearized around the 0° point (equilibrium).
+The physical model of the robot has been derived by calculating the Lagrangian function. This model
+is heavily non-linear and has been linearized around the 0° point of equilibrium.
 
 The linearized model has been reconstructed in MatLab Simulink and a discrete-time PID controller
-has been calculated by trial and error.
+has been calculated by trial and error. This PID Controller was used to dimension the motors (max. torque
+	in particular) and was later refined with the Ziegler-Nichols method.
 
 The robot has later been built using a Raspberry Pi Pico microcontroller paired with a MPU6050
 6-axis IMU, from which the pitch angle is derived with the use of a [Madgwick](https://ahrs.readthedocs.io/en/latest/filters/madgwick.html) filter.
-(Initially this was a simple complementary filter, but it was way to noise, check the blog post).
+(Initially this was a simple complementary filter, but it was way too subject to noise from the
+ accelerometer, check the blog post).
 The two motors (Hitec HS-322HD servomotors, check blog post for the choice
      of motors) are driven by a L298N motor driver. Everything is powered by a 2S 7.4V LiPo battery.
 

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
 
@@ -16,20 +16,31 @@ Pc = 0.083
 PID Kp = 0.6Kc, Ti = 0.5Pc Td = 0.125Pc
 Ki = KpTc/ti
 Kd = KpTd/Tc
-*/
+*/ 
 
-
-constexpr double KP = 6;
-constexpr double KI = 0.2;
+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;
 
 //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;
+constexpr double max_torque_dx = 0.36;
+constexpr double max_torque = max_torque_dx+max_torque_sx;
+constexpr double torque_sx_coeff = max_torque_sx/max_torque;
+constexpr double torque_dx_coeff = max_torque_dx/max_torque;
+
 void setup(void) {
   Serial.begin(9600);
   delay(500);
@@ -46,11 +57,17 @@ void setup(void) {
     update_imu();
   }
 
-  myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
-  myController.setOutputLimits(-0.72, 0.72);  // double of max torque motors can exhert
-  // myController.setWindUpLimits(-0.2 , 0.02);
-  myController.setSampleTime(1);
-  myController.start();
+  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();
+
 
 
   digitalWrite(LED_BUILTIN, LOW);
@@ -65,18 +82,30 @@ 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) * 0.5;
+  double torque = abs(output);
 
   double s = sign(output);
-  double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque)*s;
-  double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque)*s;
+  double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque*torque_dx_coeff)*s;
+  double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque*torque_sx_coeff)*s;
 
   move_pwm(MOT_SX, pwm_sx);
   move_pwm(MOT_DX, pwm_dx);
+
+
+  speed_input = torque*0.5*s;
 }

matlab-sim-v2/slprj/sim/varcache/self_balance_alone/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="a895ae6d-62f1-41eb-8898-3e45d1f4e90e">
+    <checksum>Lb/jesGfhHHIDu90dapcipftUCY9o9NhkvhP/XJ3BQXuHr3IUkBcW/fPUAxXIdZWqCUuUKCUuY5W8wtaxZ2B+w==</checksum>
+  </slcache.FileAttributes>
+</MF0>

matlab-sim-v2/slprj/sim/varcache/self_balance_customcontroller/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="81d5dcd0-043c-4202-b2eb-282248cfd3e7">
+    <checksum>dve/KW2AkD1hUjt0JOZll5FNSnvs4lU9s9KvUWTxKelL6/0n4gjtZ5QDJHRYboTA5D96TchHng97Avmp6nil6w==</checksum>
+  </slcache.FileAttributes>
+</MF0>

matlab-sim-v2/slprj/sim/varcache/self_balance_pidcont/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="78aa8f73-1e99-4ad7-baec-c27c8b664994">
+    <checksum>z9UdsWR5MLrsZnhLZ6JnGXqVn0khIn4P8lHVs5tHu/tzURqRLyUXB9/aOwDYj3nVU1QjOwCshC7ZzrwihHH9Uw==</checksum>
+  </slcache.FileAttributes>
+</MF0>

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)
+%}