Revision 1:3258d62af038, committed 2018-08-01

Comitter:

ryanlin97

Date:

Wed Aug 01 22:39:10 2018 +0000

Parent:

0:ad7a811c859f

Child:

2:7d6467fa1977

Commit message:

imu changes;

Changed in this revision

--- a/chair_BNO055.cpp	Fri Jul 20 17:54:09 2018 +0000
+++ b/chair_BNO055.cpp	Wed Aug 01 22:39:10 2018 +0000
@@ -1,118 +1,159 @@
 #include "chair_BNO055.h"
 
-Timer t;
-
-
-chair_BNO055::chair_BNO055(){
-  imu = new BNO055(SDA, SCL);
-}
-
-chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin){
-  imu = new BNO055(sda_pin, scl_pin);
+static float total_yaw = 0;
+static float minGyroZ;
+chair_BNO055::chair_BNO055(Serial* out, Timer* time)
+{
+    imu = new BNO055(SDA, SCL);
+    usb = out;
+    t = time;
+    start = false;
 }
 
-void chair_BNO055::setup(){
-  imu->reset();
-  while (imu->check() == 0) {
-    wait(.5);
-  }
-  imu->set_accel_units(MPERSPERS);
-  imu->setmode(OPERATION_MODE_AMG);
-  imu->get_calib();
-  imu->write_calibration_data();
-  imu->set_angle_units(DEGREES);
-  imu->setmode(OPERATION_MODE_AMG); //put into while loop 
-  t.start();
+chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time)
+{
+    imu = new BNO055(sda_pin, scl_pin);
+    usb = out;
+    t = time;
+    start = false;
 }
 
-double chair_BNO055::accel_x(){
-  imu->get_accel();
-  return (double)imu->accel.x;
-}
+void chair_BNO055::setup()
+{
+    imu->reset();
+    usb->printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
+    while (imu->check() == 0) {
+        usb->printf("Bosch BNO055 is NOT available!!\r\n");
+        wait(.5);
+    }
+    usb->printf("BNO055 found\r\n\r\n");
+    usb->printf("Chip          ID: %0z\r\n",imu->ID.id);
+    usb->printf("Accelerometer ID: %0z\r\n",imu->ID.accel);
+    usb->printf("Gyroscope     ID: %0z\r\n",imu->ID.gyro);
+    usb->printf("Magnetometer  ID: %0z\r\n\r\n",imu->ID.mag);
+    usb->printf("Firmware version v%d.%0d\r\n",imu->ID.sw[0],imu->ID.sw[1]);
+    usb->printf("Bootloader version v%d\r\n\r\n",imu->ID.bootload);
+    imu->set_accel_units(MPERSPERS);
+    imu->setmode(OPERATION_MODE_AMG);
+    imu->read_calibration_data();
+    imu->write_calibration_data();
+    imu->set_angle_units(DEGREES);
 
-double chair_BNO055::accel_y(){
-  imu->get_accel();
-  return (double)imu->accel.y;
+    t->start();
+    chair_BNO055::calibrate_yaw();
+
 }
 
-double chair_BNO055::accel_z(){
-  imu->get_accel();
-  return (double)imu->accel.z;
+void chair_BNO055::calibrate_yaw() {
+    float start = t->read();
+    
+    while( t->read()- start <= CAL_TIME ){
+        total_yaw = boxcar(chair_BNO055::angle_north());
+        usb->printf("total_yaw %f\n", total_yaw);
+        }
+    }
+
+
+double chair_BNO055::accel_x()
+{
+    imu->get_accel();
+    return (double)imu->accel.x;
 }
 
-double chair_BNO055::gyro_x(){
-  imu->get_gyro();
-  return (double)imu->gyro.x;
+double chair_BNO055::accel_y()
+{
+    imu->get_accel();
+    return (double)imu->accel.y;
 }
 
-double chair_BNO055::gyro_y(){
-  imu->get_gyro();
-  return (double)imu->gyro.y;
+double chair_BNO055::accel_z()
+{
+    imu->get_accel();
+    return (double)imu->accel.z;
 }
 
-double chair_BNO055::gyro_z(){
-  imu->get_gyro();
-  return (double)imu->gyro.z;
+double chair_BNO055::gyro_x()
+{
+    imu->get_gyro();
+    return (double)imu->gyro.x;
 }
 
-double chair_BNO055::angle_north(){
-  imu->get_mag();
-  float x = imu->mag.x;
-  float y = imu->mag.y;
-  
-  float result = x/y;
-  
-  float angleToNorth;
-  if(imu->mag.y>0)
-    angleToNorth = 90.0 - atan(result)*180/PI;
-  else if(imu->mag.y<0)
-    angleToNorth = 270.0 - atan(result)*180/PI;
-  else if(y == 0 && x <= 0)
-    angleToNorth = 180;
-  else if(y == 0 && x > 0)
-    angleToNorth = 0;
-  
-  return (double)angleToNorth;
+double chair_BNO055::gyro_y()
+{
+    imu->get_gyro();
+    return (double)imu->gyro.y;
+}
+
+double chair_BNO055::gyro_z()
+{
+    imu->get_gyro();
+    return lowPass(imu->gyro.z);
+}
+
+double chair_BNO055::angle_north()
+{
+    imu->get_mag();
+    float x = imu->mag.x - 520;
+    float y = imu->mag.y;
+
+    float result = x/y;
+
+    float angleToNorth;
+    if(imu->mag.y>0)
+        angleToNorth = 90.0 - atan(result)*180/PI;
+    else if(imu->mag.y<0)
+        angleToNorth = 270.0 - atan(result)*180/PI;
+    else if(y == 0 && x <= 0)
+        angleToNorth = 180;
+    else if(y == 0 && x > 0)
+        angleToNorth = 0;
+
+    return (double)angleToNorth;
 }
 
-double chair_BNO055::roll(){
-  imu->get_accel();
-  imu->get_gyro();
-  
-  float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) + 
-  (imu->accel.z * imu->accel.z))));
-  roll = roll*57.3;
-  
-  t.reset();
+double chair_BNO055::roll()
+{
+    imu->get_accel();
+    imu->get_gyro();
 
-  return (double)roll;
+    float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) +
+                                        (imu->accel.z * imu->accel.z))));
+    roll = roll*57.3;
+
+    t->reset();
+
+    return (double)roll;
 }
 
