Revision 6:8cd00c26bb47, committed 2018-07-17

Comitter:

cpbenite

Date:

Tue Jul 17 19:19:26 2018 +0000

Parent:

5:e0ccaab3959a

Commit message:

Wheelchair control

Changed in this revision

--- 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);