Diff: main.cpp

Revision:

0:7a6b6c687fa3

Child:

1:f6b396987546

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 17 05:54:30 2018 +0000
@@ -0,0 +1,171 @@
+#include "function.h"
+
+PwmOut pwm11(D5);           //左正転
+PwmOut pwm12(D1);           //左逆転
+PwmOut pwm21(D6);           //右正転
+PwmOut pwm22(D9);           //右逆転
+DigitalOut LED(D2);
+InterruptIn button1(A2);    //フォトインタラプタ左
+InterruptIn button2(A1);    //フォトインタラプタ右
+AnalogIn   in1(A6);         //フォトリフレクタ左
+AnalogIn   in2(A4);         //フォトリフレクタ真ん中
+AnalogIn   in3(A3);         //フォトリフレクタ右
+
+float g = 0.0f;
+float gain1 = 0.0f;
+float gain2 = 0.0f;
+float dif1 = 0.0f;
+float dif2 = 0.0f;
+float thr1 = 0.40f;
+float thr2 = 0.40f;
+float thr3 = 0.40f;
+int judge1 = 0;
+int judge2 = 0;
+int judge3 = 0;
+
+int main() {
+    pwm11.period(0.01f);  //100Hz
+    pwm12.period(0.01f);  //100Hz
+    pwm21.period(0.01f);  //100Hz
+    pwm22.period(0.01f);  //100Hz
+    button1.rise(&flip1);
+    button2.rise(&flip2);
+    int n;//3biteで状態を表現
+    
+    flipper.attach(&flip3,0.05);//0.１秒ごとに回転数cを更新
+    
+    while(1) {
+        
+        if(  led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
+        LED = 1.0f;
+        wait(0.5f);
+        LED = 0;
+        led_count1 = 0;
+        led_count2 = 0;
+        }
+//////////////////floatのinをintのjudgeに変換
+        if(in1 > thr1){
+            judge1 = 1;
+            }
+         else{
+            judge1 = 0;
+            }
+            if(in2 > thr2){
+            judge2 = 1;
+            }
+         else{
+            judge2 = 0;
+            }
+            if(in3 > thr3){
+            judge3 = 1;
+            }
+         else{
+            judge3 = 0;
+            }
+/////////////////
+        n = judge1 + judge2*2 + judge3*4;
+   /*     distance = (distance1 + distance2) / 2;
+        if(ditance < 43*12*){
+            */
+            switch(n){
+              case(2)://白黒白ゆっくり進む
+              g = 500.0f;
+              dif1 = g - c2;
+              dif2 = g - c1;
+              gain1 = 0.0006f;
+              gain2 = 0.0006f;
+              pwm11.write(0.45f + dif1*gain1);
+              pwm12.write(0.30f);
+              pwm21.write(0.45f + dif2*gain2);
+              pwm22.write(0.30f);
+              break;
+              
+              case(0)://白白白バック
+              g = 500.0f;
+              dif1 = g - c1;
+              dif2 = g - c2;
+              gain1 = 0.001f;
+              gain2 = 0.001f;
+              
+              pwm11.write(0.10f);
+              pwm12.write(0.27f +dif1*gain1);
+              pwm21.write(0.10f);
+              pwm22.write(0.27f +dif2*gain2);
+              break;
+              
+              
+            
+              case(1)://黒白白左回転かなり強
+              g = 500.0f;
+              dif1 = g - c1;
+              dif2 = g - c2;
+              gain1 = 0.0002f;
+              gain2 = 0.0002f;
+              
+              pwm11.write(0.30f + dif1*gain1);
+              pwm12.write(0.30f);
+              pwm21.write(0.50f + dif2*gain2);
+              pwm22.write(0.30f);
+              break;
+            
+              case(4)://白白黒右回転かなり強
+              g = 500.0f;
+              dif1 = g - c1;
+              dif2 = g - c2;
+              gain1 = 0.0002f;
+              gain2 = 0.0002f;
+              
+              pwm11.write(0.50f + dif1*gain1);
+              pwm12.write(0.30f);
+              pwm21.write(0.20f);
+              pwm22.write(0.30f + dif2*gain2);
+              break;
+              
+              case(3)://黒黒白左回転強
+              g = 500.0f;
+              dif1 = g - c1;
+              dif2 = g - c2;         
+              gain1 = 0.0002f;
+              gain2 = 0.0002f;
+                  
+              pwm11.write(0.00f);
+              pwm12.write(0.00f);
+              pwm21.write(0.40f + dif2*gain2);
+              pwm22.write(0.20f);
+              break;
+              
+              case(6)://白黒黒右回転強
+              g = 500.0f;
+              dif1 = g - c1;
+              dif2 = g - c2; 
+              gain1 = 0.0002f;
+              gain2 = 0.0002f;
+              
+              pwm11.write(0.40f + dif1*gain1);
+              pwm12.write(0.20f);
+              pwm21.write(0.00f);
+              pwm22.write(0.00f);
+              break;
+              
+              case(7)://黒黒黒ゆっくり
+              dif1 = g - c1;
+              dif2 = g - c2;
+              gain1 = 0.0004f;
+              gain2 = 0.0004f;
+              
+              pwm11.write(0.45f + dif1*gain1);
+              pwm12.write(0.30f);
+              pwm21.write(0.45f + dif2*gain2);
+              pwm22.write(0.30f);
+              break;
+               
+              default://それ以外(多分ないけど)
+              pwm11.write(0.10f);
+              pwm12.write(0.70f);
+              pwm21.write(0.10f);
+              pwm22.write(0.70f);
+              break;
+              
+        }
+    }
+}
\ No newline at end of file