Revision 1:d90cd9d60f7e, committed 2020-11-30

Comitter:

mkistler

Date:

Mon Nov 30 21:31:20 2020 +0000

Parent:

0:eb5dec59e660

Commit message:

Final code.

Changed in this revision

--- a/main.cpp	Mon Nov 30 21:25:57 2020 +0000
+++ b/main.cpp	Mon Nov 30 21:31:20 2020 +0000
@@ -2,24 +2,25 @@
 
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
-LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
-Timer t;
+LSM9DS1 lol(p28, p27, 0xD6, 0x3C); // IMU pins.
+Timer t; // Timer to collect time series.
 
-PwmOut servo(p21);
+PwmOut servo(p21); // Servo pin
 
 float PI = 3.14159265358979323846f;
-float angle;
-float pulseWidth;
+float angle; // Angle of the servo
+float pulseWidth; // Pulse width and other pulse variables help communicate to the servo the correct angle through the PMW
 float pulseCoeff = 10.0;
 float pulseOffset = 400;
 
 void mag_correction(float mx, float my, float mz, float mag_c[3])
 {
-    float bias[3] = {0.2230,0.8809,0.1835};
+    float bias[3] = {0.2230,0.8809,0.1835}; // These matrices for bias and scale were found from the previous calibration
     float scale[3][3] = {{0.9918,0.0913,0.0922},
         {0.0913,1.6211,0.1597},
         {0.0922,0.1597,0.6479}
     };
+    // Full completing the calibration using the values of bias and scale.
     mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
     mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
     mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
@@ -28,11 +29,11 @@
 int main()
 {
     
-    float roll, pitch, yaw;
+    float roll, pitch, yaw; // Creating variables
     float accel[3], mag[3], gyro[3];
 
-    lol.begin();
-    if(!lol.begin()) {
+    lol.begin(); // Starting the connecting to the imu
+    if(!lol.begin()) { // If it didn't connect, print the script below.
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     lol.calibrate(true);
@@ -41,19 +42,20 @@
     wait(1);
     t.start();
     while(1) {
-        lol.readMag();
+        lol.readMag(); // reading the different values from the IMU
         lol.readGyro();
         lol.readAccel();
-        accel[0] = lol.calcAccel(lol.ax);
+        accel[0] = lol.calcAccel(lol.ax); // Putting the acceleration values all into one matrix
         accel[1] = lol.calcAccel(lol.ay);
         accel[2] = - lol.calcAccel(lol.az);
-        gyro[0] = lol.calcGyro(lol.gx);
+        gyro[0] = lol.calcGyro(lol.gx); // Putting all the gyro values all into one matrix
         gyro[1] = lol.calcGyro(lol.gy);
         gyro[2] = lol.calcGyro(lol.gz);
 
-        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
-        mag[2] = -mag[2];
-
+        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag); // Correcting mag.
+        mag[2] = -mag[2]; // Making mag negative.
+        
+        // Calculating roll pitch and yaw from the values above.
         roll = atan2(accel[1], accel[2]/abs(accel[2])*(sqrt((accel[0]*accel[0])+(accel[2]*accel[2]))));
         pitch = -atan2(-accel[0], (sqrt((accel[1]*accel[1])+(accel[2]*accel[2]))));
         float Yh = (mag[1]*cos(roll)) - (mag[2]*sin(roll));
@@ -63,7 +65,7 @@
         yaw *= 180.0f / PI;
         roll *= 180.0f / PI;
 
-        if(yaw<=0) {
+        if(yaw<=0) { // Reseting yaw roll and pitch to 360 if it ever gets below zero.
             yaw = yaw+360;
         }
         if(roll<=0) {
@@ -73,17 +75,17 @@
             pitch = pitch+360;
         }
 
-        angle = yaw/4;
-        if (angle < 0) { // Ensuring the angle cannot exceed 180 or go below 0.
+        angle = yaw/4; // Setting the angle of the servo to be proportional to the angle of the yaw
+        if (angle < 0) { // Ensuring the angle cannot exceed 190 or go below 0.
             angle = 0;
         } else if (angle > 90) {
             angle = 90.0;
         }
-        pulseWidth = pulseCoeff * angle + pulseOffset;
-        servo.pulsewidth(pulseWidth/1000000.000);
+        pulseWidth = pulseCoeff * angle + pulseOffset; // Calculting the pulse width from the angle value
+        servo.pulsewidth(pulseWidth/1000000.000); // Setting the physical servo to the right angle.
 
 
-
+        // Printing the values to coolterm
         pc.printf("$IMU,3,11,%f,%3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f;\r\n", t.read(), lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.calcAccel(lol.ax), lol.calcAccel(lol.ay), lol.calcAccel(lol.az), roll, pitch, yaw, pulseWidth);
     }
 }