Diff: main.cpp

Revision:

96:7465ab270e7a

Parent:

95:250afd53b710

diff -r 250afd53b710 -r 7465ab270e7a main.cpp
--- a/main.cpp	Sat Jun 08 12:00:38 2019 +0000
+++ b/main.cpp	Thu Jun 13 03:26:42 2019 +0000
@@ -12,6 +12,10 @@
 #define NUM_COLORS 6    //number of colors to store in the array
 #define NUM_STEPS 8    //number of steps between colors
 
+#define KP 0.45
+#define KI 0.0001
+#define KD 0.1
+
 #define PCF8574_ADDR 0x20
 #define beep_on  PCF8574Write(0xDF & PCF8574Read())
 #define beep_off PCF8574Write(0x20 | PCF8574Read())
@@ -24,10 +28,16 @@
 
 // Control motor drive
 Drive drive(D6, D5, A1, A0, A2, A3);
-int control_speed = 2500;
-int calibrate_speed = 4000;
+int control_speed = 3000;
+int calibrate_speed = 6000;
 int running_speed = 4000;
 
+//auto
+const int maximum = 1000;
+float multiples, multiples2;
+int cnt = 0;
+
+
 // TR Sensor
 TRSensors trs = TRSensors(D10, D11, D12, D13);
 uint16_t sensorValues[NUM_SENSOR];
@@ -40,6 +50,7 @@
 
 // Ultra sonic sensor
 Ultrasonic us(D3, D2, 0.1, false, 0.1);
+bool isClose = false;
 
 // OLED time print
 I2C i2c_for_oled(I2C_SDA,I2C_SCL);
@@ -97,7 +108,6 @@
         
         wait(0.1);
         drive.Break();
-        pc.printf("calibration!\r\n");
                 
         trs.calibrate();
     }
