Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termB_11_9

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@1:1afb2ef8c2cc, 2018-11-08 (annotated)

Committer:
kamorei
Date:
Thu Nov 08 06:43:04 2018 +0000
Revision:
1:1afb2ef8c2cc
Parent:
0:f1a55bedce7d
Child:
2:12c99b6f6d20
11/8 ??????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 0:f1a55bedce7d 1 /*
kamorei 0:f1a55bedce7d 2 memo
kamorei 0:f1a55bedce7d 3 grayCR,grayCLを変動させる
kamorei 0:f1a55bedce7d 4 grayR,grayLも変動させたほうがいい…？
kamorei 1:1afb2ef8c2cc 5 止まるポイントで電圧値が変化している（していないと困るわけだが）なら変動を見て判断もできる（コメントアウトしてある）
kamorei 0:f1a55bedce7d 6 */
kamorei 0:f1a55bedce7d 7
kamorei 0:f1a55bedce7d 8 #include "mbed.h"
kamorei 0:f1a55bedce7d 9
kamorei 0:f1a55bedce7d 10 #define KP 1.5 //Pゲイン
kamorei 0:f1a55bedce7d 11 #define CHG 1.0 //change_gray用の係数
kamorei 1:1afb2ef8c2cc 12 #define WHITE_CL 0.39 //初期値
kamorei 1:1afb2ef8c2cc 13 #define WHITE_CR 0.39 //初期値
kamorei 1:1afb2ef8c2cc 14 #define GRAY_CL 0.21 //初期値
kamorei 1:1afb2ef8c2cc 15 #define GRAY_CR 0.32 //初期値
kamorei 1:1afb2ef8c2cc 16 //#define STOP_RATIO 0.06 //止まる判断をする変動率
kamorei 0:f1a55bedce7d 17
kamorei 0:f1a55bedce7d 18 DigitalOut ledL( PTB8);
kamorei 0:f1a55bedce7d 19 DigitalOut ledR( PTE5);
kamorei 0:f1a55bedce7d 20 BusOut ledLL( PTB8, PTB9);
kamorei 0:f1a55bedce7d 21 BusOut ledRR( PTE5, PTE4); //わざと右から書いてます
kamorei 0:f1a55bedce7d 22
kamorei 0:f1a55bedce7d 23 AnalogIn sensorR( PTB1);
kamorei 0:f1a55bedce7d 24 AnalogIn sensorL( PTB3);
kamorei 0:f1a55bedce7d 25 AnalogIn sensorCR( PTB0);
kamorei 0:f1a55bedce7d 26 AnalogIn sensorCL( PTB2);
kamorei 0:f1a55bedce7d 27
kamorei 0:f1a55bedce7d 28 //モータ1
kamorei 0:f1a55bedce7d 29 BusOut Mlefti(PTA1, PTA2);
kamorei 0:f1a55bedce7d 30 PwmOut Mleftp(PTD4);
kamorei 0:f1a55bedce7d 31 //モータ2
kamorei 0:f1a55bedce7d 32 BusOut Mrighti(PTC0, PTC7);
kamorei 0:f1a55bedce7d 33 PwmOut Mrightp(PTA12);
kamorei 0:f1a55bedce7d 34
kamorei 0:f1a55bedce7d 35 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が基準値
kamorei 0:f1a55bedce7d 36 float blackCL = 0.03; //閾値（再定義必須）の跡地
kamorei 0:f1a55bedce7d 37 float blackCR = 0.26; //↑と同じ
kamorei 0:f1a55bedce7d 38 float whiteL = 0.23, blackL = 0.2, grayL = 0.215;
kamorei 0:f1a55bedce7d 39 float whiteR = 0.25, blackR = 0.2, grayR = 0.225;
kamorei 0:f1a55bedce7d 40 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:f1a55bedce7d 41 float pr, pl;
kamorei 0:f1a55bedce7d 42
kamorei 1:1afb2ef8c2cc 43 //float sensorLval[2], sensorRval[2]; //[0]が今，[1]が一個前
kamorei 1:1afb2ef8c2cc 44
kamorei 0:f1a55bedce7d 45 void set_threshold(){
kamorei 0:f1a55bedce7d 46 for( int i = 0; i < 2; i++){
kamorei 0:f1a55bedce7d 47 whiteCL[i] = WHITE_CL;
kamorei 0:f1a55bedce7d 48 whiteCR[i] = WHITE_CR;
kamorei 0:f1a55bedce7d 49 grayCL[i] = GRAY_CL;
kamorei 0:f1a55bedce7d 50 grayCR[i] = GRAY_CR;
kamorei 0:f1a55bedce7d 51 }
kamorei 0:f1a55bedce7d 52 }
kamorei 0:f1a55bedce7d 53
kamorei 0:f1a55bedce7d 54 void go_straight_p(){
kamorei 0:f1a55bedce7d 55 Mrighti = 1;
kamorei 0:f1a55bedce7d 56 Mrightp = (KP * pr) * 1.0f;
kamorei 0:f1a55bedce7d 57 Mlefti = 1;
kamorei 0:f1a55bedce7d 58 Mleftp = (KP * pl) * 1.0f;
kamorei 0:f1a55bedce7d 59 ledR = 0;
kamorei 0:f1a55bedce7d 60 ledL = 0;
kamorei 0:f1a55bedce7d 61 }
kamorei 0:f1a55bedce7d 62
kamorei 0:f1a55bedce7d 63 void turn_right(){
kamorei 0:f1a55bedce7d 64 Mrighti = 2;
kamorei 1:1afb2ef8c2cc 65 Mrightp = 0.5f;
kamorei 0:f1a55bedce7d 66 Mlefti = 1;
kamorei 0:f1a55bedce7d 67 Mleftp = 0.8f;
kamorei 0:f1a55bedce7d 68 ledR = 0;
kamorei 0:f1a55bedce7d 69 ledL = 1;
kamorei 0:f1a55bedce7d 70 }
kamorei 0:f1a55bedce7d 71
kamorei 0:f1a55bedce7d 72 void turn_left(){
kamorei 0:f1a55bedce7d 73 Mrighti = 1;
kamorei 0:f1a55bedce7d 74 Mrightp = 0.8f;
kamorei 0:f1a55bedce7d 75 Mlefti = 2;
kamorei 1:1afb2ef8c2cc 76 Mleftp = 0.5f;
kamorei 0:f1a55bedce7d 77 ledR = 1;
kamorei 0:f1a55bedce7d 78 ledL = 0;
kamorei 0:f1a55bedce7d 79 }
kamorei 0:f1a55bedce7d 80
kamorei 1:1afb2ef8c2cc 81 void turn_right_ver2(){
kamorei 1:1afb2ef8c2cc 82 while( sensor[0] > grayL)
kamorei 1:1afb2ef8c2cc 83 turn_right();
kamorei 1:1afb2ef8c2cc 84 while( sensor[2] > grayCR[0])
kamorei 1:1afb2ef8c2cc 85 turn_left();
kamorei 1:1afb2ef8c2cc 86 }
kamorei 1:1afb2ef8c2cc 87
kamorei 1:1afb2ef8c2cc 88 void turn_left_ver2(){
kamorei 1:1afb2ef8c2cc 89 while( sensor[3] > grayR)
kamorei 1:1afb2ef8c2cc 90 turn_left();
kamorei 1:1afb2ef8c2cc 91 while( sensor[1] > grayCL[0])
kamorei 1:1afb2ef8c2cc 92 turn_right();
kamorei 1:1afb2ef8c2cc 93 }
kamorei 1:1afb2ef8c2cc 94
kamorei 0:f1a55bedce7d 95 void go_straight_check(){ //モータドライバの調子の確認用
kamorei 0:f1a55bedce7d 96 Mrighti = 1;
kamorei 0:f1a55bedce7d 97 Mrightp = 0.8f;
kamorei 0:f1a55bedce7d 98 Mlefti = 1;
kamorei 0:f1a55bedce7d 99 Mleftp = 0.8f;
kamorei 0:f1a55bedce7d 100 }
kamorei 0:f1a55bedce7d 101
kamorei 0:f1a55bedce7d 102 void stop_point(){
kamorei 0:f1a55bedce7d 103 Mrighti = 2;
kamorei 0:f1a55bedce7d 104 Mrightp = 0.5f;
kamorei 0:f1a55bedce7d 105 Mlefti = 2;
kamorei 0:f1a55bedce7d 106 Mleftp = 0.5f;
kamorei 0:f1a55bedce7d 107 wait(0.05);
kamorei 0:f1a55bedce7d 108 Mrighti = 0;
kamorei 0:f1a55bedce7d 109 Mlefti = 0;
kamorei 0:f1a55bedce7d 110 ledRR = 0b11;
kamorei 0:f1a55bedce7d 111 ledLL = 0b11;
kamorei 0:f1a55bedce7d 112 }
kamorei 0:f1a55bedce7d 113
kamorei 0:f1a55bedce7d 114 void change_gray(){
kamorei 1:1afb2ef8c2cc 115 if( sensor[1] <= grayCL[0]){ //sensorCL <=grayCL
kamorei 0:f1a55bedce7d 116 whiteCR[0] = sensorCR.read();
kamorei 0:f1a55bedce7d 117 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:f1a55bedce7d 118 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:f1a55bedce7d 119 }
kamorei 1:1afb2ef8c2cc 120 else{ //sensorCR <= grayCR
kamorei 1:1afb2ef8c2cc 121 whiteCL[0] = sensorCL.read();
kamorei 0:f1a55bedce7d 122 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:f1a55bedce7d 123 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:f1a55bedce7d 124 }
kamorei 0:f1a55bedce7d 125 }
kamorei 0:f1a55bedce7d 126
kamorei 0:f1a55bedce7d 127 int main() {
kamorei 0:f1a55bedce7d 128 set_threshold();
kamorei 0:f1a55bedce7d 129 go_straight_check();
kamorei 0:f1a55bedce7d 130 wait(0.2);
kamorei 0:f1a55bedce7d 131 while(1) {
kamorei 0:f1a55bedce7d 132 sensor[0] = sensorL.read();
kamorei 0:f1a55bedce7d 133 sensor[1] = sensorCL.read();
kamorei 0:f1a55bedce7d 134 sensor[2] = sensorCR.read();
kamorei 0:f1a55bedce7d 135 sensor[3] = sensorR.read();
kamorei 0:f1a55bedce7d 136
kamorei 1:1afb2ef8c2cc 137 /*sensorLval[1] = sensorLval[0];
kamorei 1:1afb2ef8c2cc 138 sensorRval[1] = sensorRval[0];
kamorei 1:1afb2ef8c2cc 139 sensorLval[0] = sensor[0];
kamorei 1:1afb2ef8c2cc 140 sensorRval[0] = sensor[3];*/
kamorei 1:1afb2ef8c2cc 141
kamorei 0:f1a55bedce7d 142 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 0:f1a55bedce7d 143 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 0:f1a55bedce7d 144
kamorei 0:f1a55bedce7d 145 if( sensor[1] <= grayCL[0]){ //sensorCL <= grayCL
kamorei 0:f1a55bedce7d 146 turn_left();
kamorei 0:f1a55bedce7d 147 change_gray();
kamorei 0:f1a55bedce7d 148 } else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
kamorei 0:f1a55bedce7d 149 turn_right();
kamorei 0:f1a55bedce7d 150 change_gray();
kamorei 1:1afb2ef8c2cc 151 } else if( sensor[0] <= grayL && sensor[1] <= grayCL[0]){
kamorei 1:1afb2ef8c2cc 152 turn_left_ver2();
kamorei 1:1afb2ef8c2cc 153 change_gray();
kamorei 1:1afb2ef8c2cc 154 } else if( sensor[3] <= grayR && sensor[2] <= grayCR[0]){
kamorei 1:1afb2ef8c2cc 155 turn_right_ver2();
kamorei 1:1afb2ef8c2cc 156 change_gray();
kamorei 0:f1a55bedce7d 157 } else
kamorei 0:f1a55bedce7d 158 go_straight_p();
kamorei 1:1afb2ef8c2cc 159 //if( ((sensorLval[1] - sensorLval[0]) / sensorLval[1]) > STOP_RATIO && ((sensorRval[1] - sensorRval[0]) / sensorRval[1]) > STOP_RATIO){
kamorei 0:f1a55bedce7d 160 if( sensor[0] <= grayL && sensor[3] <= grayR){
kamorei 0:f1a55bedce7d 161 stop_point();
kamorei 0:f1a55bedce7d 162 break;
kamorei 0:f1a55bedce7d 163 }
kamorei 0:f1a55bedce7d 164 }
kamorei 0:f1a55bedce7d 165 }