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@0:500d22d1efeb, 2018-11-15 (annotated)

Committer:
kamorei
Date:
Thu Nov 15 06:19:52 2018 +0000
Revision:
0:500d22d1efeb
Child:
1:5c26d3744592
11/15 ????????

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 0:500d22d1efeb 5 #define WHITE_CL 0.88 //初期値
kamorei 0:500d22d1efeb 6 #define WHITE_CR 0.88 //初期値
kamorei 0:500d22d1efeb 7 #define GRAY_CL 0.65 //初期値
kamorei 0:500d22d1efeb 8 #define GRAY_CR 0.65 //初期値
kamorei 0:500d22d1efeb 9 #define GRAY_L 0.26 //初期値
kamorei 0:500d22d1efeb 10 #define GRAY_R 0.25 //初期値
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 0:500d22d1efeb 32 float blackCL = 0.12; //閾値の跡地
kamorei 0:500d22d1efeb 33 float blackCR = 0.3; //↑と同じ
kamorei 0:500d22d1efeb 34 float whiteL = 0.27, blackL = 0.22;
kamorei 0:500d22d1efeb 35 float whiteR = 0.28, blackR = 0.22;
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 0:500d22d1efeb 53 Mrighti = 1;
kamorei 0:500d22d1efeb 54 Mrightp = (KP * pr) * 1.0f;
kamorei 0:500d22d1efeb 55 Mlefti = 1;
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 0:500d22d1efeb 62 Mrighti = 1;
kamorei 0:500d22d1efeb 63 Mrightp = (KP * pr + 0.3) * 1.0f;
kamorei 0:500d22d1efeb 64 Mlefti = 1;
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 0:500d22d1efeb 70 void turn_right(){
kamorei 0:500d22d1efeb 71 Mrighti = 2;
kamorei 0:500d22d1efeb 72 Mrightp = 0.05f;
kamorei 0:500d22d1efeb 73 Mlefti = 1;
kamorei 0:500d22d1efeb 74 Mleftp = 0.1f;
kamorei 0:500d22d1efeb 75 ledR = 1;
kamorei 0:500d22d1efeb 76 ledL = 0;
kamorei 0:500d22d1efeb 77 }
kamorei 0:500d22d1efeb 78
kamorei 0:500d22d1efeb 79 void turn_left(){
kamorei 0:500d22d1efeb 80 Mrighti = 1;
kamorei 0:500d22d1efeb 81 Mrightp = 0.1f;
kamorei 0:500d22d1efeb 82 Mlefti = 2;
kamorei 0:500d22d1efeb 83 Mleftp = 0.05f;
kamorei 0:500d22d1efeb 84 ledR = 0;
kamorei 0:500d22d1efeb 85 ledL = 1;
kamorei 0:500d22d1efeb 86 }
kamorei 0:500d22d1efeb 87
kamorei 0:500d22d1efeb 88 void turn_right_ver2(){
kamorei 0:500d22d1efeb 89 while( sensor[0] > grayL[0]){ //sensorL > grayL[0]
kamorei 0:500d22d1efeb 90 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 91 turn_right();
kamorei 0:500d22d1efeb 92 }
kamorei 0:500d22d1efeb 93 stop_point_ver2();
kamorei 0:500d22d1efeb 94 while( sensor[2] > grayCR[0]){ //補正
kamorei 0:500d22d1efeb 95 sensor[2] = sensorCR.read();
kamorei 0:500d22d1efeb 96 turn_left();
kamorei 0:500d22d1efeb 97 }
kamorei 0:500d22d1efeb 98 stop_point_ver2();
kamorei 0:500d22d1efeb 99 }
kamorei 0:500d22d1efeb 100
kamorei 0:500d22d1efeb 101 void turn_left_ver2(){
kamorei 0:500d22d1efeb 102 while( sensor[3] > grayR[0]){ //sensorR > grayR[0]
kamorei 0:500d22d1efeb 103 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 104 turn_left();
kamorei 0:500d22d1efeb 105 }
kamorei 0:500d22d1efeb 106 stop_point_ver2();
kamorei 0:500d22d1efeb 107 while( sensor[1] > grayCL[0]){ //補正
kamorei 0:500d22d1efeb 108 sensor[1] = sensorCL.read();
kamorei 0:500d22d1efeb 109 turn_right();
kamorei 0:500d22d1efeb 110 }
kamorei 0:500d22d1efeb 111 stop_point_ver2();
kamorei 0:500d22d1efeb 112 }
kamorei 0:500d22d1efeb 113
kamorei 0:500d22d1efeb 114 void motor_check(){ //モータドライバの調子の確認用
kamorei 0:500d22d1efeb 115 Mrighti = 1;
kamorei 0:500d22d1efeb 116 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 117 Mlefti = 1;
kamorei 0:500d22d1efeb 118 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 119 }
kamorei 0:500d22d1efeb 120
kamorei 0:500d22d1efeb 121 void stop_point(){
kamorei 0:500d22d1efeb 122 Mrighti = 2;
kamorei 0:500d22d1efeb 123 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 124 Mlefti = 2;
kamorei 0:500d22d1efeb 125 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 126 wait(0.05);
kamorei 0:500d22d1efeb 127 Mrighti = 0;
kamorei 0:500d22d1efeb 128 Mlefti = 0;
kamorei 0:500d22d1efeb 129 ledRR = 0b11;
kamorei 0:500d22d1efeb 130 ledLL = 0b11;
kamorei 0:500d22d1efeb 131 }
kamorei 0:500d22d1efeb 132
kamorei 0:500d22d1efeb 133 void stop_point_ver2(){
kamorei 0:500d22d1efeb 134 if( Mrighti == 1)
kamorei 0:500d22d1efeb 135 Mrighti = 2;
kamorei 0:500d22d1efeb 136 else if( Mrighti == 2)
kamorei 0:500d22d1efeb 137 Mrighti = 1;
kamorei 0:500d22d1efeb 138 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 139 if( Mlefti == 1)
kamorei 0:500d22d1efeb 140 Mlefti = 2;
kamorei 0:500d22d1efeb 141 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 142 Mlefti = 1;
kamorei 0:500d22d1efeb 143 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 144 wait(0.05);
kamorei 0:500d22d1efeb 145 Mrighti = 0;
kamorei 0:500d22d1efeb 146 Mlefti = 0;
kamorei 0:500d22d1efeb 147 ledRR = 0b11;
kamorei 0:500d22d1efeb 148 ledLL = 0b11;
kamorei 0:500d22d1efeb 149 }
kamorei 0:500d22d1efeb 150
kamorei 0:500d22d1efeb 151 void change_gray(){
kamorei 0:500d22d1efeb 152 if( sensor[1] <= grayCL[0]){ //sensorCL <=grayCL
kamorei 0:500d22d1efeb 153 whiteCR[0] = sensorCR.read();
kamorei 0:500d22d1efeb 154 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 155 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 156 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 157 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 158 ledRR = 0b10;
kamorei 0:500d22d1efeb 159 ledLL = 0;
kamorei 0:500d22d1efeb 160 }
kamorei 0:500d22d1efeb 161 else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
kamorei 0:500d22d1efeb 162 whiteCL[0] = sensorCL.read();
kamorei 0:500d22d1efeb 163 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 164 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 165 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 166 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 167 ledRR = 0;
kamorei 0:500d22d1efeb 168 ledLL = 0b10;
kamorei 0:500d22d1efeb 169 }
kamorei 0:500d22d1efeb 170 }
kamorei 0:500d22d1efeb 171
kamorei 0:500d22d1efeb 172 int main() {
kamorei 0:500d22d1efeb 173 ledC = 1;
kamorei 0:500d22d1efeb 174 set_threshold();
kamorei 0:500d22d1efeb 175 motor_check();
kamorei 0:500d22d1efeb 176 wait(0.5);
kamorei 0:500d22d1efeb 177 while(1) {
kamorei 0:500d22d1efeb 178 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 179 sensor[1] = sensorCL.read();
kamorei 0:500d22d1efeb 180 sensor[2] = sensorCR.read();
kamorei 0:500d22d1efeb 181 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 182
kamorei 0:500d22d1efeb 183 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 0:500d22d1efeb 184 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 0:500d22d1efeb 185
kamorei 0:500d22d1efeb 186 if( sensor[0] <= grayL[0]){
kamorei 0:500d22d1efeb 187 turn_left_ver2();
kamorei 0:500d22d1efeb 188 change_gray();
kamorei 0:500d22d1efeb 189 #ifdef DEBUG
kamorei 0:500d22d1efeb 190 break;
kamorei 0:500d22d1efeb 191 #endif
kamorei 0:500d22d1efeb 192 } else if( sensor[3] <= grayR[0]){
kamorei 0:500d22d1efeb 193 turn_right_ver2();
kamorei 0:500d22d1efeb 194 change_gray();
kamorei 0:500d22d1efeb 195 #ifdef DEBUG
kamorei 0:500d22d1efeb 196 break;
kamorei 0:500d22d1efeb 197 #endif
kamorei 0:500d22d1efeb 198 } else
kamorei 0:500d22d1efeb 199 go_straight_p();
kamorei 0:500d22d1efeb 200 if( sensor[0] < blackL && sensor[3] < blackR){
kamorei 0:500d22d1efeb 201 stop_point_ver2();
kamorei 0:500d22d1efeb 202 break;
kamorei 0:500d22d1efeb 203 }
kamorei 0:500d22d1efeb 204 }
kamorei 0:500d22d1efeb 205 }