@@ -167,82 +177,142 @@
 }
 
 void check_obstacle_with_ultrasonic_sensor(void){
-    if(us.getDistance()< 10){ // Reduce magic number
-        beep_on;
-    } else {
-        beep_off;
+    while(true){
+        PCF8574Write(0xC0 | PCF8574Read());
+        int dist = us.getDistance();
+        if(dist < 30 && dist != -1){ // Reduce magic number
+//            beep_on;
+            isClose = true;
+        } else {
+//            beep_off;
+            isClose = false;
+        }
+//        myOled.clearDisplay();
+//        myOled.setTextCursor(0, 0);
+//        myOled.printf("dist: %d\r\n", dist);
+//        myOled.display();
+        wait(0.05);
     }
 }
 
 void start_auto_drive(){
     drive.setSpeed(running_speed);
-//    drive.Forward();
+//       drive.Forward();
+    Thread thread;
+    thread.start(check_obstacle_with_ultrasonic_sensor);
+    
     while(true){
-        check_irsensor();
-        check_obstacle_with_ultrasonic_sensor();
+        unsigned int position = trs.readLine(sensorValues);
+        int proportional = (int)position - 2000;
+        int derivative = proportional - last_proportional;
+        integral += proportional;
+//        last_proportional = proportional;
+    
+        int power_difference = proportional*KP + integral*KI + derivative*KD;
+        
+        // It need to have a specific rule
+        if( (sensorValues[0] >= 700) &&
+            (sensorValues[1] >= 700) && 
+            (sensorValues[2] >= 700) &&
+            (sensorValues[4] >= 700) )
+        {
+            drive.Break();
+            break;
+        }
         
-        unsigned int position = trs.readLine(sensorValues);
+        if(power_difference > maximum)
+            power_difference = maximum;
+        if(power_difference < -maximum)
+            power_difference = -maximum;
+            
+        int power_difference_n = power_difference * 2.3;
+        float left, right;
+        
+        // error 보정
+        if(power_difference_n < 0)
+        {
+            left = control_speed;
+            right = control_speed-power_difference_n;
+        }
+        else
+        {
+            left = control_speed+power_difference_n;
+            right = control_speed;
+        }
         
-//        // for pc debug
-//        for(int i=0; i<5; i++){
-//            pc.printf("%d\t", sensorValues[i]);
-//        }
-//        pc.printf("\r\n");
-//        
+        if(position < 900) {
+            multiples2 = 0.5f;
+            left = 0;
+            cnt = 0;
+        } else if(position < 1300) {
+            multiples2 = 0.5f;
+            left = 0;
+            cnt = 0;
+        } else if(position < 1550) {
+            multiples2 = 0.5f;
+            left = 0;
+            cnt = 0;
+        } else if(position < 2000) {
+            if(cnt < 5) multiples2 = 0.5f;
+            else multiples2 = 1.0f;
+            cnt++;
+        } else if(position < 2350) {
+            if(cnt < 5) multiples2 = 0.5f;
+            else multiples2 = 1.0f;
+            cnt++;
+        } else if(position < 2500) {
+            multiples2 = 0.5f;
+            right = 0;
+            cnt = 0;
+        } else if(position < 3100) {
+            multiples2 = 0.5f;
+            right = 0;
+            cnt = 0;
+        } else {
+            multiples2 = 0.5f;
+            right = 0;
+            cnt = 0;
+        }
+        
+        if(isClose) {
+            multiples = 1.2f * multiples2;
+            if((sensorValues[0] >= 500) || (sensorValues[1] >= 500)){
+                do{
+                    drive.setSpeed(0, 5000);
+                    drive.Forward();
+                    wait(0.01);
+                    drive.Break();
+                    position = trs.readLine(sensorValues);
+                } while(sensorValues[2] >= 700);
+                continue;
+            }
+        } else {
+            multiples = 2.4f * multiples2;
+        }
+        
+        last_proportional = proportional;
+        
+        left *= multiples;
+        right *= multiples;
+        
+        drive.setSpeed((int)left, (int)right);
+        drive.Forward(); 
+
+        wait(0.005);
+        drive.Break();
+        
+//        myOled.clearDisplay();
+//        myOled.setTextCursor(0, 0);
 //        // for oled debug
 //        for(int i=0; i<5; i++){
 //            myOled.printf("%d\t", sensorValues[i]);
 //            myOled.display();
 //        }
-//        myOled.printf("\r");
+//        
+//        myOled.printf("\r\np:%d\r\npo:%d\r\n", position, power_difference);
 //        myOled.display();
-        
-        // It need to have a specific rule
-        if( (sensorValues[0] >= 800) &&
-            (sensorValues[1] >= 800) && 
-            (sensorValues[2] >= 800) &&
-            (sensorValues[3] >= 800) &&
-            (sensorValues[4] >= 800) )
-        {
-            drive.Break();
-            break;
-        }
-        
-        if(sensorValues[0] >= trs.calibratedMax[0]-15 && sensorValues[0] <= trs.calibratedMax[0]+15)
-        {
-            drive.setSpeed(running_speed-2800, running_speed);
-//            drive.Forward();
-        }
-        
-        else if(sensorValues[1] >= trs.calibratedMax[1]-15 && sensorValues[1] <= trs.calibratedMax[1]+15)
-        {
-            drive.setSpeed(running_speed-700, running_speed);
-//            drive.Forward();
-        }
-        
-        else if(sensorValues[2] >= trs.calibratedMax[2]-15 && sensorValues[2] <= trs.calibratedMax[2]+15)
-        {
-            drive.setSpeed(running_speed, running_speed);
-//            drive.Forward();
-        }
-        
-        else if(sensorValues[3] >= trs.calibratedMax[3]-15 && sensorValues[3] <= trs.calibratedMax[3]+15)
-        {
-            drive.setSpeed(running_speed, running_speed-700);
-//            drive.Forward();
-        }
-        
-        else if(sensorValues[4] >= trs.calibratedMax[4]-15 && sensorValues[4] <= trs.calibratedMax[4]+15)
-        {
-            drive.setSpeed(running_speed, running_speed-2800);
-//            drive.Forward();
-        }
-        else {
-//            drive.Forward();
-        }
-        
-        wait(0.1);
-        drive.Break();
+//        myOled.printf("\r\nleft:%d\r\nright:%d\r\n", (int)left, (int)right);
+//        myOled.display();
     }
 }
 
@@ -260,7 +330,7 @@
 
 
 void bottom_led(void){  //led 제어 
-     int colorIdx = 0;
+    int colorIdx = 0;
     int colorTo = 0;
     int colorFrom = 0;
     
@@ -275,7 +345,7 @@
     int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
     
     // Now the buffer is written, write it to the led array.
-    while (1) 
+    while (true) 
     {
         //get starting RGB components for interpolation
         std::size_t c1 = colorbuf[colorFrom];
@@ -321,9 +391,12 @@
 }
 
 int main(){
-    //program setup
+    //program setupa
     myOled.begin();
-    i2c_for_irsensor.frequency(100000);    // main에 써야함
+    i2c_for_irsensor.frequency(100000);    // main에 써야함  
+    PCF8574Initialize();
+    beep_off;
+    PCF8574Write(0xC0 | PCF8574Read());
     
     // Test OLED
     myOled.printf("oled size: %ux%u \r\nStart Alphabot2\r\n", myOled.width(), myOled.height());
@@ -368,8 +441,12 @@
     myOled.printf("\nGoal! \r\nThe time taken was\r\n%f seconds\r\n", t.read());
     myOled.display();
     /* LED run*/ 
-    bottom_led();
+    Thread thread;
+    thread.start(bottom_led);
+//    bottom_led();
     
-    while(true){}
-}
-
+    while(true){
+        beep_off;
+        myOled.display();    
+    }
+}
\ No newline at end of file