[DO NOT USE] firmware with complementary filter

firmware-arduino/selfbalance/imu.ino

@@ -0,0 +1,102 @@
+#include <Adafruit_MPU6050.h>
+
+Adafruit_MPU6050 mpu;
+Adafruit_Sensor *mpu_temp, *mpu_accel, *mpu_gyro;
+
+double dt;
+unsigned long imu_dt;
+
+/* Get a new normalized sensor event */
+sensors_event_t accel;
+sensors_event_t gyro;
+
+  
+void setup_imu(double dt_){
+  if (!mpu.begin()) {
+    Serial.println("Failed to find MPU6050 chip");
+    while (1) {
+      delay(100);
+    }
+  }
+
+  Serial.println("MPU6050 Found!");
+  mpu_accel = mpu.getAccelerometerSensor();
+  mpu_gyro = mpu.getGyroSensor();
+
+  imu_dt = (unsigned long) (dt_+1);
+  dt = imu_dt * 0.001;
+  Serial.print("DeltaT: ");
+  Serial.println(imu_dt);
+}
+
+
+void computeAngles(){
+  static float old_t = millis();
+
+  unsigned long m = millis();
+  unsigned long t = m - old_t;
+  
+  if(t < imu_dt) return;
+  old_t = m;
+
+  mpu_accel->getEvent(&accel);
+  mpu_gyro->getEvent(&gyro);
+
+  // angular velocity (rad/s) * time (s) = rotation angle (rad)
+  // backwards euler integration using the estimare of the previous loop and the new angular velocity reading
+  double giroAngleX = gyro.gyro.x * dt;
+  double giroAngleY = gyro.gyro.y * dt;
+  double giroAngleZ = gyro.gyro.z * dt;
+
+  angleX += giroAngleX;
+  angleY += giroAngleY;
+  angleZ += giroAngleZ;
+
+  // Get angles from accelerometer
+  // https://mwrona.com/posts/accel-roll-pitch/
+  double accRoll = atan2(accel.acceleration.y, accel.acceleration.z);
+  double accPitch = asin (accel.acceleration.x / sqrt ( accel.acceleration.x*accel.acceleration.x + accel.acceleration.y*accel.acceleration.y + accel.acceleration.z*accel.acceleration.z));
+
+  // A little complementary filter to avoid gyroscope drift
+  angleX = 0.96*angleX + 0.04*accRoll;
+  angleY = 0.96*angleY + 0.04*accPitch;
+  
+#ifdef PRINT_SENSOR_DATA
+  /* Display the results (acceleration is measured in m/s^2) */
+  Serial.print("\t\tAccel X: ");
+  Serial.print(accel.acceleration.x);
+  Serial.print(" \tY: ");
+  Serial.print(accel.acceleration.y);
+  Serial.print(" \tZ: ");
+  Serial.print(accel.acceleration.z);
+  Serial.println(" m/s^2 ");
+
+  /* Display the results (rotation is measured in rad/s) */
+  Serial.print("\t\tGyro X: ");
+  Serial.print(gyro.gyro.x);
+  Serial.print(" \tY: ");
+  Serial.print(gyro.gyro.y);
+  Serial.print(" \tZ: ");
+  Serial.print(gyro.gyro.z);
+  Serial.println(" radians/s ");
+ #endif
+
+#ifdef PRINT_ANGLES
+#ifdef PRINT_ACC_ANGLES
+  Serial.print("\t\\t Roll: ");
+  Serial.print(accRoll);
+  Serial.print(" \tPitch: ");
+  Serial.print(accPitch);
+  Serial.println(" radians ");
+#endif
+  
+  Serial.print("\t\tAngle X: ");
+  Serial.print(angleX);
+  Serial.print(" \tY: ");
+  Serial.print(angleY);
+  Serial.print(" \tZ: ");
+  Serial.print(angleZ);
+  Serial.println(" radians ");
+  Serial.println();
+#endif
+}

firmware-arduino/selfbalance/motors.ino

