move gyroscope code into seperate file, add lqr code file

EENX15_LQR/EENX15_LQR.ino

@@ -1,13 +1,25 @@
-//gyroscope stuff
-#include <Adafruit_MPU6050.h>
-#include <Adafruit_Sensor.h>
-#include <Wire.h>
-
-Adafruit_MPU6050 mpu;
-
 //temporary variable to measure main loops
 int temp_loops;
 
+//lqr stuff
+const uint8_t statesNumber = 4;
+/** Low pass filter angular Position*/
+float angularPositionLP = 0;
+/** Zumo's angular position */
+float angularPosition = 0;
+/** Corrected angular position */
+float correctedAngularPosition = 0;
+/** Zumo's angular speed */
+float angularSpeed = 0;
+/** Motor's angular position */
+float motorAngularPosition = 0;
+/** Motor's angular speed */
+float motorAngularSpeed = 0;
+/** PWM signal applied to the motor's driver 400 is 100% cicle and -400 100% but inverse direction */
+int32_t speed;
+
+
+//gyro stuff
 float AccX, AccY, AccZ;
 float GyroX, GyroY, GyroZ;
 float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
@@ -48,85 +60,11 @@ unsigned long timeoldA;
 unsigned long timeoldB;
 
 void setup() {
-  //gyroscope
   Serial.begin(115200);
   while (!Serial)
     delay(10); // will pause Zero, Leonardo, etc until serial console opens
-
+  
-  Serial.println("Adafruit MPU6050 test!");
+  gyro_setup();
-
-  // Try to initialize!
-  if (!mpu.begin()) {
-    Serial.println("Failed to find MPU6050 chip");
-    while (1) {
-      delay(10);
-    }
-  }
-  Serial.println("MPU6050 Found!");
-
-  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
-  Serial.print("Accelerometer range set to: ");
-  switch (mpu.getAccelerometerRange()) {
-  case MPU6050_RANGE_2_G:
-    Serial.println("+-2G");
-    break;
-  case MPU6050_RANGE_4_G:
-    Serial.println("+-4G");
-    break;
-  case MPU6050_RANGE_8_G:
-    Serial.println("+-8G");
-    break;
-  case MPU6050_RANGE_16_G:
-    Serial.println("+-16G");
-    break;
-  }
-  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
-  Serial.print("Gyro range set to: ");
-  switch (mpu.getGyroRange()) {
-  case MPU6050_RANGE_250_DEG:
-    Serial.println("+- 250 deg/s");
-    break;
-  case MPU6050_RANGE_500_DEG:
-    Serial.println("+- 500 deg/s");
-    break;
-  case MPU6050_RANGE_1000_DEG:
-    Serial.println("+- 1000 deg/s");
-    break;
-  case MPU6050_RANGE_2000_DEG:
-    Serial.println("+- 2000 deg/s");
-    break;
-  }
-
-  mpu.setFilterBandwidth(MPU6050_BAND_184_HZ);
-  Serial.print("Filter bandwidth set to: ");
-  switch (mpu.getFilterBandwidth()) {
-  case MPU6050_BAND_260_HZ:
-    Serial.println("260 Hz");
-    break;
-  case MPU6050_BAND_184_HZ:
-    Serial.println("184 Hz");
-    break;
-  case MPU6050_BAND_94_HZ:
-    Serial.println("94 Hz");
-    break;
-  case MPU6050_BAND_44_HZ:
-    Serial.println("44 Hz");
-    break;
-  case MPU6050_BAND_21_HZ:
-    Serial.println("21 Hz");
-    break;
-  case MPU6050_BAND_10_HZ:
-    Serial.println("10 Hz");
-    break;
-  case MPU6050_BAND_5_HZ:
-    Serial.println("5 Hz");
-    break;
-  }
-
-  Serial.println("");
-  delay(100);
-
-  calibrateGyro();
   
   //motor
   pinMode(MotorPinA, OUTPUT);
@@ -149,58 +87,8 @@ void setup() {
 }
 
 void loop() {
+  gyro_loop();
   
-  /* Get new sensor events with the readings */
-  sensors_event_t a, g, temp;
-  mpu.getEvent(&a, &g, &temp);
-
-
-   //Subtract the offset values from the raw gyro values
-  gyro_x = g.gyro.x;
-  gyro_y = g.gyro.y;
-  gyro_z = g.gyro.z;
-
-  acc_x = a.acceleration.x;
-  acc_y = a.acceleration.y;
-  acc_z = a.acceleration.z;
-
-  gyro_x -= gyro_x_cal;                                                
-  gyro_y -= gyro_y_cal;                                                
-  gyro_z -= gyro_z_cal;   
-    previousTime = currentTime;        // Previous time is stored before the actual time read
-  currentTime = millis();            // Current time actual time read
-  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds                                             
-         
-  //Gyro angle calculations 
-  angle_pitch += gyro_x * elapsedTime * 180/PI;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable, rad/s ---> degrees
-  angle_roll += gyro_y * elapsedTime * 180/PI ;                                    //Calculate the traveled roll angle and add this to the angle_roll variable
-  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) sin function is in radians
-  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel
-  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel
-  
-  //Accelerometer angle calculations
-  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector
-  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
-  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle
-  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       //Calculate the roll angle
-  
-  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
-  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll
-
-  if(set_gyro_angles){                                                 //If the IMU is already started
-    angle_pitch = angle_pitch * 0.96 + angle_pitch_acc * 0.04;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
-    angle_roll = angle_roll * 0.96 + angle_roll_acc * 0.04;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
-  }
-  else{                                                                //At first start
-    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle 
-    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle 
-    set_gyro_angles = true;                                            //IMU started flag
-  }
-  
-  // complementary filter
-  angle_pitch_output = angle_pitch_output * 0.8 + angle_pitch * 0.2;   //Take 90% of the output pitch value and add 10% of the raw pitch value
-  angle_roll_output = angle_roll_output * 0.8 + angle_roll * 0.2;      //Take 90% of the output roll value and add 10% of the raw roll value
-
   if(temp_loops>100){
     Serial.println("");
     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
@@ -240,45 +128,6 @@ void loop() {
   } 
 }
 
-void calibrateGyro() {
-  for (int cal_int = 0; cal_int < 1000 ; cal_int ++){                  //Read the raw acc and gyro data from the MPU-6050 for 1000 times
-  sensors_event_t a, g, temp;
-  mpu.getEvent(&a, &g, &temp);
-
-  gyro_x = g.gyro.x;
-  gyro_y = g.gyro.y;
-  gyro_z = g.gyro.z;
-                                                 
-    gyro_x_cal += g.gyro.x  ;                                            //Add the gyro x offset to the gyro_x_cal variable
-    gyro_y_cal += g.gyro.y  ;                                           //Add the gyro y offset to the gyro_y_cal variable
-    gyro_z_cal += g.gyro.z  ;                                            //Add the gyro z offset to the gyro_z_cal variable
-
-    delay(3);                                                          //Delay 3us to have 250Hz for-loop
-  }
-
-  // divide by 1000 to get avarage offset
-  gyro_x_cal /= 1000;                                                 
-  gyro_y_cal /= 1000;                                                 
-  gyro_z_cal /= 1000;                                                 
-
-  loop_timer = micros();                                               //Reset the loop timer
-
-  if (half_revolutionsA >= 20) { 
-     rpmA = 30*1000/(millis() - timeoldA)*half_revolutionsA;
-     timeoldA = millis();
-     half_revolutionsA = 0;
-     Serial.println(rpmA);
-     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
-   }
-
- if (half_revolutionsB >= 20) { 
-     rpmB = 30*1000/(millis() - timeoldB)*half_revolutionsB;
-     timeoldB = millis();
-     half_revolutionsB = 0;
-     Serial.println(rpmB);
-     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
-   }
-}
 void magnet_detectA()//This function is called whenever a magnet/interrupt is detected by the arduino
  {
    half_revolutionsA++;

EENX15_LQR/LQR.ino

@@ -0,0 +1,20 @@
+/** Control Law*/
+const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237};
+/** Correction factor */
+const float scaleConst = 2.5;
+
+/**
+ * LQR control law
+ */
+void lqr() {
+  speed = 0;
+  
+  speed -= motorAngularPosition * K[0];
+  speed -= correctedAngularPosition * K[1];
+  speed -= motorAngularSpeed * K[2];
+  speed -= angularSpeed * K[3];
+
+  speed = speed*scaleConst;
+ 
+}
+

EENX15_LQR/gyro.ino

@@ -0,0 +1,177 @@
+//gyroscope stuff
+#include <Adafruit_MPU6050.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+Adafruit_MPU6050 mpu;
+
+void gyro_setup(){
+  Serial.println("Adafruit MPU6050 test!");
+
+  // Try to initialize!
+  if (!mpu.begin()) {
+    Serial.println("Failed to find MPU6050 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  Serial.println("MPU6050 Found!");
+
+  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
+  Serial.print("Accelerometer range set to: ");
+  switch (mpu.getAccelerometerRange()) {
+  case MPU6050_RANGE_2_G:
+    Serial.println("+-2G");
+    break;
+  case MPU6050_RANGE_4_G:
+    Serial.println("+-4G");
+    break;
+  case MPU6050_RANGE_8_G:
+    Serial.println("+-8G");
+    break;
+  case MPU6050_RANGE_16_G:
+    Serial.println("+-16G");
+    break;
+  }
+  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
+  Serial.print("Gyro range set to: ");
+  switch (mpu.getGyroRange()) {
+  case MPU6050_RANGE_250_DEG:
+    Serial.println("+- 250 deg/s");
+    break;
+  case MPU6050_RANGE_500_DEG:
+    Serial.println("+- 500 deg/s");
+    break;
+  case MPU6050_RANGE_1000_DEG:
+    Serial.println("+- 1000 deg/s");
+    break;
+  case MPU6050_RANGE_2000_DEG:
+    Serial.println("+- 2000 deg/s");
+    break;
+  }
+
+  mpu.setFilterBandwidth(MPU6050_BAND_184_HZ);
+  Serial.print("Filter bandwidth set to: ");
+  switch (mpu.getFilterBandwidth()) {
+  case MPU6050_BAND_260_HZ:
+    Serial.println("260 Hz");
+    break;
+  case MPU6050_BAND_184_HZ:
+    Serial.println("184 Hz");
+    break;
+  case MPU6050_BAND_94_HZ:
+    Serial.println("94 Hz");
+    break;
+  case MPU6050_BAND_44_HZ:
+    Serial.println("44 Hz");
+    break;
+  case MPU6050_BAND_21_HZ:
+    Serial.println("21 Hz");
+    break;
+  case MPU6050_BAND_10_HZ:
+    Serial.println("10 Hz");
+    break;
+  case MPU6050_BAND_5_HZ:
+    Serial.println("5 Hz");
+    break;
+  }
+
+  Serial.println("");
+  delay(100);
+
+  calibrateGyro();
+}
+
+void gyro_loop(){
+  
+  /* Get new sensor events with the readings */
+  sensors_event_t a, g, temp;
+  mpu.getEvent(&a, &g, &temp);
+
+
+   //Subtract the offset values from the raw gyro values
+  gyro_x = g.gyro.x;
+  gyro_y = g.gyro.y;
+  gyro_z = g.gyro.z;
+
+  acc_x = a.acceleration.x;
+  acc_y = a.acceleration.y;
+  acc_z = a.acceleration.z;
+
+  gyro_x -= gyro_x_cal;                                                
+  gyro_y -= gyro_y_cal;                                                
+  gyro_z -= gyro_z_cal;   
+    previousTime = currentTime;        // Previous time is stored before the actual time read
+  currentTime = millis();            // Current time actual time read
+  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds                                             
+         
+  //Gyro angle calculations 
+  angle_pitch += gyro_x * elapsedTime * 180/PI;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable, rad/s ---> degrees
+  angle_roll += gyro_y * elapsedTime * 180/PI ;                                    //Calculate the traveled roll angle and add this to the angle_roll variable
+  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) sin function is in radians
+  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel
+  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel
+  
+  //Accelerometer angle calculations
+  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector
+  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
+  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle
+  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       //Calculate the roll angle
+  
+  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
+  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll
+
+  if(set_gyro_angles){                                                 //If the IMU is already started
+    angle_pitch = angle_pitch * 0.96 + angle_pitch_acc * 0.04;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
+    angle_roll = angle_roll * 0.96 + angle_roll_acc * 0.04;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
+  }
+  else{                                                                //At first start
+    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle 
+    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle 
+    set_gyro_angles = true;                                            //IMU started flag
+  }
+  
+  // complementary filter
+  angle_pitch_output = angle_pitch_output * 0.8 + angle_pitch * 0.2;   //Take 90% of the output pitch value and add 10% of the raw pitch value
+  angle_roll_output = angle_roll_output * 0.8 + angle_roll * 0.2;      //Take 90% of the output roll value and add 10% of the raw roll value
+}
+
+void calibrateGyro() {
+  for (int cal_int = 0; cal_int < 1000 ; cal_int ++){                  //Read the raw acc and gyro data from the MPU-6050 for 1000 times
+  sensors_event_t a, g, temp;
+  mpu.getEvent(&a, &g, &temp);
+
+  gyro_x = g.gyro.x;
+  gyro_y = g.gyro.y;
+  gyro_z = g.gyro.z;
+                                                 
+    gyro_x_cal += g.gyro.x  ;                                            //Add the gyro x offset to the gyro_x_cal variable
+    gyro_y_cal += g.gyro.y  ;                                           //Add the gyro y offset to the gyro_y_cal variable
+    gyro_z_cal += g.gyro.z  ;                                            //Add the gyro z offset to the gyro_z_cal variable
+
+    delay(3);                                                          //Delay 3us to have 250Hz for-loop
+  }
+
+  // divide by 1000 to get avarage offset
+  gyro_x_cal /= 1000;                                                 
+  gyro_y_cal /= 1000;                                                 
+  gyro_z_cal /= 1000;                                                 
+
+  loop_timer = micros();                                               //Reset the loop timer
+
+  if (half_revolutionsA >= 20) { 
+     rpmA = 30*1000/(millis() - timeoldA)*half_revolutionsA;
+     timeoldA = millis();
+     half_revolutionsA = 0;
+     Serial.println(rpmA);
+     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
+   }
+
+ if (half_revolutionsB >= 20) { 
+     rpmB = 30*1000/(millis() - timeoldB)*half_revolutionsB;
+     timeoldB = millis();
+     half_revolutionsB = 0;
+     Serial.println(rpmB);
+     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
+   }
+}