Revision 5:e0ccaab3959a, committed 2018-07-17

Comitter:

ryanlin97

Date:

Tue Jul 17 07:19:04 2018 +0000

Parent:

4:29a27953fe70

Child:

6:0cd57bdd8fbc

Commit message:

imu added

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_imu.cpp	Tue Jul 17 07:19:04 2018 +0000
@@ -0,0 +1,121 @@
+#include "chair_imu.h"
+
+Serial pc(USBTX, USBRX);
+Timer t;
+
+chair_imu::chair_imu(){
+  imu = new BNO055(SDA, SCL);
+}
+
+chair_imu::chair_imu(PinName sda_pin, PinName scl_pin){
+  imu = new BNO055(sda_pin, scl_pin);
+}
+
+void chair_imu::setup(){
+  imu->reset();
+  pc.printf("Bosch Sensortec BNO055 test program on \r\n");
+  while (imu->check() == 0) {
+    pc.printf("Bosch BNO055 is NOT available!!\r\n");
+    wait(.5);
+  }
+  pc.printf("Bosch Sensortec BNO055 available \r\n");
+  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();
+}
+
+double chair_imu::accel_x(){
+  imu->get_accel();
+  return (double)imu->accel.x;
+}
+
+double chair_imu::accel_y(){
+  imu->get_accel();
+  return (double)imu->accel.y;
+}
+
+double chair_imu::accel_z(){
+  imu->get_accel();
+  return (double)imu->accel.z;
+}
+
+double chair_imu::gyro_x(){
+  imu->get_gyro();
+  return (double)imu->gyro.x;
+}
+
+double chair_imu::gyro_y(){
+  imu->get_gyro();
+  return (double)imu->gyro.y;
+}
+
+double chair_imu::gyro_z(){
+  imu->get_gyro();
+  return (double)imu->gyro.z;
+}
+
+double chair_imu::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_imu::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();
+
+  return (double)roll;
+}
+
+double chair_imu::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();
+
+  return (double)pitch;
+}
+
+double chair_imu::yaw(){
+  imu->get_gyro();
+  float yaw = (yaw - t.read()*imu->gyro.z);
+  
+  if(yaw > 360)
+    yaw -= 360;
+  if(yaw < 0)
+    yaw += 360;
+    
+  t.reset();
+
+  return (double)yaw;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_imu.h	Tue Jul 17 07:19:04 2018 +0000
@@ -0,0 +1,34 @@
+#ifndef Chair_imu
+#define Chair_imu
+
+#include  "mbed.h"
+#include  "math.h"
+#include  "BNO055.h"
+
+#define PI 3.141593
+#define SDA D14
+#define SCL D15
+
+class chair_imu
+{
+public:
+  chair_imu();
+  chair_imu(PinName sda_pin, PinName scl_pin);
+  void setup();
+  double accel_x();
+  double accel_y();
+  double accel_z();
+  double gyro_x();
+  double gyro_y();
+  double gyro_z();
+  double angle_north();
+  double roll();
+  double pitch();
+  double yaw();
+
+private:
+  BNO055* imu;
+
+};
+
+#endif

--- a/imu.cpp	Tue Jul 17 02:29:26 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-#include "imu.h"
-
-Imu::Imu(){
-  imu = new BNO055(SDA, SCL);
-}
-
-Imu::Imu(PinName sda_pin, PinName scl_pin){
-  imu = new BNO055(sda_pin, scl_pin);
-}
-void Imu::setup(){
-  imu.reset();
-  pc.printf("Bosch Sensortec BNO055 test program on \r\n");
-  while (imu.check() == 0) {
-    pc.printf("Bosch BNO055 is NOT available!!\r\n");
-    wait(.5);
-  }
-  pc.printf("Bosch Sensortec BNO055 available \r\n");
-  imu.set_accel_units(MPERSPERS);
-  imu.setmode(OPERATION_MODE_AMG);
-  imu.get_calib();
-  imu.write_calibration_data();
-  imu.set_angle_units(DEGREES);
-}
\ No newline at end of file

--- a/imu.h	Tue Jul 17 02:29:26 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,24 +0,0 @@
-#ifndef Imu
-#define imu
-
-#include  "mbed.h"
-#include  "math.h"
-#include    "BNO055.h"
-
-#define PI 3.141593
-#define SDA D14
-#define SCL D15
-
-class Imu
-{
-public:
-  Imu();
-  Imu(BNO055*)
-  void setup();
-
-
-private:
-
-};
-
-#endif

--- a/main.cpp	Tue Jul 17 02:29:26 2018 +0000
+++ b/main.cpp	Tue Jul 17 07:19:04 2018 +0000
@@ -1,7 +1,5 @@
 #include "wheelchair.h"
 
-Serial pc(USBTX, USBRX);
-
 AnalogIn x(A0);
 AnalogIn y(A1);
 

--- a/wheelchair.cpp	Tue Jul 17 02:29:26 2018 +0000
+++ b/wheelchair.cpp	Tue Jul 17 07:19:04 2018 +0000
@@ -4,14 +4,14 @@
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
+    imu = new chair_imu();
 }
 /*
 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
 */
 void Wheelchair::move(float x_coor, float y_coor)
 {
-    printf("raw is %f %f \n", x_coor, y_coor);
-    printf("x is %f y is %f \n", ((x_coor*1.6f) + 1.7f), ((y_coor*1.6f) + 1.7f)); 
+     
     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
     x->write(scaled_x);
@@ -19,6 +19,27 @@
     
 }
 
+void Wheelchair::turn_right(){
+    double start = imu->yaw();
+    double final = start + 90;
+    if(final > 360)
+        final -= 360;
+   
+    while(imu->yaw() <= final) {
+        Wheelchair::right();
+        } 
+}
+
+void Wheelchair::turn_left(){
+    double start = imu->yaw();
+    double final = start - 90;
+    if(final <0)
+        final += 360;
+   
+    while(imu->yaw() >= final) {
+        Wheelchair::left();
+        } 
+}
 void Wheelchair::forward()
 {
     x->write(high);

--- a/wheelchair.h	Tue Jul 17 02:29:26 2018 +0000
+++ b/wheelchair.h	Tue Jul 17 07:19:04 2018 +0000
@@ -1,8 +1,7 @@
 #ifndef wheelchair
 #define wheelchair
 
-#include "mbed.h"
-#include "math.h"
+#include "chair_imu.h"
 
 #define def (2.5f/3.3f)
 #define high 3.3f
@@ -17,6 +16,8 @@
 public:
     Wheelchair(PinName xPin, PinName yPin);
     void move(float x_coor, float y_coor);
+    void turn_right();
+    void turn_left();
     void forward();
     void backward();
     void right();
@@ -26,6 +27,7 @@
 private:
     PwmOut* x;
     PwmOut* y;
+    chair_imu* imu;
 
 };
 #endif
\ No newline at end of file