@@ -0,0 +1,127 @@
+
+int mot_pins[][3] = {
+  {16, 17, 20},
+  {18, 19, 21}
+};
+
+
+// PWM value at which the motors start moving (empirical)
+constexpr int base_pwm = 100  ; // Actually starts moving at 70
+// Max PWM value
+constexpr int max_pwm = 255;
+
+void setup_motors(){
+  pinMode(mot_pins[0][0], OUTPUT);
+  pinMode(mot_pins[0][1], OUTPUT);
+  pinMode(mot_pins[1][0], OUTPUT);
+  pinMode(mot_pins[1][1], OUTPUT);
+}
+
+double torque_to_pwm(double t){
+  return 149.43 * t + 89.17;
+}
+
+double speed_to_pwm(double speed){
+  double pwm = constrain(abs(speed), 0, 100);
+  pwm= map(pwm, 0, 100, base_pwm, max_pwm);
+
+  return pwm;
+}
+
+int sign(auto b){
+  return b == 0 ? 0 : b > 0 ? 1 : -1;
+}
+
+void move(int mot, double speed){
+  double pwm = speed_to_pwm(speed);
+  move_pwm(mot, pwm*sign(speed));
+}
+
+void move_pwm(int mot, int pwm){
+  if(pwm == 0){
+    digitalWrite(mot_pins[mot][0], LOW);
+    digitalWrite(mot_pins[mot][1], LOW);
+  }else if(pwm < 0){
+    digitalWrite(mot_pins[mot][0], LOW);
+    digitalWrite(mot_pins[mot][1], HIGH);
+  }else{
+    digitalWrite(mot_pins[mot][1], LOW);
+    digitalWrite(mot_pins[mot][0], HIGH);
+  }
+
+ analogWrite(mot_pins[mot][2], abs(pwm));
+}
+
+void test_motors(){
+  while(!Serial) delay(10);
+
+  int m = 0;
+  while(true){
+    digitalWrite(mot_pins[m][0], LOW);
+    digitalWrite(mot_pins[m][1], HIGH);
+    
+    for(int i = 0; i < 255; i++){
+      Serial.print("PWM: ");
+      Serial.println(i);
+      analogWrite(mot_pins[m][2], i);
+      delay(100);
+    }
+    delay(1000);
+    digitalWrite(mot_pins[m][0], LOW);
+    digitalWrite(mot_pins[m][1], LOW);
+    m = 1-m;
+  }
+}
+
+void test_motors_nodeadzone(){
+  int m = 0;
+  while(true){
+    for(int i = 0; i < 100; i++){
+      Serial.print("Speed: ");
+      Serial.println(i);
+      move(m, i);
+      delay(100);
+    }
+    delay(1000);
+    move(m, 0);
+    m = 1-m;
+  }
+}
+
+void test_motors_rpm(){
+  while(!Serial) delay(10);
+
+  unsigned long tstart = millis();
+  while(millis() - tstart < 200) {
+  move(0, 20);
+  }
+  move(0,0);
+}
+
+void test_motors_diff(){
+  move(MOT_SX, 50*MOT_SX_MULT);
+  move(MOT_DX, 50*MOT_DX_MULT);
+}
+
+void test_motors_torque(int motor){
+  while(1){
+    delay(1000);
+    Serial.println("Setup, waiting 8 secs");
+    delay(8000);
+    
+  for(int i = base_pwm; i < 255; i+= 15){
+    digitalWrite(mot_pins[motor][1], LOW);
+    digitalWrite(mot_pins[motor][0], HIGH);
+    Serial.print("PWM: ");
+    Serial.println(i);
+
+    analogWrite(mot_pins[motor][2], i);
+    delay(8000);
+    
+    digitalWrite(mot_pins[motor][0], LOW);
+    digitalWrite(mot_pins[motor][1], LOW);
+    delay(2000);
+    
+    }
+  }
+}

firmware-arduino/selfbalance/selfbalance.ino

@@ -0,0 +1,92 @@
+#include <ArduPID.h>
+
+#define MOT_DX 0
+#define MOT_SX 1
+
+constexpr float MOT_SX_MULT = 1.0;
+constexpr float MOT_DX_MULT = 1.0;
+
+ArduPID myController;
+
+// Calculated with matlab
+constexpr double KP = 1;
+constexpr double KI = 0.02;
+constexpr double KD = 0.05;
+
+double setpoint = 0;
+double output = 0;
+double input = 0;
+
+double setpoint_front = 0;
+double output_front;
+
+double angleX{0}, angleY{0}, angleZ{0};
+double angAccX{0}, angAccY{0};
+double accX{0}, accY{0};
+
+void setup(void) {
+  Serial.begin(9600);
+  delay(500);
+  
+  setup_imu(5);;
+  Serial.println("IMU up");
+  setup_motors();
+  Serial.println("Motors up");
+  
+  pinMode(LED_BUILTIN, OUTPUT);
+  digitalWrite(LED_BUILTIN, HIGH);
+
+  delay(200);
+   
+  myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
+  myController.setOutputLimits(-0.4, 0.4); // max torque motors can exhert
+  myController.setSampleTime(2);
+  myController.start();
+
+  delay(200);
+  digitalWrite(LED_BUILTIN, LOW);
+
+  test_motors_torque(1);
+  /*frontController.begin(&angleZ, &output_front, &setpoint_front, 100, 0, 1, P_ON_E, FORWARD);
+  frontController.setOutputLimits(-100,100);  
+  myController.setWindUpLimits(-PI, PI);
+  frontController.setSampleTime(1);
+  frontController.start();*/
+
+}
+
+double map_double(double x, double in_min, double in_max, double out_min, double out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+unsigned long t = 0;
+double oldpwm = 0;
+
+void loop(){
+  computeAngles();
+
+  //static double oldAngleY = 0;
+  //oldAngleY = 0.99 * oldAngleY + 0.01 * angleY;
+  input = angleY;
+  
+  
+  myController.compute();
+
+  double torque = abs(output) * 0.5;
+  double pwm = map_double(torque, 0, 0.2, 80, 255);
+  pwm *= sign(output);
+
+  Serial.println("input\toutput\ttorque\tpwm");
+  Serial.print(input, 6);
+  Serial.print("\t");
+  Serial.print(output, 6);
+  Serial.print("\t");
+  Serial.print(torque);
+  Serial.print("\t");
+  Serial.println(pwm);
+  
+  move_pwm(MOT_SX, pwm*MOT_SX_MULT);
+  move_pwm(MOT_DX, pwm*MOT_DX_MULT);
+
+  oldpwm = pwm;
+}