12_han_meiji / Mbed 2 deprecated 7_1_traveling_ver3_2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:617b5fe8f980, 2019-11-12 (annotated)

Committer:
komachiangel72
Date:
Tue Nov 12 01:02:51 2019 +0000
Revision:
0:617b5fe8f980
11/11; PM21:00; ver3.2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komachiangel72 0:617b5fe8f980 1 #include "mbed.h"
komachiangel72 0:617b5fe8f980 2
komachiangel72 0:617b5fe8f980 3 //走行用プログラムver3.2
komachiangel72 0:617b5fe8f980 4 //11/11pm21:00時点
komachiangel72 0:617b5fe8f980 5 //モータードライバー＋回転数計測プログラム
komachiangel72 0:617b5fe8f980 6 //走るためのプログラム+道が白か黒かを判定するプログラム
komachiangel72 0:617b5fe8f980 7 //A入力に右のモーター、B入力に左のモーター
komachiangel72 0:617b5fe8f980 8
komachiangel72 0:617b5fe8f980 9 InterruptIn enc_R(D12);//フォトインタラプタ右
komachiangel72 0:617b5fe8f980 10 InterruptIn enc_L(D11);//フォトインタラプタ左
komachiangel72 0:617b5fe8f980 11
komachiangel72 0:617b5fe8f980 12 AnalogIn sensor_R(A0);//進行方向右のフォトリフレクタセンサー
komachiangel72 0:617b5fe8f980 13 AnalogIn sensor_L(A1);//進行方向左のセンサー
komachiangel72 0:617b5fe8f980 14 AnalogIn sensor_C(A2);//中央配置のセンサー
komachiangel72 0:617b5fe8f980 15
komachiangel72 0:617b5fe8f980 16 DigitalOut led(LED1);
komachiangel72 0:617b5fe8f980 17
komachiangel72 0:617b5fe8f980 18 int counter =0;
komachiangel72 0:617b5fe8f980 19
komachiangel72 0:617b5fe8f980 20 void event_handler(void){
komachiangel72 0:617b5fe8f980 21 counter++;
komachiangel72 0:617b5fe8f980 22 led =!led;
komachiangel72 0:617b5fe8f980 23 }
komachiangel72 0:617b5fe8f980 24
komachiangel72 0:617b5fe8f980 25
komachiangel72 0:617b5fe8f980 26 PwmOut AIN1(D6);
komachiangel72 0:617b5fe8f980 27 PwmOut AIN2(D5);
komachiangel72 0:617b5fe8f980 28 PwmOut BIN1(D9);
komachiangel72 0:617b5fe8f980 29 PwmOut BIN2(D10);//D9とD10は回路上で逆にしないと、なぜか回転方向が逆転
komachiangel72 0:617b5fe8f980 30
komachiangel72 0:617b5fe8f980 31 void motorStop(PwmOut IN1,PwmOut IN2) {
komachiangel72 0:617b5fe8f980 32 IN1 = 1;
komachiangel72 0:617b5fe8f980 33 IN2 = 1;
komachiangel72 0:617b5fe8f980 34 }
komachiangel72 0:617b5fe8f980 35
komachiangel72 0:617b5fe8f980 36 void motorForward(PwmOut IN1,PwmOut IN2,float duty) {
komachiangel72 0:617b5fe8f980 37 IN1 = duty;
komachiangel72 0:617b5fe8f980 38 IN2 = 0;
komachiangel72 0:617b5fe8f980 39 }
komachiangel72 0:617b5fe8f980 40
komachiangel72 0:617b5fe8f980 41 void motorReverse(PwmOut IN1,PwmOut IN2,float duty) {
komachiangel72 0:617b5fe8f980 42 IN1 = 0;
komachiangel72 0:617b5fe8f980 43 IN2 = duty;
komachiangel72 0:617b5fe8f980 44 }
komachiangel72 0:617b5fe8f980 45
komachiangel72 0:617b5fe8f980 46 //停止
komachiangel72 0:617b5fe8f980 47 void machine_Stop(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2) {
komachiangel72 0:617b5fe8f980 48 motorStop(AIN1,AIN2);
komachiangel72 0:617b5fe8f980 49 motorStop(BIN1,BIN2);
komachiangel72 0:617b5fe8f980 50 }
komachiangel72 0:617b5fe8f980 51
komachiangel72 0:617b5fe8f980 52 //前進
komachiangel72 0:617b5fe8f980 53 void machine_Forward(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 54 motorForward(AIN1,AIN2,duty);
komachiangel72 0:617b5fe8f980 55 motorForward(BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 56 }
komachiangel72 0:617b5fe8f980 57
komachiangel72 0:617b5fe8f980 58 //後退
komachiangel72 0:617b5fe8f980 59 void machine_Back(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 60 machine_Stop(AIN1,AIN2,BIN1,BIN2);
komachiangel72 0:617b5fe8f980 61 wait(2);
komachiangel72 0:617b5fe8f980 62 motorReverse(AIN1,AIN2,duty);
komachiangel72 0:617b5fe8f980 63 motorReverse(BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 64 }
komachiangel72 0:617b5fe8f980 65
komachiangel72 0:617b5fe8f980 66 //右カーブ
komachiangel72 0:617b5fe8f980 67 void Right_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 68 motorForward(AIN1,AIN2,duty-0.4f);
komachiangel72 0:617b5fe8f980 69 motorForward(BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 70 }
komachiangel72 0:617b5fe8f980 71
komachiangel72 0:617b5fe8f980 72 //左カーブ
komachiangel72 0:617b5fe8f980 73 void Left_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 74
komachiangel72 0:617b5fe8f980 75 motorForward(AIN1,AIN2,duty);
komachiangel72 0:617b5fe8f980 76 motorForward(BIN1,BIN2,duty-0.4f);
komachiangel72 0:617b5fe8f980 77 }
komachiangel72 0:617b5fe8f980 78
komachiangel72 0:617b5fe8f980 79
komachiangel72 0:617b5fe8f980 80 //右急カーブ
komachiangel72 0:617b5fe8f980 81 void Right_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 82
komachiangel72 0:617b5fe8f980 83 motorForward(AIN1,AIN2,duty-0.6f);
komachiangel72 0:617b5fe8f980 84 motorForward(BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 85 }
komachiangel72 0:617b5fe8f980 86
komachiangel72 0:617b5fe8f980 87 //左急カーブ
komachiangel72 0:617b5fe8f980 88 void Left_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
komachiangel72 0:617b5fe8f980 89
komachiangel72 0:617b5fe8f980 90 motorForward(AIN1,AIN2,duty);
komachiangel72 0:617b5fe8f980 91 motorForward(BIN1,BIN2,duty-0.6f);
komachiangel72 0:617b5fe8f980 92 }
komachiangel72 0:617b5fe8f980 93 //制御(maenoshin)
komachiangel72 0:617b5fe8f980 94 void control (int Left, int Center, int Right, int sec,float duty){
komachiangel72 0:617b5fe8f980 95 if(Left == 0 && Center == 1 && Right == 0){//全速前進だ！
komachiangel72 0:617b5fe8f980 96 machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 97 printf("Direction%d:Forward\n",sec);
komachiangel72 0:617b5fe8f980 98 }
komachiangel72 0:617b5fe8f980 99 if(Left == 1 && Center == 1 && Right == 0){//左折
komachiangel72 0:617b5fe8f980 100 Left_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 101 printf("Direction%d:Left turn\n",sec);
komachiangel72 0:617b5fe8f980 102 }
komachiangel72 0:617b5fe8f980 103 if(Left == 0 && Center == 1 && Right == 1){//右折
komachiangel72 0:617b5fe8f980 104 Right_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 105 printf("Direction%d:Right turn\n",sec);
komachiangel72 0:617b5fe8f980 106 }
komachiangel72 0:617b5fe8f980 107 if(Left == 1 && Center == 0 && Right == 0){//左折(急)
komachiangel72 0:617b5fe8f980 108 Left_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 109 printf("Direction%d:Left turn hurry!\n",sec);
komachiangel72 0:617b5fe8f980 110 }
komachiangel72 0:617b5fe8f980 111 if(Left == 0 && Center == 0 && Right == 1){//右折(急)
komachiangel72 0:617b5fe8f980 112 Right_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 113 printf("Direction%d:Right turn hurry!\n",sec);
komachiangel72 0:617b5fe8f980 114 }
komachiangel72 0:617b5fe8f980 115 else{ printf("Direction%d:error\n",sec);
komachiangel72 0:617b5fe8f980 116 }
komachiangel72 0:617b5fe8f980 117 }
komachiangel72 0:617b5fe8f980 118
komachiangel72 0:617b5fe8f980 119
komachiangel72 0:617b5fe8f980 120
komachiangel72 0:617b5fe8f980 121
komachiangel72 0:617b5fe8f980 122 //最終的には
komachiangel72 0:617b5fe8f980 123 //左のモーターのスピード = 比例ゲイン×（左のセンサ値 - 左の黒値） / （左の白値 - 左の黒値）＋基本最低スピード
komachiangel72 0:617b5fe8f980 124 //右のモーターのスピード = 比例ゲイン×（右のセンサ値 - 右の黒値） / （右の白値 - 右の黒値）＋基本最低スピード
komachiangel72 0:617b5fe8f980 125
komachiangel72 0:617b5fe8f980 126 //白値はセンサーで白を検出する最大値
komachiangel72 0:617b5fe8f980 127 //黒値はセンサーで黒を検出する最少値
komachiangel72 0:617b5fe8f980 128
komachiangel72 0:617b5fe8f980 129 int main() {
komachiangel72 0:617b5fe8f980 130
komachiangel72 0:617b5fe8f980 131 machine_Stop(AIN1,AIN2,BIN1,BIN2);
komachiangel72 0:617b5fe8f980 132 int sec =0;
komachiangel72 0:617b5fe8f980 133
komachiangel72 0:617b5fe8f980 134 while(1) {
komachiangel72 0:617b5fe8f980 135 float duty = 0.9f;//デューティー比90%
komachiangel72 0:617b5fe8f980 136 int Center = 0;
komachiangel72 0:617b5fe8f980 137 int Right = 0;
komachiangel72 0:617b5fe8f980 138 int Left =0;
komachiangel72 0:617b5fe8f980 139 if(sensor_C*3.3f>1.0f){//中央センサーが黒
komachiangel72 0:617b5fe8f980 140 Center = 1;
komachiangel72 0:617b5fe8f980 141 }else{Center =0;}
komachiangel72 0:617b5fe8f980 142 if(sensor_L*3.3f>1.0f){//左センサーが黒
komachiangel72 0:617b5fe8f980 143 Left = 1;
komachiangel72 0:617b5fe8f980 144 }else{Left =0;}
komachiangel72 0:617b5fe8f980 145 if(sensor_R*3.3f>1.0f){//右センサーが黒
komachiangel72 0:617b5fe8f980 146 Right = 1;
komachiangel72 0:617b5fe8f980 147 }else{Right =0;}
komachiangel72 0:617b5fe8f980 148
komachiangel72 0:617b5fe8f980 149 control(Left,Center,Right,sec,duty);
komachiangel72 0:617b5fe8f980 150 wait (5);
komachiangel72 0:617b5fe8f980 151 machine_Stop(AIN1,AIN2,BIN1,BIN2);
komachiangel72 0:617b5fe8f980 152 wait (2);
komachiangel72 0:617b5fe8f980 153 sec++;
komachiangel72 0:617b5fe8f980 154
komachiangel72 0:617b5fe8f980 155
komachiangel72 0:617b5fe8f980 156
komachiangel72 0:617b5fe8f980 157
komachiangel72 0:617b5fe8f980 158 //if(sensor_L*3.3f<1.2f){//左センサーだけ白
komachiangel72 0:617b5fe8f980 159 // Right_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 160 // printf("Direction:Right turn\n");
komachiangel72 0:617b5fe8f980 161 //}
komachiangel72 0:617b5fe8f980 162
komachiangel72 0:617b5fe8f980 163 //else if(sensor_R*3.3f<1.2f){//右センサーだけ白
komachiangel72 0:617b5fe8f980 164 // Left_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 165 // printf("Direction:Left turn\n");
komachiangel72 0:617b5fe8f980 166 //}
komachiangel72 0:617b5fe8f980 167 //else{
komachiangel72 0:617b5fe8f980 168 // machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 169 // printf("Direction:Forward\n");
komachiangel72 0:617b5fe8f980 170 //}
komachiangel72 0:617b5fe8f980 171 //}
komachiangel72 0:617b5fe8f980 172
komachiangel72 0:617b5fe8f980 173 //else if(sensor_C<1.2f){//中央センサーが白
komachiangel72 0:617b5fe8f980 174
komachiangel72 0:617b5fe8f980 175 // duty = 0.6f;//デューティー比60%
komachiangel72 0:617b5fe8f980 176 //machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり進む
komachiangel72 0:617b5fe8f980 177
komachiangel72 0:617b5fe8f980 178 //if(sensor_R*3.3f<1.0f){//中央が出た後右センサーが出たら、左に急カーブ
komachiangel72 0:617b5fe8f980 179 // Left_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 180 // printf("Direction:Left high speed turn\n");
komachiangel72 0:617b5fe8f980 181 //}
komachiangel72 0:617b5fe8f980 182
komachiangel72 0:617b5fe8f980 183 //else if(sensor_L*3.3f<1.0f){//中央が出た後左センサーが出たら、右に急カーブ
komachiangel72 0:617b5fe8f980 184 // Right_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
komachiangel72 0:617b5fe8f980 185 // printf("Direction:Right high speed turn\n");
komachiangel72 0:617b5fe8f980 186 //}
komachiangel72 0:617b5fe8f980 187 //else if((sensor_R*3.3f<1.0f)&&(sensor_L*3.3f<1.0f)){//全部白（勢いがつきすぎて飛び出たなど）
komachiangel72 0:617b5fe8f980 188 // duty = 0.6f;//デューティー比60%
komachiangel72 0:617b5fe8f980 189 // machine_Back(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり後ろにさがる
komachiangel72 0:617b5fe8f980 190 // printf("Direction:Back\n");
komachiangel72 0:617b5fe8f980 191 // }
komachiangel72 0:617b5fe8f980 192 //}
komachiangel72 0:617b5fe8f980 193
komachiangel72 0:617b5fe8f980 194 printf("sensor_Right:L432[%.3f]>\n",sensor_R*3.3F);//電圧測定
komachiangel72 0:617b5fe8f980 195 printf("sensor_Left:L432[%.3f]>\n",sensor_L*3.3F);//電圧測定
komachiangel72 0:617b5fe8f980 196 printf("sensor_Center:L432[%.3f]>\n",sensor_C*3.3F);//電圧測定
komachiangel72 0:617b5fe8f980 197 wait(3);
komachiangel72 0:617b5fe8f980 198 }
komachiangel72 0:617b5fe8f980 199 }