Diff: main.cpp

Revision:

0:500d22d1efeb

Child:

1:5c26d3744592

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 15 06:19:52 2018 +0000
@@ -0,0 +1,205 @@
+#include "mbed.h"
+
+#define KP 5.8  //Pゲイン
+#define CHG 1.0 //change_gray用の係数
+#define WHITE_CL 0.88   //初期値
+#define WHITE_CR 0.88    //初期値
+#define GRAY_CL 0.65    //初期値
+#define GRAY_CR 0.65     //初期値
+#define GRAY_L 0.26     //初期値
+#define GRAY_R 0.25     //初期値
+
+#define DEBUG
+
+DigitalOut ledL( PTB8);
+DigitalOut ledR( PTE5);
+DigitalOut ledC( PTE2); //動作check
+BusOut ledLL( PTB8, PTB9);
+BusOut ledRR( PTE4, PTE5);
+AnalogIn sensorR( PTB1);
+AnalogIn sensorL( PTB3);
+AnalogIn sensorCR( PTB0);
+AnalogIn sensorCL( PTB2);
+//モータ1
+BusOut Mlefti(PTA1, PTA2);
+PwmOut Mleftp(PTD4);
+//モータ2
+BusOut Mrighti(PTC0, PTC7);
+PwmOut Mrightp(PTA12);
+
+float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
+float grayL[2], grayR[2];
+float blackCL = 0.12;   //閾値の跡地
+float blackCR = 0.3;   //↑と同じ
+float whiteL = 0.27, blackL = 0.22;
+float whiteR = 0.28, blackR = 0.22;
+float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
+float pr, pl;
+
+void stop_point_ver2();
+
+void set_threshold(){   //閾値の初期設定
+    for( int i = 0; i < 2; i++){
+        whiteCL[i] = WHITE_CL;
+        whiteCR[i] = WHITE_CR;
+        grayCL[i] = GRAY_CL;
+        grayCR[i] = GRAY_CR;
+        grayL[i] = GRAY_L;
+        grayR[i] = GRAY_R;
+    }
+}
+
+void go_straight_p(){   //P制御でトレース
+    Mrighti = 1;
+    Mrightp = (KP * pr) * 1.0f;
+    Mlefti = 1;
+    Mleftp = (KP * pl) * 1.0f;
+    ledR = 1;
+    ledL = 1;
+}
+
+void go_straight_CR(){  //CRのみでトレース
+    Mrighti = 1;
+    Mrightp = (KP * pr + 0.3) * 1.0f;
+    Mlefti = 1;
+    Mleftp = (1.3 - KP * pr) * 1.0f;
+    ledRR = 0b11;
+    ledLL = 0b10;
+}
+
+void turn_right(){
+    Mrighti = 2;
+    Mrightp = 0.05f;
+    Mlefti = 1;
+    Mleftp = 0.1f;
+    ledR = 1;
+    ledL = 0;
+}
+
+void turn_left(){
+    Mrighti = 1;
+    Mrightp = 0.1f;
+    Mlefti = 2;
+    Mleftp = 0.05f;
+    ledR = 0;
+    ledL = 1;
+}
+
+void turn_right_ver2(){
+    while( sensor[0] > grayL[0]){   //sensorL > grayL[0] 
+        sensor[0] = sensorL.read();
+        turn_right();
+    }
+    stop_point_ver2();
+    while( sensor[2] > grayCR[0]){  //補正
+        sensor[2] = sensorCR.read();
+        turn_left();
+    }
+    stop_point_ver2();
+}
+
+void turn_left_ver2(){
+    while( sensor[3] > grayR[0]){   //sensorR > grayR[0]
+        sensor[3] = sensorR.read();
+        turn_left();
+    }
+    stop_point_ver2();
+    while( sensor[1] > grayCL[0]){  //補正
+        sensor[1] = sensorCL.read();
+        turn_right();
+    }
+    stop_point_ver2();
+}
+
+void motor_check(){   //モータドライバの調子の確認用
+    Mrighti = 1;
+    Mrightp = 1.0f;
+    Mlefti = 1;
+    Mleftp = 1.0f;
+}
+
+void stop_point(){
+    Mrighti = 2;
+    Mrightp = 1.0f;
+    Mlefti = 2;
+    Mleftp = 1.0f;
+    wait(0.05);
+    Mrighti = 0;
+    Mlefti = 0;
+    ledRR = 0b11;
+    ledLL = 0b11;
+}
+
+void stop_point_ver2(){
+    if( Mrighti == 1)
+        Mrighti = 2;
+    else if( Mrighti == 2)
+        Mrighti = 1;
+    Mrightp = 1.0f;
+    if( Mlefti == 1)
+        Mlefti = 2;
+    else if( Mlefti == 2)
+        Mlefti = 1;
+    Mleftp = 1.0f;
+    wait(0.05);
+    Mrighti = 0;
+    Mlefti = 0;
+    ledRR = 0b11;
+    ledLL = 0b11;
+}
+
+void change_gray(){
+    if( sensor[1] <= grayCL[0]){    //sensorCL <=grayCL
+        whiteCR[0] = sensorCR.read();
+        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
+        grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
+        grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
+        ledRR = 0b10;
+        ledLL = 0;
+    }
+    else if( sensor[2] <= grayCR[0]){   //sensorCR <= grayCR
+        whiteCL[0] = sensorCL.read();
+        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
+        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
+        grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
+        grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
+        ledRR = 0;
+        ledLL = 0b10;
+    }
+}
+
+int main() {
+    ledC = 1;
+    set_threshold();
+    motor_check();
+    wait(0.5);
+    while(1) {
+        sensor[0] = sensorL.read();
+        sensor[1] = sensorCL.read();
+        sensor[2] = sensorCR.read();
+        sensor[3] = sensorR.read();
+
+        pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
+        pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
+        
+        if( sensor[0] <= grayL[0]){
+            turn_left_ver2();
+            change_gray();
+            #ifdef DEBUG
+                break;
+            #endif
+        } else if( sensor[3] <= grayR[0]){
+            turn_right_ver2();
+            change_gray();
+            #ifdef DEBUG
+                break;
+            #endif
+        } else
+            go_straight_p();
+        if( sensor[0] < blackL && sensor[3] < blackR){
+            stop_point_ver2();
+            break;
+        }
+    }
+}
\ No newline at end of file