Diff: main.cpp

Revision:

105:bbded1accbb9

Parent:

104:94ed54c74466

--- a/main.cpp	Sat Jun 08 17:25:05 2019 +0000
+++ b/main.cpp	Mon Jun 10 04:58:13 2019 +0000
@@ -171,11 +171,11 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    motor.kp = 10.0;
+    motor.kp = 0.1;
     motor.kd = 1.0;
     motor.ki = 5000;
-    motor.max = 250;
-    
+    motor.max = 180;
+    motor.standard = 800.0;
  
     DataComm = 0;
  
@@ -250,8 +250,8 @@
     while(1){
         myOled.clearDisplay();
         //myOled.printf("%.2f\r", motor.kd);
-        myOled.printf("time: %u:%u.%u\r", min_t, sec_t, msec_t);
-//        myOled.printf("%.2f %.2f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
+//        myOled.printf("time: %u:%u.%u\r", min_t, sec_t, msec_t);
+        myOled.printf("%.3f %.3f %d %.0f\r", motor.kp, motor.kd, motor.max, motor.standard);
         myOled.display();
     }
 }
@@ -361,18 +361,18 @@
    }
    
    // finish line
-   if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
-        count_stop ++;
-        if(count_stop > 7){
-            timer_flag = 1;
-            motor.stop();
-        
-            bottom_led();     
-        }  
-    }
-    else{
-        count_stop = 0;
-    }
+   //if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
+//        count_stop ++;
+//        if(count_stop > 7){
+//            timer_flag = 1;
+//            motor.stop();
+//        
+//            bottom_led();     
+//        }  
+//    }
+//    else{
+//        count_stop = 0;
+//    }
 
 }
  
@@ -417,16 +417,14 @@
     
     int proportional = pos - 2000;
     
-    int derivative = proportional - last_proportional;
+    int derivative = proportional - last_proportional; 
 
     integral += proportional;
     
     last_proportional  = proportional;
  
-    int power_difference = proportional / motor.kp + derivative * motor.kd;
-                                                                                                                                                                                                                                                                                                                                                                                                                         
-    const float std = 1000.0;
-    
+    int power_difference = proportional * motor.kp + derivative * motor.kd;
+                                                                                                                                                                                                                                                                                                                                                                                                                             
     
     if(power_difference > motor.max)
         power_difference = motor.max;
@@ -435,7 +433,7 @@
         power_difference = -motor.max;
     
         
-    motor.speed_r((motor.max + power_difference)/std);
-    motor.speed_l((motor.max - power_difference)/std); 
+    motor.speed_r((motor.max + power_difference)/motor.standard);
+    motor.speed_l((motor.max - power_difference)/motor.standard); 
 
 }
\ No newline at end of file