pid: recalibrate with ziegler-nichols2 and matlab simulation

.gitignore

@@ -0,0 +1,2 @@
+matlab-sim/slprj/
+matlab-sim/*.slxc

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

@@ -9,14 +9,18 @@ constexpr float MOT_DX_MULT = 1.0;
 ArduPID myController;
 
 // Calculated with matlab, then adjusted by hand
-// Decent values?
-/*constexpr double KP = 2.5;
-constexpr double KI = 0.04;
-constexpr double KD = 0.03;*/
 
-// Event better
-constexpr double KP = 4;
-constexpr double KI = 5;
+/* With KP = 10, about  12 periods of oscillation in 1sec
+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 KD = 0.1;
 
 //double setpoint = -0.015;
@@ -32,22 +36,23 @@ void setup(void) {
 
   setup_imu();
   setup_motors();
-
+ 
   pinMode(LED_BUILTIN, OUTPUT);
   digitalWrite(LED_BUILTIN, HIGH);
 
   // Let the initial error from madgwick filter discharge without affecting the integral term of the PID
   unsigned long t = millis();
-  while (millis() - t < 2000) {
+  while (millis() - t < 3000) {
     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.00001, 0.0001);
+  // myController.setWindUpLimits(-0.2 , 0.02);
   myController.setSampleTime(1);
   myController.start();
 
+
   digitalWrite(LED_BUILTIN, LOW);
 }
 
@@ -55,15 +60,13 @@ double map_double(double x, double in_min, double in_max, double out_min, double
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-unsigned long t = 0;
-double oldpwm = 0;
-
 void loop() {
+  compute();
+}
+
+void compute(){
   update_imu();
 
-  /*static double oldPitch = 0;
-  input = 0.5*oldPitch + 0.5*pitch;
-  oldPitch = pitch;*/
   input = pitch;
 
   myController.compute();