Diff: main.cpp

Revision:

8:0318c56aae15

Parent:

7:49b71bc18049

diff -r 49b71bc18049 -r 0318c56aae15 main.cpp
--- a/main.cpp	Thu Oct 17 09:10:11 2019 +0000
+++ b/main.cpp	Wed Mar 11 05:40:51 2020 +0000
@@ -1,26 +1,426 @@
-//  たぶん一番簡単な時間管理プログラム - OS5
-//  2019.10.17 ... Y.Kuroda
 #include "mbed.h"
-Thread thread;
-DigitalOut p1(LED1);
+#include "math.h"
+
+InterruptIn encR(D12);
+InterruptIn encL(D11);
+DigitalOut led(LED1);
+PwmOut motL_a(D6);
+PwmOut motL_b(D9);
+PwmOut motR_a(D3);
+PwmOut motR_b(D5);
+AnalogIn v_R(A0);
+AnalogIn v_L(A2);
+AnalogIn v_C(A1);
+Thread thread_1;
+Thread thread_2;
+
 #define CYC_FLAG 1
 
-void my_thread()       // スレッドにする関数．正確な時間間隔で起こされる
-{
-    while (true) {
-        ThisThread::flags_wait_all(CYC_FLAG);   // 起こされるまで寝ている
-        //
-        p1 = !p1;                   // ユーザコードはここに書く
-        //
+float counter_L = 0.0f;
+float counter_R = 0.0f;
+float LED_L = 0.0f;
+float LED_R = 0.0f;
+float tire_count_L = 0.0f;
+float tire_count_R = 0.0f;
+float LED_tire_R = 0.0f;
+float LED_tire_L = 0.0f;
+float LED_tire = 0.0f;
+float PI = 3.1416;
+float dis_L = 0.0f;
+float dis_R = 0.0f;
+float delta_theta = 0.0f;
+float dis = 0.0f;
+float r = 0.0f;
+float velocity_L = 0.0f;
+float velocity_R = 0.0f;
+float velocity = 0.0f;
+float omega = 0.0f;
+float diff[2] = {0.0f, 0.0f};
+float d1 = 0.13f;
+float d2 = 0.23f;
+float b = 1.4f;
+float b1 = 0.6f;
+float w = 0.3f;
+float g1 = 0.4f;
+float g_CL2 = 1.2f;
+float Kp = 3.23f;
+float Ki = 0.05f; //0.05
+float n = 0.5f;
+float i_control = 0.0f;
+float delta_R = 0.0f;
+float delta_L = 0.0f;
+int t = 0;
+int memo_R = 0;
+int memo_L = 0;
+
+/*void mot_L(float a, int n){
+    switch(n) {
+        case 0: //正転
+        motL_a = 0;
+        motL_b = a;
+        break;
+        case 1: //逆転
+        motL_a = a;
+        motL_b = 0;
+        break;
+        case 2: //ブレーキ
+        motL_a = 1;
+        motL_b = 1;
+        break;
+    }
+        
+}*/
+
+void event_handler_L(void) {
+    counter_L++;
+    LED_L++;
+}
+
+void event_handler_R(void) {
+    counter_R++;
+    LED_R++;
+}
+
+void omega_controller_R(float b, int n) {
+    switch(n) {
+        case 0: //0.05秒で0.03回
+        diff[0] = diff[1];
+        //diff[1] = 0.03f - b; //普通充電時
+        diff[1] = 0.025f - b; //Max充電時
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 1: //0.01秒で0.025回転
+        diff[0] = diff[1];
+        //diff[1] = 0.02f - b; //普通充電時
+        diff[1] = 0.0125f - b; //Max充電時
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 2: //0.01秒で0.0回転
+        diff[0] = diff[1];
+        diff[1] = 0.1f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d2+delta_R;
+        motR_b = 0;
+        break;
+        case 3: //0.01秒で0.0
+        diff[0] = diff[1];
+        diff[1] = 0.01f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = 0;
+        motR_b = 0.24f;
+        break;
+        case 4: //0.01秒で0.0回転
+        diff[0] = diff[1];
+        diff[1] = 0.07f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d2+delta_R;
+        motR_b = 0;
+        break;
+        case 5: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.009f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 6: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.0045f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 7: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.002f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 8: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.0001f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = 0;
+        motR_b = 0;
+        break;
+        case 9: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.055f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d2+delta_R;
+        motR_b = 0;
+        break;
+        case 10: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.015f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 11: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.002f - b;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_R = Kp * diff[1] + i_control * n;
+        motR_a = d1+delta_R;
+        motR_b = 0;
+        break;
+        case 12: //0.01秒で0.001回転
+        motR_a = 0;
+        motR_b = 0;
+        break;
+    }
+        
+}
+
+void omega_controller_L(float a, int n) {
+    switch(n) {
+        case 0: //0.01秒で0.005回
+        diff[0] = diff[1];
+        //diff[1] = 0.03f - a; //普通充電時
+        diff[1] = 0.025f - a; //Max充電時
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 1: //0.01秒で0.003回転
+        diff[0] = diff[1];
+        //diff[1] = 0.02f - a; //普通充電時
+        diff[1] = 0.0125f - a; //Max充電時
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 2: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.1f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d2+delta_L;
+        break;
+        case 3: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.01f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0.24f;
+        motL_b = 0;
+        break;
+        case 4: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.07f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d2+delta_L;
+        break;
+        case 5: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.01f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 6: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.005f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 7: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.003f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 8: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.0007f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = 0;
+        break;
+        case 9: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.055f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d2+delta_L;
+        break;
+        case 10: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.015f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 11: //0.01秒で0.001回転
+        diff[0] = diff[1];
+        diff[1] = 0.002f - a;
+        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
+        delta_L = Kp * diff[1] + i_control * n;
+        motL_a = 0;
+        motL_b = d1+delta_L;
+        break;
+        case 12: //0.01秒で0.001回転
+        motL_a = 0;
+        motL_b = 0;
+        break;
     }
 }
 
-int main (void) {      // mainスレッド
-    thread.start(callback(my_thread)); // 関数をスレッドとして起動する
-
-    while (true) {                  // スレッドの一つとして走る
-        wait(0.01);                 // 待つことしかしてないのでかなり正確
-        thread.flags_set(CYC_FLAG);     // 寝ているスレッドを起こす．この処理は一瞬
-    }
+void LED() {
+       // ThisThread::flags_wait_all(CYC_FLAG);
+        
+        LED_tire_R = (LED_R/12.0f)/38.2f; 
+        LED_tire_L = (LED_L/12.0f)/38.2f;
+        
+        dis_R = LED_tire_R * 57.3f * PI;
+        dis_L = LED_tire_L * 57.3f * PI;        
+        dis += (dis_R + dis_L) / 2.0f;
+        LED_R = 0.0f;
+        LED_L = 0.0f; 
+        
+        if(dis >= 200.0f) {
+            led = 1;
+        }
+        
+        if(dis >= 400.0f) {
+            led = 0;
+            dis = 0;
+        }
 }
 
+void robot_control_thread() {
+    while(true) {
+        ThisThread::flags_wait_all(CYC_FLAG);
+        tire_count_R = (counter_R/12.0f)/38.2f;
+        tire_count_L = (counter_L/12.0f)/38.2f;
+        LED_tire += (counter_R/12.0f)/38.2f;
+        
+        if(LED_tire < 24) {
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
+                omega_controller_R(tire_count_R, 5);
+                omega_controller_L(tire_count_L, 5);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
+                omega_controller_R(tire_count_R, 5);
+                omega_controller_L(tire_count_L, 6);
+            }
+        
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
+                omega_controller_R(tire_count_R, 7);
+                omega_controller_L(tire_count_L, 5);
+            }
+            
+            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
+                omega_controller_R(tire_count_R, 12);
+                omega_controller_L(tire_count_L, 7);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
+                omega_controller_R(tire_count_R, 7);
+                omega_controller_L(tire_count_L, 12);
+            }
+        } 
+        
+        if(LED_tire > 24) {  
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
+                omega_controller_R(tire_count_R, 0);
+                omega_controller_L(tire_count_L, 0);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
+                omega_controller_R(tire_count_R, 0);
+                omega_controller_L(tire_count_L, 12);
+            }
+        
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
+                omega_controller_R(tire_count_R, 12);
+                omega_controller_L(tire_count_L, 0);
+            }
+        
+            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
+                omega_controller_R(tire_count_R, 3);
+                omega_controller_L(tire_count_L, 1);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
+                omega_controller_R(tire_count_R, 1);
+                omega_controller_L(tire_count_L, 3);
+            }
+        }
+        
+        /*if(LED_tire > 39 && LED_tire < 48) {
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
+                omega_controller_R(tire_count_R, 5);
+                omega_controller_L(tire_count_L, 5);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
+                omega_controller_R(tire_count_R, 6);
+                omega_controller_L(tire_count_L, 7);
+            }
+        
+            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
+                omega_controller_R(tire_count_R, 7);
+                omega_controller_L(tire_count_L, 6);
+            }
+        
+            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
+                omega_controller_R(tire_count_R, 12);
+                omega_controller_L(tire_count_L, 7);
+            }
+        
+            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
+                omega_controller_R(tire_count_R, 7);
+                omega_controller_L(tire_count_L, 12);
+            }
+        }*/
+        //printf("v_L = %.1f, v_R = %.1f, v_C = %.1f\n", v_L*3.3F, v_R*3.3F, v_C*3.3F);
+        counter_R = 0;
+        counter_L = 0;
+    }
+}      
+              
+int main() {
+    thread_1.start(callback(robot_control_thread));
+    //thread_2.start(callback(LED));
+    encL.rise(&event_handler_L);
+    encR.rise(&event_handler_R);
+    
+     while(true) {
+         wait(0.01);
+         thread_1.flags_set(CYC_FLAG);
+         LED();
+        // thread_2.flags_set(CYC_FLAG);
+    }
+}
\ No newline at end of file