Added gyroscope + complementary filter for pitch angle + lowpass filter for motors

Main/IMU.ino

@@ -1,9 +1,22 @@
-//IMU variables
+//CONSTANTS
+const float   alpha               = 0.95;
+const int     acc_overflow_value  = 65535;
+const int     gyro_overflow_value = 4558;   // 4096+512-50=4558 ?
+
+
+//IMU VARIABLES
 int           ax_, ay_, az_;
 int           ax, ay, az;
-int   cx, cy, cz;
+int           cx_, cy_, cz_;
-float gx, gy, gz, gt;
+int           gx_, gy_, gz_;
+int           gx, gy, gz;
+//float gt;
 float         acc_pitch;
+float         pitch_rate;
+float         dAngle, estAngle;
+float         pitch               = 0;
+float         pitch_prev          = 0;
+
 
 void readIMU() {
   ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() );
@@ -14,51 +27,92 @@ void readIMU() {
   //  cy = IMU.compass_y( IMU.readFromCompass() );
   //  cz = IMU.compass_z( IMU.readFromCompass() );
 
-  gx = IMU.gyro_x( IMU.readGyro() );
+  gx_ = IMU.gyro_x( IMU.readGyro() );
-  gy = IMU.gyro_y( IMU.readGyro() );
+  gy_ = IMU.gyro_y( IMU.readGyro() );
-  gz = IMU.gyro_z( IMU.readGyro() );
+  gz_ = IMU.gyro_z( IMU.readGyro() );
   //  gt = IMU.temp  ( IMU.readGyro() );
 
 
   //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
-  if (ax_ > 60000) {
+  //Convert accelerometer
-    ax = (ax_ - 65535);
+  if (ax_ > 32768) {
+    ax = (ax_ - acc_overflow_value);
   }
   else  {
     ax = ax_;
   }
 
-  if (ay_ > 60000) {
+  if (ay_ > 32768) {
-    ay = (ay_ - 65535);
+    ay = (ay_ - acc_overflow_value);
   }
   else  {
     ay = ay_;
   }
 
-  if (az_ > 60000) {
+  if (az_ > 32768) {
-    az = (az_ - 65535);
+    az = (az_ - acc_overflow_value);
   }
   else  {
     az = az_;
   }
 
-  acc_pitch = -1*atan2(ax,sqrt((pow(ay,2)+pow(az,2))))*180/3.14;
+
+  //Convert gyro
+  //  Gyroscope coordinate system
+  //  gx -  Pitch rate
+  //  gy -  Roll rate
+  //  gz -  Yaw rate
+
+  //  Gyro is calibrated for +-2000deg/s
+  //  Conversion is happening in GY_85.h line 48-50
+
+  if (gx_ > 2279) {
+    gx = (gx_ - gyro_overflow_value);
+  }
+  else  {
+    gx = gx_;
+  }
+
+  if (gy_ > 2279) {
+    gy = (gy_ - gyro_overflow_value);
+  }
+  else  {
+    gy = gy_;
+  }
+
+  if (gz_ > 2279) {
+    gz = (gz_ - gyro_overflow_value);
+  }
+  else  {
+    gz = gz_;
+  }
 
 
-//Serial plotter
+  // Pitch angle from accelerometer
-//  map(ax,-140,140,-1,1)
+  acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI;
-//  map(ay,-140,140,-1,1)
-//  map(az,-140,140,-1,1)
-
-//      Serial.print  ( " x:" );
-//  Serial.print  ( ax );
-//  Serial.print  ( "," );
-//  Serial.print  ( ay );
-//  Serial.print  ( "," );
-//  Serial.println  ( az );
-//  Serial.print  ( "," );
-//  Serial.println  ( acc_pitch);
 
 
+  //Pitch rate from gyroscope
+  pitch_rate = -gx;
 
+
+  //Complementary filter
+  dAngle      = pitch_rate * dT * pow(10, -6);
+  pitch       = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
+  pitch_prev  = pitch;
+
+
+  //Serial plotter
+//  Serial.print  ( "Pitch:" );
+//  Serial.print  ( pitch );
+//  Serial.print  (" ");
+//  Serial.print  ( "Accelerometer_Pitch:" );
+//  Serial.print  ( acc_pitch );
+//  Serial.print  (" ");
+  //  Serial.print  ( "," );
+  //  Serial.println  ( gz );
+  //  Serial.print  ( "," );
+  //  Serial.println  ( gt );
+  //  Serial.print  ( "," );
+  //  Serial.println  ( acc_pitch);
 }

Main/Main.ino

@@ -1,3 +1,5 @@
+#include <GY_85.h>
+
 //Import
 #include "GY_85.h"
 #include <Wire.h>
@@ -22,6 +24,8 @@ const byte  IMU_I2C_SDA     = 27;
 
 //System variables
 unsigned long tNow          = micros();
+unsigned long tLast         = micros()+13000;
+int           dT            = 0;
 
 
 //Motor variables
@@ -156,6 +160,8 @@ void setup() {
 
   //Initialize IMU
   IMU.init();
+  //Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
+  IMU.GyroCalibrate();
   delay(10);
 
   //Initialize encoder interrupts
@@ -188,13 +194,14 @@ void setup() {
   ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
   ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
 
-  Serial.println("Reference,Actual,SpeedCommand");
+//  Serial.println("Reference,Actual,SpeedCommand");
 
 }
 
 void loop() {
   //Update system variables
   tNow  = micros();
+  dT    = tNow - tLast;             //[Cycle time in microseconds]
 
   ////  //Only print encoder value if value changed since last print
   //  if (m1Raw != m1RawLast) {
@@ -212,7 +219,7 @@ void loop() {
   //Act
   motorControl();
 
-
+  tLast = tNow;
   //Delay
   delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
 }

Main/motorControl.ino

@@ -1,16 +1,17 @@
 //Constants
 const float   wheel_diameter  = 0.067708;
 const float   pulses_per_turn = 1320.0;
+const float   filt_gain = 15;
 //Tuning
-const float   K = 2.0;
+const float   K = 3.5;
-const float   I = 10.0;
+const float   I = 7.5;
 
 //Help variables
-int           dEnc, dT;
+int           dEnc;
-float         dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered;
+float         dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered;
-unsigned long tLast;
+float         dVel, diff;
 
-float         speed_setp, referenceSpeed, actualSpeed;
+float         speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered;
 float         error, iError;
 int           speedCMD;
 
@@ -18,9 +19,10 @@ int           speedCMD;
 void motorControl() {
 
   //Speed Controller
-  referenceSpeed  = acc_pitch * ((4096.0) / (90.0));
+  referenceSpeed  = pitch * ((4096.0) / (90.0));
   actualSpeed     = lin_vel * 4096.0;
-  error           = referenceSpeed - actualSpeed;
+  actualSpeedFiltered = lin_vel_filtered * 4096.0;
+  error           = referenceSpeed - actualSpeedFiltered;
   iError          = iError + (error * dT * pow(10, -6) * I);
   speedCMD        = round((error * K) + iError);
 
@@ -46,13 +48,7 @@ void motorControl() {
     ledcWrite(1, -1 * speedCMD);
     ledcWrite(2, 0);
   }
-  //  Serial.print  (acc_pitch);
-  //  Serial.print  ( "," );
-  //  Serial.println(speed_setp);
 
-  //
-  //  ledcWrite(1, 0);
-  //  ledcWrite(2, 0);
 
 
   //Motor 2
@@ -60,19 +56,38 @@ void motorControl() {
   //  ledcWrite(4, 255);
 
 
+  //No speed command
+  //  ledcWrite(1, 0);
+  //  ledcWrite(2, 0);
+  ledcWrite(3, 0);
+  ledcWrite(4, 0);
+
+
   //Calculate speed from encoders
   dEnc    = m1Raw - m1RawLast;        //[Number of encoder pulses this cycle]
-  dT      = tNow - tLast;             //[Cycle time in microseconds]
+
   dTurn   = dEnc / pulses_per_turn;   //[Amount wheel turned this cycle. 1 = full rotation]
 
   ang_vel = (dTurn * 2 * PI) / (dT * 0.000001);
   lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001);
 
-  Serial.print(referenceSpeed);
+  //  Lowpass filter
-  Serial.print  ( "," );
+  //  lin_vel_filtered        = lin_vel_filtered + ((lin_vel - lin_vel_filtered)*(1-abs(diff))* dT * pow(10, -6)*filt_gain);
-  Serial.print(actualSpeed);
+  lin_vel_filtered        = lin_vel_filtered + ((lin_vel - lin_vel_filtered) * dT * pow(10, -6) * filt_gain);
-  Serial.print  ( "," );
+
-  Serial.println(speedCMD);
+
+  //  Serial plotter
+  Serial.print("referenceSpeed:");
+  Serial.print(referenceSpeed * (100.0 / 4096.0));
+  Serial.print(" ");
+//  Serial.print("actualSpeed:");
+//  Serial.print(actualSpeed * (100.0 / 4096.0));
+//  Serial.print(" ");
+  Serial.print("actualSpeedFiltered:");
+  Serial.print(actualSpeedFiltered * (100.0 / 4096.0));
+  Serial.print(" ");
+  Serial.print("speedCMD:");
+  Serial.println(speedCMD * (100.0 / 4096.0));
   //    Serial.print  ( "," );
   //  Serial.println(lin_vel);
   //  Serial.print  ( "," );
@@ -80,6 +95,6 @@ void motorControl() {
 
   //Update variables for next scan cycle
   m1RawLast = m1Raw;
-  tLast = tNow;
+
 
 }