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@0:f1a55bedce7d, 2018-11-06 (annotated)

Committer:
kamorei
Date:
Tue Nov 06 17:24:21 2018 +0000
Revision:
0:f1a55bedce7d
Child:
1:1afb2ef8c2cc
11/6

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 0:f1a55bedce7d 5 */
kamorei 0:f1a55bedce7d 6
kamorei 0:f1a55bedce7d 7 #include "mbed.h"
kamorei 0:f1a55bedce7d 8
kamorei 0:f1a55bedce7d 9 #define KP 1.5 //Pゲイン
kamorei 0:f1a55bedce7d 10 #define CHG 1.0 //change_gray用の係数
kamorei 0:f1a55bedce7d 11 #define WHITE_CL 0.39
kamorei 0:f1a55bedce7d 12 #define WHITE_CR 0.39
kamorei 0:f1a55bedce7d 13 #define GRAY_CL 0.21
kamorei 0:f1a55bedce7d 14 #define GRAY_CR 0.32
kamorei 0:f1a55bedce7d 15
kamorei 0:f1a55bedce7d 16 DigitalOut ledL( PTB8);
kamorei 0:f1a55bedce7d 17 DigitalOut ledR( PTE5);
kamorei 0:f1a55bedce7d 18 BusOut ledLL( PTB8, PTB9);
kamorei 0:f1a55bedce7d 19 BusOut ledRR( PTE5, PTE4); //わざと右から書いてます
kamorei 0:f1a55bedce7d 20
kamorei 0:f1a55bedce7d 21 AnalogIn sensorR( PTB1);
kamorei 0:f1a55bedce7d 22 AnalogIn sensorL( PTB3);
kamorei 0:f1a55bedce7d 23 AnalogIn sensorCR( PTB0);
kamorei 0:f1a55bedce7d 24 AnalogIn sensorCL( PTB2);
kamorei 0:f1a55bedce7d 25
kamorei 0:f1a55bedce7d 26 //モータ1
kamorei 0:f1a55bedce7d 27 BusOut Mlefti(PTA1, PTA2);
kamorei 0:f1a55bedce7d 28 PwmOut Mleftp(PTD4);
kamorei 0:f1a55bedce7d 29 //モータ2
kamorei 0:f1a55bedce7d 30 BusOut Mrighti(PTC0, PTC7);
kamorei 0:f1a55bedce7d 31 PwmOut Mrightp(PTA12);
kamorei 0:f1a55bedce7d 32
kamorei 0:f1a55bedce7d 33 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が基準値
kamorei 0:f1a55bedce7d 34 float blackCL = 0.03; //閾値（再定義必須）の跡地
kamorei 0:f1a55bedce7d 35 float blackCR = 0.26; //↑と同じ
kamorei 0:f1a55bedce7d 36 float whiteL = 0.23, blackL = 0.2, grayL = 0.215;
kamorei 0:f1a55bedce7d 37 float whiteR = 0.25, blackR = 0.2, grayR = 0.225;
kamorei 0:f1a55bedce7d 38 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:f1a55bedce7d 39 float pr, pl;
kamorei 0:f1a55bedce7d 40
kamorei 0:f1a55bedce7d 41 void set_threshold(){
kamorei 0:f1a55bedce7d 42 for( int i = 0; i < 2; i++){
kamorei 0:f1a55bedce7d 43 whiteCL[i] = WHITE_CL;
kamorei 0:f1a55bedce7d 44 whiteCR[i] = WHITE_CR;
kamorei 0:f1a55bedce7d 45 grayCL[i] = GRAY_CL;
kamorei 0:f1a55bedce7d 46 grayCR[i] = GRAY_CR;
kamorei 0:f1a55bedce7d 47 }
kamorei 0:f1a55bedce7d 48 }
kamorei 0:f1a55bedce7d 49
kamorei 0:f1a55bedce7d 50 void go_straight_p(){
kamorei 0:f1a55bedce7d 51 Mrighti = 1;
kamorei 0:f1a55bedce7d 52 Mrightp = (KP * pr) * 1.0f;
kamorei 0:f1a55bedce7d 53 Mlefti = 1;
kamorei 0:f1a55bedce7d 54 Mleftp = (KP * pl) * 1.0f;
kamorei 0:f1a55bedce7d 55 ledR = 0;
kamorei 0:f1a55bedce7d 56 ledL = 0;
kamorei 0:f1a55bedce7d 57 }
kamorei 0:f1a55bedce7d 58
kamorei 0:f1a55bedce7d 59 void turn_right(){
kamorei 0:f1a55bedce7d 60 Mrighti = 2;
kamorei 0:f1a55bedce7d 61 Mrightp = 0.8f;
kamorei 0:f1a55bedce7d 62 Mlefti = 1;
kamorei 0:f1a55bedce7d 63 Mleftp = 0.8f;
kamorei 0:f1a55bedce7d 64 ledR = 0;
kamorei 0:f1a55bedce7d 65 ledL = 1;
kamorei 0:f1a55bedce7d 66 }
kamorei 0:f1a55bedce7d 67
kamorei 0:f1a55bedce7d 68 void turn_left(){
kamorei 0:f1a55bedce7d 69 Mrighti = 1;
kamorei 0:f1a55bedce7d 70 Mrightp = 0.8f;
kamorei 0:f1a55bedce7d 71 Mlefti = 2;
kamorei 0:f1a55bedce7d 72 Mleftp = 0.8f;
kamorei 0:f1a55bedce7d 73 ledR = 1;
kamorei 0:f1a55bedce7d 74 ledL = 0;
kamorei 0:f1a55bedce7d 75 }
kamorei 0:f1a55bedce7d 76
kamorei 0:f1a55bedce7d 77 void go_straight_check(){ //モータドライバの調子の確認用
kamorei 0:f1a55bedce7d 78 Mrighti = 1;
kamorei 0:f1a55bedce7d 79 Mrightp = 0.8f;
kamorei 0:f1a55bedce7d 80 Mlefti = 1;
kamorei 0:f1a55bedce7d 81 Mleftp = 0.8f;
kamorei 0:f1a55bedce7d 82 }
kamorei 0:f1a55bedce7d 83
kamorei 0:f1a55bedce7d 84 void stop_point(){
kamorei 0:f1a55bedce7d 85 Mrighti = 2;
kamorei 0:f1a55bedce7d 86 Mrightp = 0.5f;
kamorei 0:f1a55bedce7d 87 Mlefti = 2;
kamorei 0:f1a55bedce7d 88 Mleftp = 0.5f;
kamorei 0:f1a55bedce7d 89 wait(0.05);
kamorei 0:f1a55bedce7d 90 Mrighti = 0;
kamorei 0:f1a55bedce7d 91 Mlefti = 0;
kamorei 0:f1a55bedce7d 92 ledRR = 0b11;
kamorei 0:f1a55bedce7d 93 ledLL = 0b11;
kamorei 0:f1a55bedce7d 94 }
kamorei 0:f1a55bedce7d 95
kamorei 0:f1a55bedce7d 96 void change_gray(){
kamorei 0:f1a55bedce7d 97 if( sensor[1] <= grayCL[0]){
kamorei 0:f1a55bedce7d 98 whiteCR[0] = sensorCR.read();
kamorei 0:f1a55bedce7d 99 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:f1a55bedce7d 100 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:f1a55bedce7d 101 }
kamorei 0:f1a55bedce7d 102 else{
kamorei 0:f1a55bedce7d 103 whiteCR[0] = sensorCL.read();
kamorei 0:f1a55bedce7d 104 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:f1a55bedce7d 105 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:f1a55bedce7d 106 }
kamorei 0:f1a55bedce7d 107 }
kamorei 0:f1a55bedce7d 108
kamorei 0:f1a55bedce7d 109 int main() {
kamorei 0:f1a55bedce7d 110 set_threshold();
kamorei 0:f1a55bedce7d 111 go_straight_check();
kamorei 0:f1a55bedce7d 112 wait(0.2);
kamorei 0:f1a55bedce7d 113 while(1) {
kamorei 0:f1a55bedce7d 114 sensor[0] = sensorL.read();
kamorei 0:f1a55bedce7d 115 sensor[1] = sensorCL.read();
kamorei 0:f1a55bedce7d 116 sensor[2] = sensorCR.read();
kamorei 0:f1a55bedce7d 117 sensor[3] = sensorR.read();
kamorei 0:f1a55bedce7d 118
kamorei 0:f1a55bedce7d 119 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 0:f1a55bedce7d 120 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 0:f1a55bedce7d 121
kamorei 0:f1a55bedce7d 122 if( sensor[1] <= grayCL[0]){ //sensorCL <= grayCL
kamorei 0:f1a55bedce7d 123 turn_left();
kamorei 0:f1a55bedce7d 124 change_gray();
kamorei 0:f1a55bedce7d 125 } else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
kamorei 0:f1a55bedce7d 126 turn_right();
kamorei 0:f1a55bedce7d 127 change_gray();
kamorei 0:f1a55bedce7d 128 } else
kamorei 0:f1a55bedce7d 129 go_straight_p();
kamorei 0:f1a55bedce7d 130 if( sensor[0] <= grayL && sensor[3] <= grayR){
kamorei 0:f1a55bedce7d 131 stop_point();
kamorei 0:f1a55bedce7d 132 break;
kamorei 0:f1a55bedce7d 133 }
kamorei 0:f1a55bedce7d 134 }
kamorei 0:f1a55bedce7d 135 }