Diff: wheelchair.cpp

Revision:

5:e0ccaab3959a

Parent:

4:29a27953fe70

Child:

6:0cd57bdd8fbc

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