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@11:99b63117ffdd, 2018-11-26 (annotated)

Committer:
kamorei
Date:
Mon Nov 26 05:20:20 2018 +0000
Revision:
11:99b63117ffdd
Parent:
10:80bac0698b04 
11/26 ????????

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