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

Committer:
kamorei
Date:
Wed Nov 21 07:10:01 2018 +0000
Revision:
9:d989333191cb
Parent:
8:b47cebb6aeb8
Child:
10:80bac0698b04
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 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 9:d989333191cb 23 float whiteCL = 0.27, grayCL = 0.15, blackCL = 0.03;
kamorei 9:d989333191cb 24 float whiteCR = 0.235, grayCR = 0.13, blackCR = 0.03;
kamorei 9:d989333191cb 25 float whiteL = 0.1, grayL = 0.06, blackL = 0.018;
kamorei 9:d989333191cb 26 float whiteR = 0.1, grayR = 0.06, blackR = 0.018;
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 0:500d22d1efeb 30 void stop_point_ver2();
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 0:500d22d1efeb 93 stop_point_ver2();
kamorei 7:f1f9bc420dba 94 // turn_left();
kamorei 7:f1f9bc420dba 95 // wait(0.3);
kamorei 0:500d22d1efeb 96 }
kamorei 0:500d22d1efeb 97
kamorei 7:f1f9bc420dba 98 void turn_left_corner( float f){
kamorei 3:e8c29cd3ca22 99 motor_check();
kamorei 7:f1f9bc420dba 100 wait(f);
kamorei 7:f1f9bc420dba 101 sensor[0] = sensorL.read();
kamorei 9:d989333191cb 102 while( sensor[0] >= blackL){
kamorei 3:e8c29cd3ca22 103 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 104 turn_left();
kamorei 0:500d22d1efeb 105 }
kamorei 0:500d22d1efeb 106 stop_point_ver2();
kamorei 7:f1f9bc420dba 107 // turn_right();
kamorei 7:f1f9bc420dba 108 // wait(0.3);
kamorei 0:500d22d1efeb 109 }
kamorei 0:500d22d1efeb 110
kamorei 4:edec6fe32ba9 111 void motor_check(){ //モータドライバの調子の確認用と単純に直進
kamorei 2:47477c2b1925 112 Mrighti = 2;
kamorei 0:500d22d1efeb 113 Mrightp = 1.0f;
kamorei 2:47477c2b1925 114 Mlefti = 2;
kamorei 0:500d22d1efeb 115 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 116 }
kamorei 0:500d22d1efeb 117
kamorei 0:500d22d1efeb 118 void stop_point_ver2(){
kamorei 0:500d22d1efeb 119 if( Mrighti == 1)
kamorei 0:500d22d1efeb 120 Mrighti = 2;
kamorei 0:500d22d1efeb 121 else if( Mrighti == 2)
kamorei 0:500d22d1efeb 122 Mrighti = 1;
kamorei 0:500d22d1efeb 123 if( Mlefti == 1)
kamorei 0:500d22d1efeb 124 Mlefti = 2;
kamorei 0:500d22d1efeb 125 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 126 Mlefti = 1;
kamorei 4:edec6fe32ba9 127 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 128 Mleftp = 1.0f;
kamorei 3:e8c29cd3ca22 129 wait(0.1);
kamorei 0:500d22d1efeb 130 Mrighti = 0;
kamorei 0:500d22d1efeb 131 Mlefti = 0;
kamorei 0:500d22d1efeb 132 ledRR = 0b11;
kamorei 0:500d22d1efeb 133 ledLL = 0b11;
kamorei 0:500d22d1efeb 134 }
kamorei 0:500d22d1efeb 135
kamorei 4:edec6fe32ba9 136 void LED_blinky(){ //プログラムの切り替わり確認用
kamorei 4:edec6fe32ba9 137 for( int i = 0; i < 4; i++){
kamorei 4:edec6fe32ba9 138 ledCount = !ledCount;
kamorei 5:30251ab9313a 139 wait(0.05);
kamorei 3:e8c29cd3ca22 140 }
kamorei 3:e8c29cd3ca22 141 }
kamorei 3:e8c29cd3ca22 142
kamorei 0:500d22d1efeb 143 int main() {
kamorei 4:edec6fe32ba9 144 ledCheck = 1;
kamorei 4:edec6fe32ba9 145 LED_blinky();
kamorei 4:edec6fe32ba9 146 while(1){ //スタート～最初の左折カーブ
kamorei 4:edec6fe32ba9 147 set_sensor();
kamorei 4:edec6fe32ba9 148 if( sensor[3] <= blackR){
kamorei 7:f1f9bc420dba 149 turn_right_corner( 0.2);
kamorei 4:edec6fe32ba9 150 break;
kamorei 4:edec6fe32ba9 151 } else
kamorei 4:edec6fe32ba9 152 go_straight_p();
kamorei 4:edec6fe32ba9 153 }
kamorei 9:d989333191cb 154 #ifdef blinky
kamorei 4:edec6fe32ba9 155 LED_blinky();
kamorei 9:d989333191cb 156 #endif
kamorei 4:edec6fe32ba9 157 while(1){ //左折カーブ～段差～左折カーブ
kamorei 4:edec6fe32ba9 158 set_sensor();
kamorei 4:edec6fe32ba9 159 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 160 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 161 break;
kamorei 4:edec6fe32ba9 162 } else
kamorei 7:f1f9bc420dba 163 go_straight_p();
kamorei 4:edec6fe32ba9 164 }
kamorei 9:d989333191cb 165 #ifdef blinky
kamorei 4:edec6fe32ba9 166 LED_blinky();
kamorei 4:edec6fe32ba9 167 #endif
kamorei 4:edec6fe32ba9 168 while(1){ //左折カーブ～左折カーブ
kamorei 4:edec6fe32ba9 169 set_sensor();
kamorei 4:edec6fe32ba9 170 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 171 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 172 break;
kamorei 4:edec6fe32ba9 173 } else
kamorei 4:edec6fe32ba9 174 go_straight_p();
kamorei 4:edec6fe32ba9 175 }
kamorei 9:d989333191cb 176 #ifdef blinky
kamorei 4:edec6fe32ba9 177 LED_blinky();
kamorei 9:d989333191cb 178 #endif
kamorei 4:edec6fe32ba9 179 while(1){ //左折カーブ～トンネル手前のマーク
kamorei 4:edec6fe32ba9 180 set_sensor();
kamorei 9:d989333191cb 181 if( sensor[0] <= grayL && sensor[3] <= grayR)
kamorei 3:e8c29cd3ca22 182 break;
kamorei 4:edec6fe32ba9 183 else
kamorei 4:edec6fe32ba9 184 go_straight_CR();
kamorei 4:edec6fe32ba9 185 }
kamorei 9:d989333191cb 186 #ifdef blinky
kamorei 4:edec6fe32ba9 187 LED_blinky();
kamorei 9:d989333191cb 188 #endif
kamorei 4:edec6fe32ba9 189 while(1){ //トンネル手前のマーク～Ｔ字
kamorei 4:edec6fe32ba9 190 set_sensor();
kamorei 4:edec6fe32ba9 191 if( sensor[0] <= blackL && sensor[3] <= blackR){
kamorei 7:f1f9bc420dba 192 turn_right_corner( 0.2);
kamorei 4:edec6fe32ba9 193 break;
kamorei 4:edec6fe32ba9 194 } else
kamorei 4:edec6fe32ba9 195 go_straight_p();
kamorei 4:edec6fe32ba9 196 }
kamorei 9:d989333191cb 197 #ifdef blinky
kamorei 4:edec6fe32ba9 198 LED_blinky();
kamorei 4:edec6fe32ba9 199 #endif
kamorei 7:f1f9bc420dba 200 while(1){ //Ｔ字～半円～鋭角カーブ
kamorei 4:edec6fe32ba9 201 set_sensor();
kamorei 4:edec6fe32ba9 202 if( sensor[0] <= blackL){
kamorei 7:f1f9bc420dba 203 turn_left_corner( 0.5);
kamorei 4:edec6fe32ba9 204 break;
kamorei 4:edec6fe32ba9 205 } else
kamorei 7:f1f9bc420dba 206 go_straight_CR();
kamorei 4:edec6fe32ba9 207 }
kamorei 9:d989333191cb 208 #ifdef blinky
kamorei 4:edec6fe32ba9 209 LED_blinky();
kamorei 9:d989333191cb 210 #endif
kamorei 7:f1f9bc420dba 211 while(1){ //鋭角カーブ終わり～ゴール
kamorei 4:edec6fe32ba9 212 set_sensor();
kamorei 4:edec6fe32ba9 213 if( sensor[0] <= blackL)
kamorei 7:f1f9bc420dba 214 turn_left_corner( 0.2);
kamorei 4:edec6fe32ba9 215 else if( sensor[3] <= blackR)
kamorei 7:f1f9bc420dba 216 turn_right_corner( 0.2);
kamorei 9:d989333191cb 217 else if( sensor[0] <= grayL && sensor[3] <= grayR){
kamorei 4:edec6fe32ba9 218 stop_point_ver2();
kamorei 4:edec6fe32ba9 219 break;
kamorei 4:edec6fe32ba9 220 } else
kamorei 4:edec6fe32ba9 221 go_straight_p();
kamorei 4:edec6fe32ba9 222 }
kamorei 9:d989333191cb 223 #ifdef blinky
kamorei 4:edec6fe32ba9 224 LED_blinky();
kamorei 9:d989333191cb 225 #endif
kamorei 5:30251ab9313a 226 }