Revision 6:0cd57bdd8fbc, committed 2018-07-20

Comitter:

ryanlin97

Date:

Fri Jul 20 17:54:43 2018 +0000

Parent:

5:e0ccaab3959a

Child:

7:5e38d43fbce3

Commit message:

using mpu9250 imu

Changed in this revision

--- a/BNO055.lib	Tue Jul 17 07:19:04 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://os.mbed.com/users/StressedDave/code/BNO055/#1f722ffec323

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_BNO055.lib	Fri Jul 20 17:54:43 2018 +0000
@@ -0,0 +1,1 @@
+chair_BNO055#ad7a811c859f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_MPU9250.lib	Fri Jul 20 17:54:43 2018 +0000
@@ -0,0 +1,1 @@
+chair_MPU9250#fd1a49d15f7f

--- a/chair_imu.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,121 +0,0 @@
-#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;
-}
-
-

--- a/chair_imu.h	Tue Jul 17 07:19:04 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#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/main.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/main.cpp	Fri Jul 20 17:54:43 2018 +0000
@@ -8,72 +8,80 @@
 DigitalOut up(D2);
 DigitalOut down(D3);
 
-Wheelchair smart(xDir,yDir);
+Serial pc(USBTX, USBRX);
+
+Wheelchair smart(xDir,yDir, &pc);
 
 int main(void)
 {
     on = 1;
-    while(1){
- /*   if( pc.readable()) {
-        char c = pc.getc();
-        if( c == 'w') {
-            pc.printf("up \n");
-            smart.forward(); 
-            }
-        
-        else if( c == 'd') {
-            pc.printf("left \n");
-            smart.left();
-            }
-        
-        else if( c == 'a') {
-            pc.printf("right \n");
-            smart.right();
-            }
-        
-        else if( c == 's') {
-            pc.printf("down \n");
-            smart.backward();
+    while(1) {
+        /*   if( pc.readable()) {
+               char c = pc.getc();
+               if( c == 'w') {
+                   pc.printf("up \n");
+                   smart.forward();
+                   }
+
+               else if( c == 'd') {
+                   pc.printf("left \n");
+                   smart.left();
+                   }
+
+               else if( c == 'a') {
+                   pc.printf("right \n");
+                   smart.right();
+                   }
+
+               else if( c == 's') {
+                   pc.printf("down \n");
+                   smart.backward();
+                   }
+
+               else {
+                   pc.printf("none \n");
+                   smart.stop();
+                   if( c == 'o') {
+                      pc.printf("turning on");
+                      on = 0;
+                      wait(process);
+                      on = 1;
+                       }
+
+                   else if( c == 'k') {
+                       off = 0;
+                       wait(process);
+                       off = 1;
+                       }
+
+                   else if( c == 'u') {
+                       up = 0;
+                       wait(process);
+                       up = 1;
+                       }
+
+                   else if( c == 'p') {
+                       down = 0;
+                       wait(process);
+                       down = 1;
+                       }
+                   }
+                  }
+
+           else {
+              pc.printf("Nothing pressed \n");
+              smart.stop();
+                   }
+          */
+        smart.move(x,y);
+        if( pc.readable()) {
+            char c = pc.getc();
+            if( c == 'r') {
+                smart.turn_right(pc);
             }
-        
-        else {
-            pc.printf("none \n");
-            smart.stop();
-            if( c == 'o') {
-               pc.printf("turning on");
-               on = 0;
-               wait(process);
-               on = 1; 
-                }
-            
-            else if( c == 'k') {
-                off = 0;
-                wait(process);
-                off = 1;
-                }
-        
-            else if( c == 'u') {
-                up = 0;
-                wait(process);
-                up = 1;
-                }
-        
-            else if( c == 'p') {
-                down = 0;
-                wait(process);
-                down = 1;
-                }
-            }
-           }
-           
-    else {
-       pc.printf("Nothing pressed \n");
-       smart.stop();
-            }
-   */
-    smart.move(x,y); 
-    wait(process);
-       }
+        }
+        wait(process);
+    }
 
 }
 

--- a/wheelchair.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/wheelchair.cpp	Fri Jul 20 17:54:43 2018 +0000
@@ -1,44 +1,25 @@
 #include "wheelchair.h"
 
-Wheelchair::Wheelchair(PinName xPin, PinName yPin)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc)
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
-    imu = new chair_imu();
+    //imu = new chair_BNO055();
+    imu = new chair_MPU9250(pc);
+    
 }
+
 /*
 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
 */
 void Wheelchair::move(float x_coor, float y_coor)
 {
-     
+
     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);
     y->write(scaled_y);
-    
-}
 
-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()
 {
@@ -68,4 +49,35 @@
 {
     x->write(def);
     y->write(def);
-}
\ No newline at end of file
+}
+
+void Wheelchair::turn_right(Serial out)
+{
+    bool stop = false;
+    double start = imu->yaw();
+    double final = start + 90;
+    if(final > 360)
+        final -= 360;
+
+    while((imu->yaw() <= final)&& (stop == false)) {
+        Wheelchair::right();
+        out.printf("turning right");
+        if( out.readable()) {
+            out.printf("stopped\n");
+            Wheelchair::stop();
+            return;
+        }
+    }
+}
+
+void Wheelchair::turn_left()
+{
+    double start = imu->yaw();
+    double final = start - 90;
+    if(final <0)
+        final += 360;
+
+    while(imu->yaw() >= final) {
+        Wheelchair::left();
+    }
+}

--- a/wheelchair.h	Tue Jul 17 07:19:04 2018 +0000
+++ b/wheelchair.h	Fri Jul 20 17:54:43 2018 +0000
@@ -1,22 +1,23 @@
 #ifndef wheelchair
 #define wheelchair
 
-#include "chair_imu.h"
+//#include "chair_BNO055.h"
+#include "chair_MPU9250.h"
 
 #define def (2.5f/3.3f)
 #define high 3.3f
 #define offset .02f
 #define low (1.7f/3.3f)
-#define process .1
+#define process .05
 #define xDir D12 //top right two pins
 #define yDir D13 //top left two pins
 
 class Wheelchair
 {
 public:
-    Wheelchair(PinName xPin, PinName yPin);
+    Wheelchair(PinName xPin, PinName yPin, Serial* pc);
     void move(float x_coor, float y_coor);
-    void turn_right();
+    void turn_right(Serial);
     void turn_left();
     void forward();
     void backward();
@@ -27,7 +28,8 @@
 private:
     PwmOut* x;
     PwmOut* y;
-    chair_imu* imu;
+    //chair_BNO055* imu;
+    chair_MPU9250* imu;
 
 };
 #endif
\ No newline at end of file