Wheelchair control

Dependencies:   BNO055 mbed

Fork of wheelchaircontrol by ryan lin

Files at this revision

API Documentation at this revision

Comitter:
cpbenite
Date:
Tue Jul 17 19:19:26 2018 +0000
Parent:
5:e0ccaab3959a
Commit message:
Wheelchair control

Changed in this revision

chair_imu.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
wheelchair.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/chair_imu.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/chair_imu.cpp	Tue Jul 17 19:19:26 2018 +0000
@@ -1,121 +1,133 @@
 #include "chair_imu.h"
-
+ 
 Serial pc(USBTX, USBRX);
 Timer t;
 
+// Default Initialization
 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;
+    imu = new BNO055(SDA, SCL);
 }
 
-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;
+// Initialize Pins 
+chair_imu::chair_imu(PinName SDA_Pin, PinName SCL_Pin){
+    imu = new BNO055(SDA_Pin, SCL_Pin);
 }
+ 
+// Check to make sure BNO055 is properly working
+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");
 
-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;
+    //Set the SI units & start the timer
+    imu->set_accel_units(MPERSPERS);
+    imu->setmode(OPERATION_MODE_AMG);
+    imu->set_angle_units(DEGREES);
+    imu->get_calib();
+    imu->write_calibration_data();
+    imu->setmode(OPERATION_MODE_AMG); //put into while loop 
+    t.start();
+}
+ 
+// Receive acceleration data (in meters per second)
+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::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::accel_z(){
+    imu->get_accel();
+    return (double)imu->accel.z;
 }
-
-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;
+ 
+ 
+// Receive Gyroscope data (in degrees)
+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;
 }
 
 
+// Receive angle relative to North
+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;
+}
+ 
+// Rotation about the X-axis
+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;         // Scaling factor   
+
+    t.reset();
+ 
+    return (double)roll;
+}
+
+// Rotation about the Y-axis
+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;       // Scaling Factor
+
+    t.reset();
+
+    return (double)pitch;
+}
+ 
+// Rotation about the Z-axis
+double chair_imu::yaw(){
+    imu->get_gyro();
+    float yaw = (yaw - t.read() * imu->gyro.z);
+
+    // Return a yaw value between 0 and 360 degrees
+    if(yaw > 360)
+    yaw -= 360;
+    if(yaw < 0)
+    yaw += 360;
+
+    t.reset();
+
+    return (double)yaw;
+}
\ No newline at end of file
--- a/main.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/main.cpp	Tue Jul 17 19:19:26 2018 +0000
@@ -1,79 +1,92 @@
 #include "wheelchair.h"
 
+// Initialize Pins
 AnalogIn x(A0);
 AnalogIn y(A1);
-
+ 
 DigitalOut off(D0);
 DigitalOut on(D1);
 DigitalOut up(D2);
 DigitalOut down(D3);
-
+ 
 Wheelchair smart(xDir,yDir);
 
 int main(void)
 {
     on = 1;
     while(1){
- /*   if( pc.readable()) {
-        char c = pc.getc();
-        if( c == 'w') {
-            pc.printf("up \n");
-            smart.forward(); 
+    /*  
+        // Waiting for command
+        if (pc.readable()) {
+            char c = pc.getc();
+            
+            // Move Forward
+            if (c == 'w') {
+                pc.printf("up \n");
+                smart.forward(); 
             }
         
-        else if( c == 'd') {
-            pc.printf("left \n");
-            smart.left();
+            // Move Left
+            else if (c == 'd') {
+                pc.printf("left \n");
+                smart.left();
+            }
+        
+            // Move Right
+            else if (c == 'a') {
+                pc.printf("right \n");
+                smart.right();
             }
         
-        else if( c == 'a') {
-            pc.printf("right \n");
-            smart.right();
-            }
-        
-        else if( c == 's') {
-            pc.printf("down \n");
-            smart.backward();
+            // Move Backward
+            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; 
+            
+            // Read non-move commands
+            else {
+                pc.printf("none \n");
+                smart.stop();
+                
+                // Turn On
+                if (c == 'o') {
+                   pc.printf("Turning on.\n");
+                   on = 0;
+                   wait(process);
+                   on = 1; 
+                }
+                
+                // Turn Off
+                else if (c == 'k') {
+                    off = 0;
+                    wait(process);
+                    off = 1;
+                }
+                
+                
+                else if (c == 'u') {
+                    up = 0;
+                    wait(process);
+                    up = 1;
                 }
             
-            else if( c == 'k') {
-                off = 0;
-                wait(process);
-                off = 1;
+                else if (c == 'p') {
+                    down = 0;
+                    wait(process);
+                    down = 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);
-       }
-
+        else {
+            pc.printf("Nothing pressed.\n");
+            smart.stop();
+        }
+        */
+        
+        smart.move(x,y); 
+        wait(process);
+    }
 }
 
--- a/wheelchair.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/wheelchair.cpp	Tue Jul 17 19:19:26 2018 +0000
@@ -1,24 +1,30 @@
 #include "wheelchair.h"
 
-Wheelchair::Wheelchair(PinName xPin, PinName yPin)
+// Note: 'def' is short for 'Default Position'
+
+// Initialize Wheelchair Pins
+Wheelchair::Wheelchair (PinName xPin, PinName yPin)
 {
     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
+*   Joystick has AnalogOut of 200-700, scale values between 1.3 and 3.3
 */
+
+// Convert Joystick Values to Move the Wheelchair
 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;
+   
+    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);
-    
+    y->write(scaled_y); 
 }
 
+// Turn 90 degrees to the Right
 void Wheelchair::turn_right(){
     double start = imu->yaw();
     double final = start + 90;
@@ -30,6 +36,7 @@
         } 
 }
 
+// Turn 90 degrees to the Left
 void Wheelchair::turn_left(){
     double start = imu->yaw();
     double final = start - 90;
@@ -40,30 +47,31 @@
         Wheelchair::left();
         } 
 }
+
 void Wheelchair::forward()
 {
     x->write(high);
     y->write(def+offset);
 }
-
+ 
 void Wheelchair::backward()
 {
     x->write(low);
     y->write(def);
 }
-
+ 
 void Wheelchair::right()
 {
     x->write(def);
     y->write(high);
 }
-
+ 
 void Wheelchair::left()
 {
     x->write(def);
     y->write(low);
 }
-
+ 
 void Wheelchair::stop()
 {
     x->write(def);