Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termC

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@8:b47cebb6aeb8, 2018-11-21 (annotated)

Committer:
kamorei
Date:
Wed Nov 21 06:57:40 2018 +0000
Revision:
8:b47cebb6aeb8
Parent:
7:f1f9bc420dba
Child:
9:d989333191cb
11/21 ????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 0:500d22d1efeb 1 #include "mbed.h"
kamorei 0:500d22d1efeb 2
kamorei 0:500d22d1efeb 3 #define KP 5.8 //Pゲイン
kamorei 7:f1f9bc420dba 4 #define CHG 1.0 //change_gray用の係数
kamorei 7:f1f9bc420dba 5 #define WHITE_CL 0.27 //初期値
kamorei 7:f1f9bc420dba 6 #define WHITE_CR 0.235 //初期値
kamorei 7:f1f9bc420dba 7 #define GRAY_CL 0.15 //初期値
kamorei 7:f1f9bc420dba 8 #define GRAY_CR 0.13 //初期値
kamorei 3:e8c29cd3ca22 9 #define WHITE_L 0.1 //初期値
kamorei 3:e8c29cd3ca22 10 #define WHITE_R 0.1 //初期値
kamorei 3:e8c29cd3ca22 11 #define GRAY_L 0.05 //初期値
kamorei 3:e8c29cd3ca22 12 #define GRAY_R 0.05 //初期値
kamorei 0:500d22d1efeb 13
kamorei 4:edec6fe32ba9 14 //#define graychange
kamorei 0:500d22d1efeb 15
kamorei 4:edec6fe32ba9 16 DigitalOut ledCheck( PTE2); //動作check
kamorei 4:edec6fe32ba9 17 DigitalOut ledCount( PTB11); //プログラムの切り替え確認
kamorei 4:edec6fe32ba9 18 BusOut ledCHG( PTB10, PTE3); //change_gray用
kamorei 4:edec6fe32ba9 19 BusOut ledLL( PTE5, PTE4); //ロボットから進行方向を見て左（わざと逆書き）
kamorei 4:edec6fe32ba9 20 BusOut ledRR( PTB8, PTB9); //ロボットから進行方向を見て右
kamorei 3:e8c29cd3ca22 21 AnalogIn sensorR( PTC2);
kamorei 0:500d22d1efeb 22 AnalogIn sensorL( PTB3);
kamorei 0:500d22d1efeb 23 AnalogIn sensorCR( PTB0);
kamorei 0:500d22d1efeb 24 AnalogIn sensorCL( PTB2);
kamorei 0:500d22d1efeb 25 //モータ1
kamorei 0:500d22d1efeb 26 BusOut Mlefti(PTA1, PTA2);
kamorei 0:500d22d1efeb 27 PwmOut Mleftp(PTD4);
kamorei 0:500d22d1efeb 28 //モータ2
kamorei 0:500d22d1efeb 29 BusOut Mrighti(PTC0, PTC7);
kamorei 0:500d22d1efeb 30 PwmOut Mrightp(PTA12);
kamorei 0:500d22d1efeb 31
kamorei 0:500d22d1efeb 32 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
kamorei 3:e8c29cd3ca22 33 float whiteL[2], whiteR[2], grayL[2], grayR[2];
kamorei 7:f1f9bc420dba 34 float blackCL = 0.03, blackCR = 0.02; //閾値
kamorei 7:f1f9bc420dba 35 float blackL = 0.018, blackR = 0.018;
kamorei 3:e8c29cd3ca22 36 float stepL = 0.45, stepR = 0.38;
kamorei 0:500d22d1efeb 37 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 5:30251ab9313a 38 float pr, pl, chg;
kamorei 0:500d22d1efeb 39
kamorei 0:500d22d1efeb 40 void stop_point_ver2();
kamorei 3:e8c29cd3ca22 41 void motor_check();
kamorei 0:500d22d1efeb 42
kamorei 0:500d22d1efeb 43 void set_threshold(){ //閾値の初期設定
kamorei 0:500d22d1efeb 44 for( int i = 0; i < 2; i++){
kamorei 0:500d22d1efeb 45 whiteCL[i] = WHITE_CL;
kamorei 0:500d22d1efeb 46 whiteCR[i] = WHITE_CR;
kamorei 0:500d22d1efeb 47 grayCL[i] = GRAY_CL;
kamorei 0:500d22d1efeb 48 grayCR[i] = GRAY_CR;
kamorei 3:e8c29cd3ca22 49 whiteL[i] = WHITE_L;
kamorei 3:e8c29cd3ca22 50 whiteR[i] = WHITE_R;
kamorei 0:500d22d1efeb 51 grayL[i] = GRAY_L;
kamorei 0:500d22d1efeb 52 grayR[i] = GRAY_R;
kamorei 0:500d22d1efeb 53 }
kamorei 0:500d22d1efeb 54 }
kamorei 0:500d22d1efeb 55
kamorei 4:edec6fe32ba9 56 void set_sensor(){ //センサ系
kamorei 4:edec6fe32ba9 57 sensor[0] = sensorL.read();
kamorei 4:edec6fe32ba9 58 sensor[1] = sensorCL.read();
kamorei 4:edec6fe32ba9 59 sensor[2] = sensorCR.read();
kamorei 4:edec6fe32ba9 60 sensor[3] = sensorR.read();
kamorei 4:edec6fe32ba9 61 }
kamorei 4:edec6fe32ba9 62
kamorei 0:500d22d1efeb 63 void go_straight_p(){ //P制御でトレース
kamorei 4:edec6fe32ba9 64 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 4:edec6fe32ba9 65 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 2:47477c2b1925 66 Mrighti = 2;
kamorei 7:f1f9bc420dba 67 if( pl >= 0.0)
kamorei 7:f1f9bc420dba 68 Mrightp = (KP * pr) * 1.0f;
kamorei 7:f1f9bc420dba 69 else
kamorei 7:f1f9bc420dba 70 Mrightp = 1.0f;
kamorei 2:47477c2b1925 71 Mlefti = 2;
kamorei 7:f1f9bc420dba 72 if( pr >= 0.0)
kamorei 8:b47cebb6aeb8 73 Mleftp = (KP * pl) * 1.0f;
kamorei 7:f1f9bc420dba 74 else
kamorei 7:f1f9bc420dba 75 Mleftp = 1.0f;
kamorei 4:edec6fe32ba9 76 ledRR = 0b01;
kamorei 3:e8c29cd3ca22 77 ledLL = 0b01;
kamorei 0:500d22d1efeb 78 }
kamorei 0:500d22d1efeb 79
kamorei 0:500d22d1efeb 80 void go_straight_CR(){ //CRのみでトレース
kamorei 4:edec6fe32ba9 81 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 2:47477c2b1925 82 Mrighti = 2;
kamorei 7:f1f9bc420dba 83 Mrightp = (KP * pr) * 1.0f;
kamorei 2:47477c2b1925 84 Mlefti = 2;
kamorei 7:f1f9bc420dba 85 Mleftp = (1.0 - KP * pr) * 1.0f;
kamorei 0:500d22d1efeb 86 ledRR = 0b11;
kamorei 6:28c5a5992a81 87 ledLL = 0;
kamorei 0:500d22d1efeb 88 }
kamorei 0:500d22d1efeb 89
kamorei 0:500d22d1efeb 90 void turn_right(){
kamorei 2:47477c2b1925 91 Mrighti = 1;
kamorei 0:500d22d1efeb 92 Mrightp = 0.05f;
kamorei 2:47477c2b1925 93 Mlefti = 2;
kamorei 3:e8c29cd3ca22 94 Mleftp = 0.5f;
kamorei 4:edec6fe32ba9 95 ledRR = 0b01;
kamorei 3:e8c29cd3ca22 96 ledLL = 0;
kamorei 0:500d22d1efeb 97 }
kamorei 0:500d22d1efeb 98
kamorei 0:500d22d1efeb 99 void turn_left(){
kamorei 2:47477c2b1925 100 Mrighti = 2;
kamorei 3:e8c29cd3ca22 101 Mrightp = 0.5f;
kamorei 2:47477c2b1925 102 Mlefti = 1;
kamorei 0:500d22d1efeb 103 Mleftp = 0.05f;
kamorei 3:e8c29cd3ca22 104 ledRR = 0;
kamorei 3:e8c29cd3ca22 105 ledLL = 0b01;
kamorei 0:500d22d1efeb 106 }
kamorei 0:500d22d1efeb 107
kamorei 7:f1f9bc420dba 108 void turn_right_corner( float f){
kamorei 3:e8c29cd3ca22 109 motor_check();
kamorei 7:f1f9bc420dba 110 wait(f);
kamorei 7:f1f9bc420dba 111 sensor[3] = sensorR.read();
kamorei 7:f1f9bc420dba 112 while( sensor[3] >= blackR){ //sensorR > grayR[0]
kamorei 3:e8c29cd3ca22 113 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 114 turn_right();
kamorei 0:500d22d1efeb 115 }
kamorei 0:500d22d1efeb 116 stop_point_ver2();
kamorei 7:f1f9bc420dba 117 // turn_left();
kamorei 7:f1f9bc420dba 118 // wait(0.3);
kamorei 0:500d22d1efeb 119 }
kamorei 0:500d22d1efeb 120
kamorei 7:f1f9bc420dba 121 void turn_left_corner( float f){
kamorei 3:e8c29cd3ca22 122 motor_check();
kamorei 7:f1f9bc420dba 123 wait(f);
kamorei 7:f1f9bc420dba 124 sensor[0] = sensorL.read();
kamorei 7:f1f9bc420dba 125 while( sensor[0] >= blackL){ //sensorL > grayL[0]
kamorei 3:e8c29cd3ca22 126 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 127 turn_left();
kamorei 0:500d22d1efeb 128 }
kamorei 0:500d22d1efeb 129 stop_point_ver2();
kamorei 7:f1f9bc420dba 130 // turn_right();
kamorei 7:f1f9bc420dba 131 // wait(0.3);
kamorei 0:500d22d1efeb 132 }
kamorei 0:500d22d1efeb 133
kamorei 4:edec6fe32ba9 134 void motor_check(){ //モータドライバの調子の確認用と単純に直進
kamorei 2:47477c2b1925 135 Mrighti = 2;
kamorei 0:500d22d1efeb 136 Mrightp = 1.0f;
kamorei 2:47477c2b1925 137 Mlefti = 2;
kamorei 0:500d22d1efeb 138 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 139 }
kamorei 0:500d22d1efeb 140
kamorei 0:500d22d1efeb 141 void stop_point_ver2(){
kamorei 0:500d22d1efeb 142 if( Mrighti == 1)
kamorei 0:500d22d1efeb 143 Mrighti = 2;
kamorei 0:500d22d1efeb 144 else if( Mrighti == 2)
kamorei 0:500d22d1efeb 145 Mrighti = 1;
kamorei 0:500d22d1efeb 146 if( Mlefti == 1)
kamorei 0:500d22d1efeb 147 Mlefti = 2;
kamorei 0:500d22d1efeb 148 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 149 Mlefti = 1;
kamorei 4:edec6fe32ba9 150 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 151 Mleftp = 1.0f;
kamorei 3:e8c29cd3ca22 152 wait(0.1);
kamorei 0:500d22d1efeb 153 Mrighti = 0;
kamorei 0:500d22d1efeb 154 Mlefti = 0;
kamorei 0:500d22d1efeb 155 ledRR = 0b11;
kamorei 0:500d22d1efeb 156 ledLL = 0b11;
kamorei 0:500d22d1efeb 157 }
kamorei 0:500d22d1efeb 158
kamorei 3:e8c29cd3ca22 159 void change_gray_ver2(int i){
kamorei 4:edec6fe32ba9 160 if( i == 0){ //Rが白
kamorei 3:e8c29cd3ca22 161 whiteR[0] = sensorR.read();
kamorei 5:30251ab9313a 162 chg = (whiteR[1] - whiteR[0]) / whiteR[1];
kamorei 5:30251ab9313a 163 grayCR[0] = grayCR[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 164 grayCL[0] = grayCL[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 165 grayR[0] = grayR[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 166 grayL[0] = grayL[1] * ( 1 + CHG * chg);
kamorei 4:edec6fe32ba9 167 ledCHG = 0b01;
kamorei 4:edec6fe32ba9 168 wait( 0.1);
kamorei 4:edec6fe32ba9 169 ledCHG = 0;
kamorei 3:e8c29cd3ca22 170 }
kamorei 4:edec6fe32ba9 171 else { //Lが白
kamorei 3:e8c29cd3ca22 172 whiteL[0] = sensorL.read();
kamorei 5:30251ab9313a 173 chg = (whiteL[1] - whiteL[0]) / whiteL[1];
kamorei 5:30251ab9313a 174 grayCR[0] = grayCR[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 175 grayCL[0] = grayCL[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 176 grayR[0] = grayR[1] * ( 1 + CHG * chg);
kamorei 5:30251ab9313a 177 grayL[0] = grayL[1] * ( 1 + CHG * chg);
kamorei 4:edec6fe32ba9 178 ledCHG = 0b10;
kamorei 4:edec6fe32ba9 179 wait( 0.1);
kamorei 4:edec6fe32ba9 180 ledCHG = 0;
kamorei 3:e8c29cd3ca22 181 }
kamorei 3:e8c29cd3ca22 182 }
kamorei 3:e8c29cd3ca22 183
kamorei 4:edec6fe32ba9 184 void LED_blinky(){ //プログラムの切り替わり確認用
kamorei 4:edec6fe32ba9 185 for( int i = 0; i < 4; i++){
kamorei 4:edec6fe32ba9 186 ledCount = !ledCount;
kamorei 5:30251ab9313a 187 wait(0.05);
kamorei 3:e8c29cd3ca22 188 }
kamorei 3:e8c29cd3ca22 189 }
kamorei 3:e8c29cd3ca22 190
kamorei 0:500d22d1efeb 191 int main() {
kamorei 4:edec6fe32ba9 192 #ifdef graychange
kamorei 4:edec6fe32ba9 193 int i = 1; //i == 0ならRがwhite，i == 1ならLがwhite
kamorei 4:edec6fe32ba9 194 #endif
kamorei 4:edec6fe32ba9 195 ledCheck = 1;
kamorei 0:500d22d1efeb 196 set_threshold();
kamorei 4:edec6fe32ba9 197 LED_blinky();
kamorei 4:edec6fe32ba9 198 while(1){ //スタート～最初の左折カーブ
kamorei 4:edec6fe32ba9 199 set_sensor();
kamorei 4:edec6fe32ba9 200 #ifdef graychange
kamorei 4:edec6fe32ba9 201 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 202 #endif
kamorei 4:edec6fe32ba9 203 if( sensor[3] <= blackR){
kamorei 7:f1f9bc420dba 204 turn_right_corner( 0.2);
kamorei 4:edec6fe32ba9 205 break;
kamorei 4:edec6fe32ba9 206 } else
kamorei 4:edec6fe32ba9 207 go_straight_p();
kamorei 4:edec6fe32ba9 208 }
kamorei 4:edec6fe32ba9 209 LED_blinky();
kamorei 4:edec6fe32ba9 210 while(1){ //左折カーブ～段差～左折カーブ
kamorei 4:edec6fe32ba9 211 set_sensor();
kamorei 4:edec6fe32ba9 212 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 213 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 214 break;
kamorei 4:edec6fe32ba9 215 } else
kamorei 7:f1f9bc420dba 216 go_straight_p();
kamorei 4:edec6fe32ba9 217 }
kamorei 4:edec6fe32ba9 218 LED_blinky();
kamorei 4:edec6fe32ba9 219 #ifdef graychange
kamorei 4:edec6fe32ba9 220 i = 0;
kamorei 4:edec6fe32ba9 221 #endif
kamorei 4:edec6fe32ba9 222 while(1){ //左折カーブ～左折カーブ
kamorei 4:edec6fe32ba9 223 set_sensor();
kamorei 4:edec6fe32ba9 224 #ifdef graychange
kamorei 4:edec6fe32ba9 225 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 226 #endif
kamorei 4:edec6fe32ba9 227 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 228 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 229 break;
kamorei 4:edec6fe32ba9 230 } else
kamorei 4:edec6fe32ba9 231 go_straight_p();
kamorei 4:edec6fe32ba9 232 }
kamorei 4:edec6fe32ba9 233 LED_blinky();
kamorei 4:edec6fe32ba9 234 while(1){ //左折カーブ～トンネル手前のマーク
kamorei 4:edec6fe32ba9 235 set_sensor();
kamorei 7:f1f9bc420dba 236 if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0])
kamorei 3:e8c29cd3ca22 237 break;
kamorei 4:edec6fe32ba9 238 else
kamorei 4:edec6fe32ba9 239 go_straight_CR();
kamorei 4:edec6fe32ba9 240 }
kamorei 4:edec6fe32ba9 241 LED_blinky();
kamorei 4:edec6fe32ba9 242 while(1){ //トンネル手前のマーク～Ｔ字
kamorei 4:edec6fe32ba9 243 set_sensor();
kamorei 4:edec6fe32ba9 244 if( sensor[0] <= blackL && sensor[3] <= blackR){
kamorei 7:f1f9bc420dba 245 turn_right_corner( 0.2);
kamorei 4:edec6fe32ba9 246 break;
kamorei 4:edec6fe32ba9 247 } else
kamorei 4:edec6fe32ba9 248 go_straight_p();
kamorei 4:edec6fe32ba9 249 }
kamorei 4:edec6fe32ba9 250 LED_blinky();
kamorei 4:edec6fe32ba9 251 #ifdef graychange
kamorei 4:edec6fe32ba9 252 i = 0;
kamorei 4:edec6fe32ba9 253 #endif
kamorei 7:f1f9bc420dba 254 while(1){ //Ｔ字～半円～鋭角カーブ
kamorei 4:edec6fe32ba9 255 set_sensor();
kamorei 4:edec6fe32ba9 256 #ifdef graychange
kamorei 4:edec6fe32ba9 257 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 258 #endif
kamorei 4:edec6fe32ba9 259 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 260 turn_left_corner( 0.5);
kamorei 4:edec6fe32ba9 261 break;
kamorei 4:edec6fe32ba9 262 } else
kamorei 7:f1f9bc420dba 263 go_straight_CR();
kamorei 4:edec6fe32ba9 264 }
kamorei 4:edec6fe32ba9 265 LED_blinky();
kamorei 7:f1f9bc420dba 266 while(1){ //鋭角カーブ終わり～ゴール
kamorei 4:edec6fe32ba9 267 set_sensor();
kamorei 4:edec6fe32ba9 268 if( sensor[0] <= blackL)
kamorei 7:f1f9bc420dba 269 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 270 else if( sensor[3] <= blackR)
kamorei 7:f1f9bc420dba 271 turn_right_corner( 0.2);
kamorei 7:f1f9bc420dba 272 else if( sensor[0] <= grayL[0] && sensor[3] <= grayR[0]){
kamorei 4:edec6fe32ba9 273 stop_point_ver2();
kamorei 4:edec6fe32ba9 274 break;
kamorei 4:edec6fe32ba9 275 } else
kamorei 4:edec6fe32ba9 276 go_straight_p();
kamorei 4:edec6fe32ba9 277 }
kamorei 4:edec6fe32ba9 278 LED_blinky();
kamorei 5:30251ab9313a 279 }