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@5:30251ab9313a, 2018-11-20 (annotated)

Committer:
kamorei
Date:
Tue Nov 20 18:51:23 2018 +0000
Revision:
5:30251ab9313a
Parent:
4:edec6fe32ba9
Child:
6:28c5a5992a81
11/21 fu*kin' cold 2

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