sim: nonlinear simulation

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

@@ -16,10 +16,10 @@ 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 KP = 5.8;
 constexpr double KI = 0.2;
 constexpr double KD = 0.1;
 
@@ -30,6 +30,12 @@ double input = 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);
@@ -47,7 +53,7 @@ void setup(void) {
   }
 
   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.setOutputLimits(-max_torque, max_torque);  // double of max torque motors can exhert
   // myController.setWindUpLimits(-0.2 , 0.02);
   myController.setSampleTime(1);
   myController.start();
@@ -71,11 +77,11 @@ void 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);

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>