Diff: main.cpp

Revision:

10:cea77ea5af93

Parent:

8:1c1c72c69af4

Child:

11:5ae0c22d5473

--- a/main.cpp	Wed Jan 15 02:31:36 2020 +0000
+++ b/main.cpp	Thu Jan 16 05:13:02 2020 +0000
@@ -22,12 +22,12 @@
 Thread thread_motor;
 Thread sensor_thread;
 
-const float INTVAL_REFLECTOR(0.1);//0.01);
+const float INTVAL_REFLECTOR(0.01);//0.01);
 const float INTVAL_MOTOR(0.25);
 
 const float pi = 3.14159265359;
 const float gear_ratio = 1 / 38.2;
-const float tire_size = 0.57 / 2;
+const float tire_size = 0.057 / 2;
 
 float dist = 0.0;
 float x = 0.0;
@@ -55,7 +55,7 @@
     //const float slow_rpm = 0;
     static float gap_pre = 0.0;
     const float KP = 850;
-    const float KD = 3000;
+    const float KD = 0;
     const float K  = 0.01;
     const int   L = 0;
     const int   R = 1;
@@ -69,41 +69,43 @@
     float current_azimuth = 0.0;
     float curvature = 0.0;
     float tread = 0.097;
+    
+    fast_rpm = 1200;
+    standard_rpm = 1200;
+    slow_rpm = 1200;
 
     while (1)
     {
-        /*
-        if (reflectorFL>0.15)
+        /*if(reflectorFL > 0.1)
         {
-            flag=L;
-        }
-        if (reflectorFR>0.15)
-        {
-            flag=R;
+            flag = L;
         }
-        
-        if ( reflectorFM<=0.4 && reflectorFL<=0.15 && reflectorFR<=0.15 )
+        if(reflectorFR > 0.1)
         {
-            if (flag==L)
+            flag = R;
+        }*/
+        if( reflectorFM<=0.4 && reflectorBL<=0.1 && reflectorBR<=0.1 )//&& reflectorFL<=0.1 && reflectorFR<=0.1 )
+        {      
+            if( flag == L )
             {
-                motorL.Set_phase(L);
-                motorL.Set_target(0);
-                motorR.Set_phase(R);
-                motorR.Set_target(slow_rpm);
-                printf("turnL\n");
+               motorL.Set_phase(L);
+               motorL.Set_target(0);
+               motorR.Set_phase(R);
+               motorR.Set_target(slow_rpm);
+               //printf("turnL\n");
             }
             else
             {
-                motorL.Set_phase(L);
-                motorL.Set_target(slow_rpm);
-                motorR.Set_phase(R);
-                motorR.Set_target(0);
-                printf("turnR\n");
-            }
+               motorL.Set_phase(L);
+               motorL.Set_target(slow_rpm);
+               motorR.Set_phase(R);
+               motorR.Set_target(0);
+               //printf("turnR\n");
+            }  
         }
         else
         {
-            if ( reflectorFM>0.4 )
+            if( reflectorFM > 0.4 )
             {
                 motorL.Set_phase(L);
                 motorL.Set_target(fast_rpm);
@@ -112,23 +114,30 @@
             }
             else
             {
-                if ( reflectorBL>0.15 )
+                if( reflectorBL.read() >= reflectorBR.read() )
                 {
+                    float voltage_gap = reflectorBL.read() - reflectorBR.read();
+                    motorR.Set_phase(R);
+                    motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
                     motorL.Set_phase(L);
-                    motorL.Set_target(standard_rpm - standard_rpm*0.18);
-                    motorR.Set_phase(R);
-                    motorR.Set_target(standard_rpm + standard_rpm*0.18);
+                    motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
+                    gap_pre = voltage_gap;
+                    flag = L;
                 }
                 else
                 {
-                    motorL.Set_phase(L);
-                    motorL.Set_target(standard_rpm + standard_rpm*0.18);
+                    float voltage_gap = reflectorBR.read() - reflectorBL.read();
                     motorR.Set_phase(R);
-                    motorR.Set_target(standard_rpm - standard_rpm*0.18);
+                    motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
+                    motorL.Set_phase(L); 
+                    motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
+                    gap_pre = voltage_gap;
+                    flag = R;
                 }
             }
-        }*/
+        }
         
+        /*
         float gap_min = 0.0;
         int i_min = 0;
         for (int i = 0; i < 478; i++)
@@ -147,6 +156,7 @@
         
         motorL.Set_target( standard_rpm + drpm );
         motorR.Set_target( standard_rpm - drpm );
+        */
         
         printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
         wait(INTVAL_REFLECTOR);
@@ -174,11 +184,6 @@
     x_dot_pre = x;
     y_dot_pre = y;
 
-    fast_rpm = 1200;
-    standard_rpm = 1200;
-    slow_rpm = 1200;
-
-    /*
     if     (dist<=2000)
     {
         fast_rpm     = 2500;
@@ -202,7 +207,7 @@
         fast_rpm     = 1500;
         standard_rpm = 1200;
         slow_rpm     = 1200;
-    }*/
+    }
 
     //printf("%f\n", dist);
 
@@ -219,7 +224,7 @@
     {
         motorL.drive();
         motorR.drive();
-        odometry();
+        //odometry();
         wait(INTVAL_MOTOR);
     }
 }