Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termB_test

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@2:8748af75523f, 2018-10-30 (annotated)

Committer:
kamorei
Date:
Tue Oct 30 06:24:21 2018 +0000
Revision:
2:8748af75523f
Parent:
1:905436937f78
Child:
3:6a42c3c3d816
10/30 ????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 1:905436937f78 1 /*
kamorei 1:905436937f78 2 タームB作戦（プログラムの流れ）
kamorei 1:905436937f78 3 ・AからスタートしてB（トンネル手前）で一旦停止
kamorei 1:905436937f78 4 ・閾値の再定義（トンネル内の明るさに）←あらかじめ用意しておいて切り替えてもいい?
kamorei 1:905436937f78 5 ・トンネル抜けた地点で再定義（ゴール判定用が急激に変化したら再定義か？）
kamorei 1:905436937f78 6 ・あとは流れでいいかな？
kamorei 1:905436937f78 7 */
kamorei 0:8b8f5dba70e9 8
kamorei 0:8b8f5dba70e9 9 #include "mbed.h"
kamorei 0:8b8f5dba70e9 10
kamorei 0:8b8f5dba70e9 11 DigitalOut ledL( PTB8);
kamorei 0:8b8f5dba70e9 12 DigitalOut ledR( PTE5);
kamorei 0:8b8f5dba70e9 13 BusOut ledLL( PTB8, PTB9);
kamorei 0:8b8f5dba70e9 14 BusOut ledRR( PTE5, PTE4); //わざと右から書いてます
kamorei 0:8b8f5dba70e9 15
kamorei 0:8b8f5dba70e9 16 AnalogIn sensorR( PTB1);
kamorei 0:8b8f5dba70e9 17 AnalogIn sensorL( PTB3);
kamorei 0:8b8f5dba70e9 18 AnalogIn sensorCR( PTB0);
kamorei 0:8b8f5dba70e9 19 AnalogIn sensorCL( PTB2);
kamorei 0:8b8f5dba70e9 20
kamorei 0:8b8f5dba70e9 21 //モータ1
kamorei 0:8b8f5dba70e9 22 BusOut Mlefti(PTA1, PTA2);
kamorei 0:8b8f5dba70e9 23 PwmOut Mleftp(PTD4);
kamorei 0:8b8f5dba70e9 24 //モータ2
kamorei 0:8b8f5dba70e9 25 BusOut Mrighti(PTC0, PTC7);
kamorei 0:8b8f5dba70e9 26 PwmOut Mrightp(PTA12);
kamorei 0:8b8f5dba70e9 27
kamorei 2:8748af75523f 28 float white = 0.6, black = 0.08, gray = 0.3; //閾値（再定義必須）
kamorei 0:8b8f5dba70e9 29 float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
kamorei 2:8748af75523f 30 float white_tunnel, black_tunnel, gray_tunnel;
kamorei 0:8b8f5dba70e9 31 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 1:905436937f78 32 float kp = 0.3; //Pゲイン
kamorei 0:8b8f5dba70e9 33 float pr, pl;
kamorei 0:8b8f5dba70e9 34 int i = 0, j = 0;
kamorei 0:8b8f5dba70e9 35
kamorei 0:8b8f5dba70e9 36 void turn_right(){
kamorei 0:8b8f5dba70e9 37 Mlefti = 2;
kamorei 0:8b8f5dba70e9 38 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 39 Mrighti = 1;
kamorei 0:8b8f5dba70e9 40 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 41 ledR = 1;
kamorei 0:8b8f5dba70e9 42 ledL = 0;
kamorei 0:8b8f5dba70e9 43 }
kamorei 0:8b8f5dba70e9 44
kamorei 0:8b8f5dba70e9 45 void turn_left(){
kamorei 0:8b8f5dba70e9 46 Mrighti = 2;
kamorei 0:8b8f5dba70e9 47 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 48 Mlefti = 1;
kamorei 0:8b8f5dba70e9 49 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 50 ledR = 0;
kamorei 0:8b8f5dba70e9 51 ledL = 1;
kamorei 0:8b8f5dba70e9 52 }
kamorei 0:8b8f5dba70e9 53
kamorei 1:905436937f78 54 void go_straight(){
kamorei 0:8b8f5dba70e9 55 Mrighti = 2;
kamorei 0:8b8f5dba70e9 56 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 57 Mlefti = 2;
kamorei 0:8b8f5dba70e9 58 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 59 ledR = 0;
kamorei 0:8b8f5dba70e9 60 ledL = 0;
kamorei 0:8b8f5dba70e9 61 }
kamorei 0:8b8f5dba70e9 62
kamorei 1:905436937f78 63 void go_straight_p(){
kamorei 1:905436937f78 64 if( pr < (gray - black) / (white - black)){
kamorei 1:905436937f78 65 Mrighti = 1;
kamorei 1:905436937f78 66 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 67 Mlefti = 2;
kamorei 1:905436937f78 68 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 1:905436937f78 69 ledR = 1;
kamorei 1:905436937f78 70 ledL = 0;
kamorei 1:905436937f78 71 } else if( pl < (gray - black) / (white - black)){
kamorei 1:905436937f78 72 Mrighti = 2;
kamorei 1:905436937f78 73 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 74 Mlefti = 1;
kamorei 1:905436937f78 75 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 1:905436937f78 76 ledR = 0;
kamorei 1:905436937f78 77 ledL = 1;
kamorei 1:905436937f78 78 } else{
kamorei 1:905436937f78 79 Mrighti = 2;
kamorei 1:905436937f78 80 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 81 Mlefti = 2;
kamorei 1:905436937f78 82 Mleftp = (kp * pl - 0.1) * 1.0f;
kamorei 1:905436937f78 83 ledR = 0;
kamorei 1:905436937f78 84 ledL = 0;
kamorei 1:905436937f78 85 }
kamorei 1:905436937f78 86 }
kamorei 1:905436937f78 87
kamorei 2:8748af75523f 88 void go_straight_p_tunnel(){
kamorei 2:8748af75523f 89 if( pr < (gray_tunnel - black_tunnel) / (white_tunnel - black_tunnel)){
kamorei 2:8748af75523f 90 Mrighti = 1;
kamorei 2:8748af75523f 91 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 2:8748af75523f 92 Mlefti = 2;
kamorei 2:8748af75523f 93 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 2:8748af75523f 94 ledR = 1;
kamorei 2:8748af75523f 95 ledL = 0;
kamorei 2:8748af75523f 96 } else if( pl < (gray_tunnel - black_tunnel) / (white_tunnel - black_tunnel)){
kamorei 2:8748af75523f 97 Mrighti = 2;
kamorei 2:8748af75523f 98 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 2:8748af75523f 99 Mlefti = 1;
kamorei 2:8748af75523f 100 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 2:8748af75523f 101 ledR = 0;
kamorei 2:8748af75523f 102 ledL = 1;
kamorei 2:8748af75523f 103 } else{
kamorei 2:8748af75523f 104 Mrighti = 2;
kamorei 2:8748af75523f 105 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 2:8748af75523f 106 Mlefti = 2;
kamorei 2:8748af75523f 107 Mleftp = (kp * pl - 0.1) * 1.0f;
kamorei 2:8748af75523f 108 ledR = 0;
kamorei 2:8748af75523f 109 ledL = 0;
kamorei 2:8748af75523f 110 }
kamorei 2:8748af75523f 111 }
kamorei 2:8748af75523f 112
kamorei 1:905436937f78 113 void go_straight_check(){ //モータドライバの調子の確認用
kamorei 1:905436937f78 114 Mrighti = 2;
kamorei 1:905436937f78 115 Mrightp = 0.8f;
kamorei 1:905436937f78 116 Mlefti = 2;
kamorei 1:905436937f78 117 Mleftp = 0.8f;
kamorei 1:905436937f78 118 }
kamorei 1:905436937f78 119
kamorei 1:905436937f78 120 void go_back(){
kamorei 0:8b8f5dba70e9 121 Mrighti = 1;
kamorei 0:8b8f5dba70e9 122 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 123 Mlefti = 1;
kamorei 0:8b8f5dba70e9 124 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 125 ledR = 0;
kamorei 0:8b8f5dba70e9 126 ledL = 0;
kamorei 0:8b8f5dba70e9 127 }
kamorei 0:8b8f5dba70e9 128
kamorei 0:8b8f5dba70e9 129 void stop_point(){
kamorei 1:905436937f78 130 Mrighti = 1;
kamorei 1:905436937f78 131 Mrightp = 0.5f;
kamorei 1:905436937f78 132 Mlefti = 1;
kamorei 1:905436937f78 133 Mleftp = 0.5f;
kamorei 1:905436937f78 134 wait(0.05);
kamorei 0:8b8f5dba70e9 135 Mrighti = 0;
kamorei 0:8b8f5dba70e9 136 Mlefti = 0;
kamorei 0:8b8f5dba70e9 137 ledRR = 2;
kamorei 0:8b8f5dba70e9 138 ledLL = 2;
kamorei 0:8b8f5dba70e9 139 }
kamorei 0:8b8f5dba70e9 140
kamorei 2:8748af75523f 141 void stop_turn_right(){
kamorei 2:8748af75523f 142 Mlefti = 1;
kamorei 2:8748af75523f 143 Mleftp = 0.7f;
kamorei 2:8748af75523f 144 Mrighti = 2;
kamorei 2:8748af75523f 145 Mrightp = 0.7f;
kamorei 2:8748af75523f 146 wait(0.05);
kamorei 2:8748af75523f 147 Mrighti = 0;
kamorei 2:8748af75523f 148 Mlefti = 0;
kamorei 2:8748af75523f 149 ledRR = 2;
kamorei 2:8748af75523f 150 ledLL = 2;
kamorei 2:8748af75523f 151 }
kamorei 2:8748af75523f 152
kamorei 2:8748af75523f 153 void stop_turn_left(){
kamorei 2:8748af75523f 154 Mlefti = 2;
kamorei 2:8748af75523f 155 Mleftp = 0.7f;
kamorei 2:8748af75523f 156 Mrighti = 1;
kamorei 2:8748af75523f 157 Mrightp = 0.7f;
kamorei 2:8748af75523f 158 wait(0.05);
kamorei 2:8748af75523f 159 Mrighti = 0;
kamorei 2:8748af75523f 160 Mlefti = 0;
kamorei 2:8748af75523f 161 ledRR = 2;
kamorei 2:8748af75523f 162 ledLL = 2;
kamorei 2:8748af75523f 163 }
kamorei 2:8748af75523f 164
kamorei 0:8b8f5dba70e9 165 int main() {
kamorei 1:905436937f78 166 go_straight_check();
kamorei 0:8b8f5dba70e9 167 wait(0.2);
kamorei 2:8748af75523f 168 while(i < 2){
kamorei 0:8b8f5dba70e9 169 while(1) {
kamorei 0:8b8f5dba70e9 170 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 171 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 172 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 173 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 174
kamorei 0:8b8f5dba70e9 175 pr = (sensor[2] - black) / (white - black);
kamorei 0:8b8f5dba70e9 176 pl = (sensor[1] - black) / (white - black);
kamorei 2:8748af75523f 177
kamorei 2:8748af75523f 178 go_straight_p();
kamorei 0:8b8f5dba70e9 179 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 0:8b8f5dba70e9 180 stop_point();
kamorei 2:8748af75523f 181 i++;
kamorei 0:8b8f5dba70e9 182 break;
kamorei 0:8b8f5dba70e9 183 }
kamorei 0:8b8f5dba70e9 184 }
kamorei 2:8748af75523f 185 //以下トンネル内部
kamorei 1:905436937f78 186 if( i < 2){
kamorei 2:8748af75523f 187 turn_right();
kamorei 2:8748af75523f 188 wait(0.2);
kamorei 2:8748af75523f 189 while(1){
kamorei 2:8748af75523f 190 sensor[3] = sensorR.read();
kamorei 2:8748af75523f 191 turn_right();
kamorei 2:8748af75523f 192 if( sensor[3] <= blackR){
kamorei 2:8748af75523f 193 stop_turn_right();
kamorei 2:8748af75523f 194 break;
kamorei 2:8748af75523f 195 }
kamorei 2:8748af75523f 196 }
kamorei 2:8748af75523f 197 black_tunnel = sensorL.read();
kamorei 2:8748af75523f 198 turn_left();
kamorei 2:8748af75523f 199 wait(0.1);
kamorei 2:8748af75523f 200
kamorei 2:8748af75523f 201 stop_turn_left();
kamorei 2:8748af75523f 202 wait(0.5);
kamorei 2:8748af75523f 203
kamorei 2:8748af75523f 204 white_tunnel = sensorL.read();
kamorei 2:8748af75523f 205 gray_tunnel = (white_tunnel - black_tunnel) / 2;
kamorei 2:8748af75523f 206 while(1){ //姿勢を直す
kamorei 2:8748af75523f 207 sensor[3] = sensorR.read();
kamorei 2:8748af75523f 208 turn_left();
kamorei 2:8748af75523f 209 if( sensor[3] <= blackR){
kamorei 2:8748af75523f 210 stop_turn_left();
kamorei 2:8748af75523f 211 break;
kamorei 2:8748af75523f 212 }
kamorei 2:8748af75523f 213 }
kamorei 2:8748af75523f 214 while(1){
kamorei 0:8b8f5dba70e9 215 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 216 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 217 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 218 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 219
kamorei 2:8748af75523f 220 pr = (sensor[2] - black_tunnel) / (white_tunnel - black_tunnel);
kamorei 2:8748af75523f 221 pl = (sensor[1] - black_tunnel) / (white_tunnel - black_tunnel);
kamorei 2:8748af75523f 222
kamorei 2:8748af75523f 223 go_straight_p_tunnel();
kamorei 0:8b8f5dba70e9 224
kamorei 2:8748af75523f 225 if( sensor[1] > white || sensor[2] > white){
kamorei 0:8b8f5dba70e9 226 stop_point();
kamorei 2:8748af75523f 227 ledRR = 0b11; //←２進数表記使える…？
kamorei 2:8748af75523f 228 wait(0.5);
kamorei 0:8b8f5dba70e9 229 break;
kamorei 0:8b8f5dba70e9 230 }
kamorei 0:8b8f5dba70e9 231 }
kamorei 0:8b8f5dba70e9 232 }
kamorei 0:8b8f5dba70e9 233 }
kamorei 0:8b8f5dba70e9 234 }