Diff: main.cpp

Revision:

4:45b45e8ca173

Parent:

1:5160ea45399b

Child:

5:237c49800e14

--- a/main.cpp	Fri Jun 23 17:59:41 2017 -0500
+++ b/main.cpp	Mon Dec 17 05:55:01 2018 +0000
@@ -1,12 +1,151 @@
-#include "mbed.h"
-
-PwmOut led(LED1);
-
-int main() {
-    // specify period first
-    led.period(4.0f);      // 4 second period
-    led.write(0.50f);      // 50% duty cycle, relative to period
-    //led = 0.5f;          // shorthand for led.write()
-    //led.pulsewidth(2);   // alternative to led.write, set duty cycle time in seconds
-    while(1);
+#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.70f;//フォトリフレクタのしきい値左
+float thr2 = 0.70f;//フォトリフレクタのしきい値真ん中
+float thr3 = 0.70f;//フォトリフレクタのしきい値右
+int judge1 = 0;
+int judge2 = 0;
+int judge3 = 0;
+int mode1 = 10;
+int mode2 = 2;
+int mode3 = 2;
+
+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.05f);//0.05秒ごとに回転数cを更新
+    
+    
+    while(1) {
+        distance = (distance1+distance2)/2;
+        
+        if(distance < 43*2*12*mode1){
+        
+        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;
+        
+            switch(n){
+              case(2)://白黒白進む
+              g = 500.0f;
+              gain1 = g - c2;
+              gain2 = g - c1;
+              
+              pwm11.write(0.50f + gain1*0.00001f);
+              pwm12.write(0.30f);
+              pwm21.write(0.50f + gain2*0.00001f);
+              pwm22.write(0.30f);
+              break;
+              
+              case(0)://白白白
+              g = 600.0f;
+              gain1 = g - c2;
+              gain2 = g - c1;
+              
+              pwm11.write(0.10f);
+              pwm12.write(0.40f );
+              pwm21.write(0.10f);
+              pwm22.write(0.40f );
+              break;
+              
+              
+            
+              case(1)://黒白白左回転強
+              pwm11.write(0.45f);
+              pwm12.write(0.30f);
+              pwm21.write(0.50f);
+              pwm22.write(0.30f);
+              break;
+            
+              case(4)://白白黒右回転
+              pwm11.write(0.50f);
+              pwm12.write(0.30f);
+              pwm21.write(0.45f);
+              pwm22.write(0.30f);
+              break;
+              
+              case(3)://黒黒白左回転弱             
+              pwm11.write(0.45f);
+              pwm12.write(0.30f);
+              pwm21.write(0.46f);
+              pwm22.write(0.30f);
+              break;
+              
+              case(6)://白黒黒右回転弱
+              pwm11.write(0.46f);
+              pwm12.write(0.30f);
+              pwm21.write(0.45f);
+              pwm22.write(0.30f);
+              break;
+              
+              case(7)://黒黒黒爆走
+              pwm11.write(0.60f);
+              pwm12.write(0.30f);
+              pwm21.write(0.60f);
+              pwm22.write(0.30f);
+              break;
+               
+              default://それ以外(ないけど)
+              pwm11.write(0.00f);
+              pwm12.write(0.00f);
+              pwm21.write(0.00f);
+              pwm22.write(0.00f);
+              break;
+              }
+        }
+        else{
+            
+            pwm11.write(0.00f);
+            pwm12.write(0.00f);
+            pwm21.write(0.00f);
+            pwm22.write(0.00f);
+            }
+    }
 }
\ No newline at end of file