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@1:905436937f78, 2018-10-30 (annotated)

Committer:
kamorei
Date:
Tue Oct 30 05:08:42 2018 +0000
Revision:
1:905436937f78
Parent:
0:8b8f5dba70e9
Child:
2:8748af75523f
10/27 ??????

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 0:8b8f5dba70e9 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 0:8b8f5dba70e9 30 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 1:905436937f78 31 float kp = 0.3; //Pゲイン
kamorei 0:8b8f5dba70e9 32 float pr, pl;
kamorei 0:8b8f5dba70e9 33 int i = 0, j = 0;
kamorei 0:8b8f5dba70e9 34
kamorei 0:8b8f5dba70e9 35 void turn_right(){
kamorei 0:8b8f5dba70e9 36 Mlefti = 2;
kamorei 0:8b8f5dba70e9 37 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 38 Mrighti = 1;
kamorei 0:8b8f5dba70e9 39 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 40 ledR = 1;
kamorei 0:8b8f5dba70e9 41 ledL = 0;
kamorei 0:8b8f5dba70e9 42 }
kamorei 0:8b8f5dba70e9 43
kamorei 0:8b8f5dba70e9 44 void turn_left(){
kamorei 0:8b8f5dba70e9 45 Mrighti = 2;
kamorei 0:8b8f5dba70e9 46 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 47 Mlefti = 1;
kamorei 0:8b8f5dba70e9 48 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 49 ledR = 0;
kamorei 0:8b8f5dba70e9 50 ledL = 1;
kamorei 0:8b8f5dba70e9 51 }
kamorei 0:8b8f5dba70e9 52
kamorei 1:905436937f78 53 void go_straight(){
kamorei 0:8b8f5dba70e9 54 Mrighti = 2;
kamorei 0:8b8f5dba70e9 55 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 56 Mlefti = 2;
kamorei 0:8b8f5dba70e9 57 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 58 ledR = 0;
kamorei 0:8b8f5dba70e9 59 ledL = 0;
kamorei 0:8b8f5dba70e9 60 }
kamorei 0:8b8f5dba70e9 61
kamorei 1:905436937f78 62 void go_straight_p(){
kamorei 1:905436937f78 63 if( pr < (gray - black) / (white - black)){
kamorei 1:905436937f78 64 Mrighti = 1;
kamorei 1:905436937f78 65 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 66 Mlefti = 2;
kamorei 1:905436937f78 67 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 1:905436937f78 68 ledR = 1;
kamorei 1:905436937f78 69 ledL = 0;
kamorei 1:905436937f78 70 } else if( pl < (gray - black) / (white - black)){
kamorei 1:905436937f78 71 Mrighti = 2;
kamorei 1:905436937f78 72 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 73 Mlefti = 1;
kamorei 1:905436937f78 74 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 1:905436937f78 75 ledR = 0;
kamorei 1:905436937f78 76 ledL = 1;
kamorei 1:905436937f78 77 } else{
kamorei 1:905436937f78 78 Mrighti = 2;
kamorei 1:905436937f78 79 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 1:905436937f78 80 Mlefti = 2;
kamorei 1:905436937f78 81 Mleftp = (kp * pl - 0.1) * 1.0f;
kamorei 1:905436937f78 82 ledR = 0;
kamorei 1:905436937f78 83 ledL = 0;
kamorei 1:905436937f78 84 }
kamorei 1:905436937f78 85 }
kamorei 1:905436937f78 86
kamorei 1:905436937f78 87 void go_straight_check(){ //モータドライバの調子の確認用
kamorei 1:905436937f78 88 Mrighti = 2;
kamorei 1:905436937f78 89 Mrightp = 0.8f;
kamorei 1:905436937f78 90 Mlefti = 2;
kamorei 1:905436937f78 91 Mleftp = 0.8f;
kamorei 1:905436937f78 92 }
kamorei 1:905436937f78 93
kamorei 1:905436937f78 94 void go_back(){
kamorei 0:8b8f5dba70e9 95 Mrighti = 1;
kamorei 0:8b8f5dba70e9 96 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 97 Mlefti = 1;
kamorei 0:8b8f5dba70e9 98 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 99 ledR = 0;
kamorei 0:8b8f5dba70e9 100 ledL = 0;
kamorei 0:8b8f5dba70e9 101 }
kamorei 0:8b8f5dba70e9 102
kamorei 0:8b8f5dba70e9 103 void stop_point(){
kamorei 1:905436937f78 104 Mrighti = 1;
kamorei 1:905436937f78 105 Mrightp = 0.5f;
kamorei 1:905436937f78 106 Mlefti = 1;
kamorei 1:905436937f78 107 Mleftp = 0.5f;
kamorei 1:905436937f78 108 wait(0.05);
kamorei 0:8b8f5dba70e9 109 Mrighti = 0;
kamorei 0:8b8f5dba70e9 110 Mlefti = 0;
kamorei 0:8b8f5dba70e9 111 ledRR = 2;
kamorei 0:8b8f5dba70e9 112 ledLL = 2;
kamorei 0:8b8f5dba70e9 113 }
kamorei 0:8b8f5dba70e9 114
kamorei 0:8b8f5dba70e9 115 int main() {
kamorei 1:905436937f78 116 go_straight_check();
kamorei 0:8b8f5dba70e9 117 wait(0.2);
kamorei 0:8b8f5dba70e9 118 while( i < 3){
kamorei 0:8b8f5dba70e9 119 while(1) {
kamorei 0:8b8f5dba70e9 120 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 121 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 122 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 123 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 124
kamorei 0:8b8f5dba70e9 125 pr = (sensor[2] - black) / (white - black);
kamorei 0:8b8f5dba70e9 126 pl = (sensor[1] - black) / (white - black);
kamorei 0:8b8f5dba70e9 127
kamorei 1:905436937f78 128 /* if( sensor[2] <= gray)
kamorei 0:8b8f5dba70e9 129 turn_right();
kamorei 0:8b8f5dba70e9 130 else if( sensor[1] <= gray)
kamorei 0:8b8f5dba70e9 131 turn_left();
kamorei 0:8b8f5dba70e9 132 else if( sensor[1] <= gray && sensor[2] <= gray){
kamorei 0:8b8f5dba70e9 133 if(ledR == 1)
kamorei 1:905436937f78 134 turn_right();
kamorei 0:8b8f5dba70e9 135 else
kamorei 1:905436937f78 136 turn_left();
kamorei 0:8b8f5dba70e9 137 }
kamorei 1:905436937f78 138 else*/
kamorei 1:905436937f78 139 go_straight_p();
kamorei 0:8b8f5dba70e9 140 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 0:8b8f5dba70e9 141 stop_point();
kamorei 0:8b8f5dba70e9 142 break;
kamorei 0:8b8f5dba70e9 143 }
kamorei 0:8b8f5dba70e9 144 }
kamorei 0:8b8f5dba70e9 145 i++;
kamorei 1:905436937f78 146 if( i < 2){
kamorei 0:8b8f5dba70e9 147 /* while(1){
kamorei 0:8b8f5dba70e9 148 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 149 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 150 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 151 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 152
kamorei 0:8b8f5dba70e9 153 pr = (sensor[2] - black) / (white - black);
kamorei 0:8b8f5dba70e9 154 pl = (sensor[1] - black) / (white - black);
kamorei 0:8b8f5dba70e9 155
kamorei 0:8b8f5dba70e9 156 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 0:8b8f5dba70e9 157 stop_point();
kamorei 0:8b8f5dba70e9 158 break;
kamorei 0:8b8f5dba70e9 159 }
kamorei 0:8b8f5dba70e9 160 else
kamorei 1:905436937f78 161 go_back();
kamorei 0:8b8f5dba70e9 162 }
kamorei 0:8b8f5dba70e9 163 */
kamorei 0:8b8f5dba70e9 164 for( j = 0; j < 2;){
kamorei 0:8b8f5dba70e9 165 turn_left();
kamorei 0:8b8f5dba70e9 166 if( sensor[2] <= black)
kamorei 0:8b8f5dba70e9 167 j++;
kamorei 0:8b8f5dba70e9 168 }
kamorei 0:8b8f5dba70e9 169 }
kamorei 0:8b8f5dba70e9 170 }
kamorei 0:8b8f5dba70e9 171 }