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@2:47477c2b1925, 2018-11-16 (annotated)

Committer:
kamorei
Date:
Fri Nov 16 07:12:57 2018 +0000
Revision:
2:47477c2b1925
Parent:
1:5c26d3744592
Child:
3:e8c29cd3ca22
11/16 ?

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 #define CHG 1.0 //change_gray用の係数
kamorei 2:47477c2b1925 5 #define WHITE_CL 0.92 //初期値
kamorei 2:47477c2b1925 6 #define WHITE_CR 0.70 //初期値
kamorei 2:47477c2b1925 7 #define GRAY_CL 0.46 //初期値
kamorei 2:47477c2b1925 8 #define GRAY_CR 0.5 //初期値
kamorei 2:47477c2b1925 9 #define GRAY_L 0.28 //初期値
kamorei 2:47477c2b1925 10 #define GRAY_R 0.28 //初期値
kamorei 0:500d22d1efeb 11
kamorei 0:500d22d1efeb 12 #define DEBUG
kamorei 0:500d22d1efeb 13
kamorei 0:500d22d1efeb 14 DigitalOut ledL( PTB8);
kamorei 0:500d22d1efeb 15 DigitalOut ledR( PTE5);
kamorei 0:500d22d1efeb 16 DigitalOut ledC( PTE2); //動作check
kamorei 0:500d22d1efeb 17 BusOut ledLL( PTB8, PTB9);
kamorei 0:500d22d1efeb 18 BusOut ledRR( PTE4, PTE5);
kamorei 0:500d22d1efeb 19 AnalogIn sensorR( PTB1);
kamorei 0:500d22d1efeb 20 AnalogIn sensorL( PTB3);
kamorei 0:500d22d1efeb 21 AnalogIn sensorCR( PTB0);
kamorei 0:500d22d1efeb 22 AnalogIn sensorCL( PTB2);
kamorei 0:500d22d1efeb 23 //モータ1
kamorei 0:500d22d1efeb 24 BusOut Mlefti(PTA1, PTA2);
kamorei 0:500d22d1efeb 25 PwmOut Mleftp(PTD4);
kamorei 0:500d22d1efeb 26 //モータ2
kamorei 0:500d22d1efeb 27 BusOut Mrighti(PTC0, PTC7);
kamorei 0:500d22d1efeb 28 PwmOut Mrightp(PTA12);
kamorei 0:500d22d1efeb 29
kamorei 0:500d22d1efeb 30 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
kamorei 0:500d22d1efeb 31 float grayL[2], grayR[2];
kamorei 2:47477c2b1925 32 float blackCL = 0.18; //閾値の跡地
kamorei 0:500d22d1efeb 33 float blackCR = 0.3; //↑と同じ
kamorei 2:47477c2b1925 34 float whiteL = 0.35, blackL = 0.23;
kamorei 2:47477c2b1925 35 float whiteR = 0.35, blackR = 0.23;
kamorei 0:500d22d1efeb 36 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:500d22d1efeb 37 float pr, pl;
kamorei 0:500d22d1efeb 38
kamorei 0:500d22d1efeb 39 void stop_point_ver2();
kamorei 0:500d22d1efeb 40
kamorei 0:500d22d1efeb 41 void set_threshold(){ //閾値の初期設定
kamorei 0:500d22d1efeb 42 for( int i = 0; i < 2; i++){
kamorei 0:500d22d1efeb 43 whiteCL[i] = WHITE_CL;
kamorei 0:500d22d1efeb 44 whiteCR[i] = WHITE_CR;
kamorei 0:500d22d1efeb 45 grayCL[i] = GRAY_CL;
kamorei 0:500d22d1efeb 46 grayCR[i] = GRAY_CR;
kamorei 0:500d22d1efeb 47 grayL[i] = GRAY_L;
kamorei 0:500d22d1efeb 48 grayR[i] = GRAY_R;
kamorei 0:500d22d1efeb 49 }
kamorei 0:500d22d1efeb 50 }
kamorei 0:500d22d1efeb 51
kamorei 0:500d22d1efeb 52 void go_straight_p(){ //P制御でトレース
kamorei 2:47477c2b1925 53 Mrighti = 2;
kamorei 0:500d22d1efeb 54 Mrightp = (KP * pr) * 1.0f;
kamorei 2:47477c2b1925 55 Mlefti = 2;
kamorei 0:500d22d1efeb 56 Mleftp = (KP * pl) * 1.0f;
kamorei 0:500d22d1efeb 57 ledR = 1;
kamorei 0:500d22d1efeb 58 ledL = 1;
kamorei 0:500d22d1efeb 59 }
kamorei 0:500d22d1efeb 60
kamorei 0:500d22d1efeb 61 void go_straight_CR(){ //CRのみでトレース
kamorei 2:47477c2b1925 62 Mrighti = 2;
kamorei 0:500d22d1efeb 63 Mrightp = (KP * pr + 0.3) * 1.0f;
kamorei 2:47477c2b1925 64 Mlefti = 2;
kamorei 0:500d22d1efeb 65 Mleftp = (1.3 - KP * pr) * 1.0f;
kamorei 0:500d22d1efeb 66 ledRR = 0b11;
kamorei 0:500d22d1efeb 67 ledLL = 0b10;
kamorei 0:500d22d1efeb 68 }
kamorei 0:500d22d1efeb 69
kamorei 1:5c26d3744592 70 void go_straight_CL(){ //CLのみでトレース
kamorei 2:47477c2b1925 71 Mrighti = 2;
kamorei 1:5c26d3744592 72 Mrightp = (1.3 - KP * pl) * 1.0f;
kamorei 2:47477c2b1925 73 Mlefti = 2;
kamorei 1:5c26d3744592 74 Mleftp = (KP * pl + 0.3) * 1.0f;
kamorei 1:5c26d3744592 75 ledRR = 0b10;
kamorei 1:5c26d3744592 76 ledLL = 0b11;
kamorei 1:5c26d3744592 77 }
kamorei 1:5c26d3744592 78
kamorei 0:500d22d1efeb 79 void turn_right(){
kamorei 2:47477c2b1925 80 Mrighti = 1;
kamorei 0:500d22d1efeb 81 Mrightp = 0.05f;
kamorei 2:47477c2b1925 82 Mlefti = 2;
kamorei 0:500d22d1efeb 83 Mleftp = 0.1f;
kamorei 0:500d22d1efeb 84 ledR = 1;
kamorei 0:500d22d1efeb 85 ledL = 0;
kamorei 0:500d22d1efeb 86 }
kamorei 0:500d22d1efeb 87
kamorei 0:500d22d1efeb 88 void turn_left(){
kamorei 2:47477c2b1925 89 Mrighti = 2;
kamorei 0:500d22d1efeb 90 Mrightp = 0.1f;
kamorei 2:47477c2b1925 91 Mlefti = 1;
kamorei 0:500d22d1efeb 92 Mleftp = 0.05f;
kamorei 0:500d22d1efeb 93 ledR = 0;
kamorei 0:500d22d1efeb 94 ledL = 1;
kamorei 0:500d22d1efeb 95 }
kamorei 0:500d22d1efeb 96
kamorei 0:500d22d1efeb 97 void turn_right_ver2(){
kamorei 0:500d22d1efeb 98 while( sensor[0] > grayL[0]){ //sensorL > grayL[0]
kamorei 0:500d22d1efeb 99 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 100 turn_right();
kamorei 0:500d22d1efeb 101 }
kamorei 0:500d22d1efeb 102 stop_point_ver2();
kamorei 0:500d22d1efeb 103 while( sensor[2] > grayCR[0]){ //補正
kamorei 0:500d22d1efeb 104 sensor[2] = sensorCR.read();
kamorei 0:500d22d1efeb 105 turn_left();
kamorei 0:500d22d1efeb 106 }
kamorei 0:500d22d1efeb 107 stop_point_ver2();
kamorei 0:500d22d1efeb 108 }
kamorei 0:500d22d1efeb 109
kamorei 0:500d22d1efeb 110 void turn_left_ver2(){
kamorei 0:500d22d1efeb 111 while( sensor[3] > grayR[0]){ //sensorR > grayR[0]
kamorei 0:500d22d1efeb 112 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 113 turn_left();
kamorei 0:500d22d1efeb 114 }
kamorei 0:500d22d1efeb 115 stop_point_ver2();
kamorei 0:500d22d1efeb 116 while( sensor[1] > grayCL[0]){ //補正
kamorei 0:500d22d1efeb 117 sensor[1] = sensorCL.read();
kamorei 0:500d22d1efeb 118 turn_right();
kamorei 0:500d22d1efeb 119 }
kamorei 0:500d22d1efeb 120 stop_point_ver2();
kamorei 0:500d22d1efeb 121 }
kamorei 0:500d22d1efeb 122
kamorei 0:500d22d1efeb 123 void motor_check(){ //モータドライバの調子の確認用
kamorei 2:47477c2b1925 124 Mrighti = 2;
kamorei 0:500d22d1efeb 125 Mrightp = 1.0f;
kamorei 2:47477c2b1925 126 Mlefti = 2;
kamorei 0:500d22d1efeb 127 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 128 }
kamorei 0:500d22d1efeb 129
kamorei 0:500d22d1efeb 130 void stop_point(){
kamorei 2:47477c2b1925 131 Mrighti = 1;
kamorei 0:500d22d1efeb 132 Mrightp = 1.0f;
kamorei 2:47477c2b1925 133 Mlefti = 1;
kamorei 0:500d22d1efeb 134 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 135 wait(0.05);
kamorei 0:500d22d1efeb 136 Mrighti = 0;
kamorei 0:500d22d1efeb 137 Mlefti = 0;
kamorei 0:500d22d1efeb 138 ledRR = 0b11;
kamorei 0:500d22d1efeb 139 ledLL = 0b11;
kamorei 0:500d22d1efeb 140 }
kamorei 0:500d22d1efeb 141
kamorei 0:500d22d1efeb 142 void stop_point_ver2(){
kamorei 0:500d22d1efeb 143 if( Mrighti == 1)
kamorei 0:500d22d1efeb 144 Mrighti = 2;
kamorei 0:500d22d1efeb 145 else if( Mrighti == 2)
kamorei 0:500d22d1efeb 146 Mrighti = 1;
kamorei 0:500d22d1efeb 147 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 148 if( Mlefti == 1)
kamorei 0:500d22d1efeb 149 Mlefti = 2;
kamorei 0:500d22d1efeb 150 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 151 Mlefti = 1;
kamorei 0:500d22d1efeb 152 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 153 wait(0.05);
kamorei 0:500d22d1efeb 154 Mrighti = 0;
kamorei 0:500d22d1efeb 155 Mlefti = 0;
kamorei 0:500d22d1efeb 156 ledRR = 0b11;
kamorei 0:500d22d1efeb 157 ledLL = 0b11;
kamorei 0:500d22d1efeb 158 }
kamorei 0:500d22d1efeb 159
kamorei 0:500d22d1efeb 160 void change_gray(){
kamorei 0:500d22d1efeb 161 if( sensor[1] <= grayCL[0]){ //sensorCL <=grayCL
kamorei 0:500d22d1efeb 162 whiteCR[0] = sensorCR.read();
kamorei 0:500d22d1efeb 163 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 164 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 165 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 166 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 167 ledRR = 0b10;
kamorei 0:500d22d1efeb 168 ledLL = 0;
kamorei 0:500d22d1efeb 169 }
kamorei 0:500d22d1efeb 170 else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
kamorei 0:500d22d1efeb 171 whiteCL[0] = sensorCL.read();
kamorei 0:500d22d1efeb 172 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 173 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 174 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 175 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 176 ledRR = 0;
kamorei 0:500d22d1efeb 177 ledLL = 0b10;
kamorei 0:500d22d1efeb 178 }
kamorei 0:500d22d1efeb 179 }
kamorei 0:500d22d1efeb 180
kamorei 0:500d22d1efeb 181 int main() {
kamorei 0:500d22d1efeb 182 ledC = 1;
kamorei 0:500d22d1efeb 183 set_threshold();
kamorei 0:500d22d1efeb 184 motor_check();
kamorei 2:47477c2b1925 185 wait(5.0);
kamorei 0:500d22d1efeb 186 while(1) {
kamorei 0:500d22d1efeb 187 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 188 sensor[1] = sensorCL.read();
kamorei 0:500d22d1efeb 189 sensor[2] = sensorCR.read();
kamorei 0:500d22d1efeb 190 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 191
kamorei 0:500d22d1efeb 192 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 0:500d22d1efeb 193 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 0:500d22d1efeb 194
kamorei 0:500d22d1efeb 195 if( sensor[0] <= grayL[0]){
kamorei 0:500d22d1efeb 196 turn_left_ver2();
kamorei 0:500d22d1efeb 197 change_gray();
kamorei 0:500d22d1efeb 198 #ifdef DEBUG
kamorei 0:500d22d1efeb 199 break;
kamorei 0:500d22d1efeb 200 #endif
kamorei 0:500d22d1efeb 201 } else if( sensor[3] <= grayR[0]){
kamorei 0:500d22d1efeb 202 turn_right_ver2();
kamorei 0:500d22d1efeb 203 change_gray();
kamorei 0:500d22d1efeb 204 #ifdef DEBUG
kamorei 0:500d22d1efeb 205 break;
kamorei 0:500d22d1efeb 206 #endif
kamorei 0:500d22d1efeb 207 } else
kamorei 0:500d22d1efeb 208 go_straight_p();
kamorei 1:5c26d3744592 209 // go_straight_CR();
kamorei 1:5c26d3744592 210 // go_straight_CL();
kamorei 1:5c26d3744592 211
kamorei 0:500d22d1efeb 212 if( sensor[0] < blackL && sensor[3] < blackR){
kamorei 0:500d22d1efeb 213 stop_point_ver2();
kamorei 0:500d22d1efeb 214 break;
kamorei 0:500d22d1efeb 215 }
kamorei 0:500d22d1efeb 216 }
kamorei 2:47477c2b1925 217 }
kamorei 2:47477c2b1925 218
kamorei 2:47477c2b1925 219 /*以下メモ
kamorei 2:47477c2b1925 220 ・段差
kamorei 2:47477c2b1925 221 if( sensor[0] > 閾値){
kamorei 2:47477c2b1925 222 wait(0.5); //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 223 sensor[2] = read.sensorCR;
kamorei 2:47477c2b1925 224 go_straight_CR();
kamorei 2:47477c2b1925 225 } else if( sensor[3] > 閾値){
kamorei 2:47477c2b1925 226 wait(0.5) //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 227 sensor[1] = read.sensorCL;
kamorei 2:47477c2b1925 228 go_straight_CL();
kamorei 2:47477c2b1925 229 }
kamorei 2:47477c2b1925 230
kamorei 2:47477c2b1925 231 ・プログラムの切り替え
kamorei 2:47477c2b1925 232 回転動作をしたら（完了したら）カウント
kamorei 2:47477c2b1925 233 ⇒回転を一発で決める必要有
kamorei 2:47477c2b1925 234 */