Revision 0:617b5fe8f980, committed 2019-11-12

Comitter:

komachiangel72

Date:

Tue Nov 12 01:02:51 2019 +0000

Commit message:

11/11; PM21:00; ver3.2

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 12 01:02:51 2019 +0000
@@ -0,0 +1,199 @@
+#include "mbed.h"
+
+//走行用プログラムver3.2
+//11/11pm21:00時点
+//モータードライバー＋回転数計測プログラム
+//走るためのプログラム+道が白か黒かを判定するプログラム
+//A入力に右のモーター、B入力に左のモーター
+
+InterruptIn enc_R(D12);//フォトインタラプタ右
+InterruptIn enc_L(D11);//フォトインタラプタ左
+
+AnalogIn sensor_R(A0);//進行方向右のフォトリフレクタセンサー    
+AnalogIn sensor_L(A1);//進行方向左のセンサー
+AnalogIn sensor_C(A2);//中央配置のセンサー
+
+DigitalOut led(LED1);
+
+int counter =0;
+
+void event_handler(void){
+    counter++;
+    led =!led;
+}
+
+
+PwmOut AIN1(D6);
+PwmOut AIN2(D5);
+PwmOut BIN1(D9);
+PwmOut BIN2(D10);//D9とD10は回路上で逆にしないと、なぜか回転方向が逆転
+ 
+void motorStop(PwmOut IN1,PwmOut IN2) {
+    IN1 = 1;
+    IN2 = 1;
+}
+ 
+void motorForward(PwmOut IN1,PwmOut IN2,float duty) {
+    IN1 = duty;
+    IN2 = 0;
+}
+ 
+void motorReverse(PwmOut IN1,PwmOut IN2,float duty) {
+    IN1 = 0;
+    IN2 = duty;
+}
+
+//停止
+void machine_Stop(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2) {
+    motorStop(AIN1,AIN2);
+    motorStop(BIN1,BIN2);
+}
+
+//前進
+void machine_Forward(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+    motorForward(AIN1,AIN2,duty);
+    motorForward(BIN1,BIN2,duty);
+}
+
+//後退
+void machine_Back(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+    machine_Stop(AIN1,AIN2,BIN1,BIN2);
+    wait(2);
+    motorReverse(AIN1,AIN2,duty);
+    motorReverse(BIN1,BIN2,duty);
+}
+
+//右カーブ
+void Right_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+    motorForward(AIN1,AIN2,duty-0.4f);
+    motorForward(BIN1,BIN2,duty);
+}
+
+//左カーブ
+void Left_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+
+    motorForward(AIN1,AIN2,duty);
+    motorForward(BIN1,BIN2,duty-0.4f);
+}
+
+
+//右急カーブ
+void Right_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+
+    motorForward(AIN1,AIN2,duty-0.6f);
+    motorForward(BIN1,BIN2,duty);
+}
+
+//左急カーブ
+void Left_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
+
+    motorForward(AIN1,AIN2,duty);
+    motorForward(BIN1,BIN2,duty-0.6f);
+}
+//制御(maenoshin)
+void control (int Left, int Center, int Right, int sec,float duty){
+    if(Left == 0 && Center == 1 && Right == 0){//全速前進だ！
+        machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
+        printf("Direction%d:Forward\n",sec);
+        }
+    if(Left == 1 && Center == 1 && Right == 0){//左折
+        Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
+        printf("Direction%d:Left turn\n",sec);
+        }
+    if(Left == 0 && Center == 1 && Right == 1){//右折
+        Right_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
+        printf("Direction%d:Right turn\n",sec);
+        }
+    if(Left == 1 && Center == 0 && Right == 0){//左折(急)
+        Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
+        printf("Direction%d:Left turn hurry!\n",sec);
+        }
+    if(Left == 0 && Center == 0 && Right == 1){//右折(急)
+        Right_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
+        printf("Direction%d:Right turn hurry!\n",sec);
+        }
+    else{ printf("Direction%d:error\n",sec);
+    }
+    }
+    
+    
+        
+        
+//最終的には
+//左のモーターのスピード = 比例ゲイン×（左のセンサ値 - 左の黒値） / （左の白値 - 左の黒値）＋基本最低スピード
+//右のモーターのスピード = 比例ゲイン×（右のセンサ値 - 右の黒値） / （右の白値 - 右の黒値）＋基本最低スピード
+
+//白値はセンサーで白を検出する最大値
+//黒値はセンサーで黒を検出する最少値
+
+int main() {
+ 
+    machine_Stop(AIN1,AIN2,BIN1,BIN2);
+    int sec =0;
+
+    while(1) {
+        float duty = 0.9f;//デューティー比90%
+        int Center = 0;
+        int Right = 0;
+        int Left =0;
+        if(sensor_C*3.3f>1.0f){//中央センサーが黒
+            Center = 1;
+        }else{Center =0;}
+        if(sensor_L*3.3f>1.0f){//左センサーが黒
+            Left = 1;
+        }else{Left =0;}
+        if(sensor_R*3.3f>1.0f){//右センサーが黒
+            Right = 1;
+        }else{Right =0;}
+        
+        control(Left,Center,Right,sec,duty);
+        wait (5);
+        machine_Stop(AIN1,AIN2,BIN1,BIN2);
+        wait (2);
+        sec++;
+          
+         
+        
+        
+            //if(sensor_L*3.3f<1.2f){//左センサーだけ白
+            //    Right_rotation(AIN1,AIN2,BIN1,BIN2,duty);
+            //    printf("Direction:Right turn\n");
+            //}
+            
+            //else if(sensor_R*3.3f<1.2f){//右センサーだけ白
+            //    Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
+            //    printf("Direction:Left turn\n");
+            //}
+            //else{
+            //   machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
+            //    printf("Direction:Forward\n");
+            //}
+        //}
+            
+        //else if(sensor_C<1.2f){//中央センサーが白
+            
+           // duty = 0.6f;//デューティー比60%
+            //machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり進む
+            
+            //if(sensor_R*3.3f<1.0f){//中央が出た後右センサーが出たら、左に急カーブ
+             //   Left_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
+               // printf("Direction:Left high speed turn\n");
+            //} 
+
+            //else if(sensor_L*3.3f<1.0f){//中央が出た後左センサーが出たら、右に急カーブ
+              //  Right_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
+               // printf("Direction:Right high speed turn\n");
+            //} 
+            //else if((sensor_R*3.3f<1.0f)&&(sensor_L*3.3f<1.0f)){//全部白（勢いがつきすぎて飛び出たなど）
+            //    duty = 0.6f;//デューティー比60%
+             //   machine_Back(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり後ろにさがる
+              //  printf("Direction:Back\n");
+           // }
+        //}
+
+        printf("sensor_Right:L432[%.3f]>\n",sensor_R*3.3F);//電圧測定
+        printf("sensor_Left:L432[%.3f]>\n",sensor_L*3.3F);//電圧測定
+        printf("sensor_Center:L432[%.3f]>\n",sensor_C*3.3F);//電圧測定
+        wait(3);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Nov 12 01:02:51 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file