Diff: wheelchair.cpp

Revision:

26:c842070aa0b8

Parent:

21:3489cffad196

Child:

27:302d03ee1ae0

--- a/wheelchair.cpp	Fri Apr 19 23:06:53 2019 +0000
+++ b/wheelchair.cpp	Sat May 25 03:32:36 2019 +0000
@@ -37,18 +37,18 @@
 
 void Wheelchair::assistSafe_thread()
 {
-    int ToFV[12];
+   /* int ToFV[12];
     for(int i = 0; i < 9; 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)
     {
@@ -59,7 +59,19 @@
             forwardSafety = 1;
         }
     }
-    else
+    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)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            forwardSafety = 1;
+        }
+    }
+    else*/
         forwardSafety = 0;
     
 }
@@ -85,7 +97,7 @@
     
     for(int i = 0; i < 12; i++)                                               // initializes the ToF Sensors
     {
-        (*(ToF+i))->initReading(0x31+((0x02)*i), 50000);
+        //(*(ToF+i))->initReading(0x31+((0x02)*i), 50000);
     }
          
     out->printf("wheelchair setup done \r\n");                                          // Make sure it initialized; prints in serial monitor
@@ -374,14 +386,14 @@
 {
     /* Initializes variables as default */
     double temporV = def;
-    double temporS = def;
+    double temporS = def+offset;
     vDesiredS = 0;
     x->write(def);
-    y->write(def);
+    y->write(def+offset);
     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);                                           
@@ -393,7 +405,7 @@
  
     while(1)
     {
-        linearV = .7;
+        //linearV = smart.linearV;
         test1 = linearV*100;
         vel = curr_vel;
         vDesired = linearV*100;