-double chair_BNO055::pitch(){
-  imu->get_accel();
-  imu->get_gyro();
-  
-  float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) + 
-  (imu->accel.z * imu->accel.z))));
-  pitch = pitch*57.3;
-  
-  t.reset();
+double chair_BNO055::pitch()
+{
+    imu->get_accel();
+    imu->get_gyro();
 
-  return (double)pitch;
+    float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) +
+                                         (imu->accel.z * imu->accel.z))));
+    pitch = pitch*57.3;
+
+    t->reset();
+
+    return (double)pitch;
 }
 
-double chair_BNO055::yaw(){
-  imu->get_gyro();
-  float yaw = (yaw - t.read()*imu->gyro.z);
-  
-  if(yaw > 360)
-    yaw -= 360;
-  if(yaw < 0)
-    yaw += 360;
-    
-  t.reset();
+double chair_BNO055::yaw()
+{
+    float gyroZ = chair_BNO055::gyro_z();
 
-  return (double)yaw;
+    if(abs(gyroZ) >= .3) {
+        total_yaw = (total_yaw - t->read()*gyroZ);
+        //usb->printf("gyroZ %f\n", gyroZ);
+    }
+    t->reset();
+
+    if(total_yaw > 360)
+        total_yaw -= 360;
+    if(total_yaw < 0)
+        total_yaw += 360;
+
+     //usb->printf("yaw %f\n", total_yaw);
+    return (double)total_yaw;
 }
-
-

--- a/chair_BNO055.h	Fri Jul 20 17:54:09 2018 +0000
+++ b/chair_BNO055.h	Wed Aug 01 22:39:10 2018 +0000
@@ -1,6 +1,7 @@
 #ifndef CHAIR_BNO055_H
 #define CHAIR_BNO055_H
 
+#include "filter.h"
 #include  "mbed.h"
 #include  "math.h"
 #include  "BNO055.h"
@@ -8,13 +9,14 @@
 #define PI 3.141593
 #define SDA D14
 #define SCL D15
-
+#define SAMPLEFREQ 50
+#define CAL_TIME 3
 
 class chair_BNO055
 {
 public:
-  chair_BNO055();
-  chair_BNO055(PinName sda_pin, PinName scl_pin);
+  chair_BNO055(Serial* out, Timer* time);
+  chair_BNO055(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time);
   void setup();
   double accel_x();
   double accel_y();
@@ -26,9 +28,14 @@
   double yaw();
   double pitch();
   double roll();
-  
+  BNO055* imu;
 private:
-  BNO055* imu;
+  //BNO055* imu;
+  Serial* usb;
+  Timer* t;
+  bool start;
+  void setStart();
+  void calibrate_yaw();
 };
 
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filter.cpp	Wed Aug 01 22:39:10 2018 +0000
@@ -0,0 +1,52 @@
+float lowPass(float sample)
+{
+    static const float a[4] = {1.00000000e+00,-2.77555756e-16,3.33333333e-01,-1.85037171e-17};
+    static const float b[4] = {0.16666667,0.5,0.5,0.16666667};
+// x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
+    static volatile float x[4] = {0};
+// x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
+    static volatile float y[4] = {0};
+    x[0] = sample;
+// Calculate the output filtered signal based on a weighted sum of previous inputs/outputs
+    y[0] = (b[0]*x[0]+b[1]*x[1]+b[2]*x[2]+b[3]*x[3])-(a[1]*y[1]+a[2]*y[2]+a[3]*y[3]);
+    y[0] /= a[0];
+// Shift the input signals by one timestep to prepare for the next call to this function
+    x[3] = x[2];
+    x[2] = x[1];
+    x[1] = x[0];
+// Shift the previously calculated output signals by one time step to prepare for the next call to this function
+    y[3] = y[2];
+    y[2] = y[1];
+    y[1] = y[0];
+    return y[0];
+}
+
+float boxcar(float sample)
+{
+    static const int boxcarWidth = 30; // Change this value to alter boxcar length
+    static float recentSamples[boxcarWidth] = {0}; // hold onto recent samples
+    static int readIndex = 0; // the index of the current reading
+    static float total = 0; // the running total
+    static float average = 0; // the average
+// subtract the last reading:
+    total = total - recentSamples[readIndex];
+// add new sample to list (overwrite oldest sample)
+    recentSamples[readIndex] = sample;
+// add the reading to the total:
+    total = total + recentSamples[readIndex];
+// advance to the next position in the array:
+    readIndex = readIndex + 1;
+// if we're at the end of the array...
+    if (readIndex >= boxcarWidth) {
+// ...wrap around to the beginning:
+        readIndex = 0;
+    }
+// calculate the average:
+    average = total / boxcarWidth;
+// send it to the computer as ASCII digits
+    return average;
+}
+float complement(float x, float y , float ratio)
+{
+    return (ratio*x + (1 - ratio)*y);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filter.h	Wed Aug 01 22:39:10 2018 +0000
@@ -0,0 +1,7 @@
+#ifndef FILTER_H
+#define FILTER_H
+
+float lowPass(float sample);
+float complement(float x, float y , float ratio);
+float boxcar(float sample);
+#endif
\ No newline at end of file