Diff: wheelchair.cpp

Revision:

26:662693bd7f31

Parent:

21:3489cffad196

Child:

27:da718b990837

--- a/wheelchair.cpp	Fri Apr 19 23:06:53 2019 +0000
+++ b/wheelchair.cpp	Thu Jun 27 16:32:12 2019 +0000
@@ -38,17 +38,29 @@
 void Wheelchair::assistSafe_thread()
 {
     int ToFV[12];
-    for(int i = 0; i < 9; i++)                                              // reads from the ToF Sensors
+    for(int i = 0; i < 6; i++)                                              // reads from the ToF Sensors
     {
         ToFV[i] = (*(ToF+i))->readFromOneSensor();
         //out->printf("%d ", ToFV[i]);
     }        
-        out->printf("\r\n");
-    int sensor1 = ToFV[2];
+        //out->printf("\r\n");
+    int sensor1 = ToFV[1];
     int sensor4 = ToFV[5];
-    out->printf("%d, %d\r\n", sensor1, sensor4);
-    if(((2 * maxDeceleration*sensor1 < curr_vel*curr_vel*1000*1000 || 
-    2 * maxDeceleration*sensor4 < curr_vel*curr_vel*1000*1000) && 
+    //out->printf("%d, %d\r\n", sensor1, sensor4);
+    /*if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && 
+    (sensor1 < 1500 || sensor4 < 1500)) ||
+    550 > sensor1 || 550 > sensor4)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            forwardSafety = 1;
+        }
+    }
+    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && 
     (sensor1 < 1500 || sensor4 < 1500)) ||
     550 > sensor1 || 550 > sensor4)
     {
@@ -61,7 +73,8 @@
     }
     else
         forwardSafety = 0;
-    
+    }
+    */
 }
 
 /* Constructor for Wheelchair class */
@@ -75,10 +88,11 @@
     x = new PwmOut(xPin);                                                               
     y = new PwmOut(yPin);
     /* Initializes IMU Library */
+    out = pc;                                                                           // "out" is called for serial monitor
+    out->printf("on\r\n");
     imu = new chair_BNO055(pc, time);
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
-    out = pc;                                                                           // "out" is called for serial monitor
     wheelS = qeiS;                                                                      // "wheel" is called for encoder
     wheel = qei;   
     ToF = ToFT;                                                                         // passes pointer with addresses of ToF sensors
@@ -110,11 +124,13 @@
 /* Automatic mode: move forward and update x,y coordinate sent to chair */
 void Wheelchair::forward()                                                              
 {
+    //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS);
     if(forwardSafety == 0)
     {
     x->write(high);
     y->write(def+offset);
     }
+    out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975));
 }
  
 /* Automatic mode: move in reverse and update x,y coordinate sent to chair */
@@ -374,14 +390,14 @@
 {
     /* Initializes variables as default */
     double temporV = def;
-    double temporS = def;
+    double temporS = def+offset;
     vDesiredS = 0;
     x->write(def);
     y->write(def);
     wheel->reset();                                                                     // Resets the encoders
     /* Sets the constants for P and D */
     PIDVelosity.SetTunings(.0005,0, 0.00);                                             
-    PIDSlaveV.SetTunings(.01,0.000001, 0.000001);     
+    PIDSlaveV.SetTunings(.005,0.000001, 0.000001);     
  
     /* Limits to the range specified */
     PIDVelosity.SetOutputLimits(-.005, .005);