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@0:8b8f5dba70e9, 2018-10-26 (annotated)

Committer:
kamorei
Date:
Fri Oct 26 18:28:37 2018 +0000
Revision:
0:8b8f5dba70e9
Child:
1:905436937f78
10/27 ????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 0:8b8f5dba70e9 1 //直線だけP制御したら直線でブレなくなるかもしれないので作りました
kamorei 0:8b8f5dba70e9 2
kamorei 0:8b8f5dba70e9 3 #include "mbed.h"
kamorei 0:8b8f5dba70e9 4
kamorei 0:8b8f5dba70e9 5 DigitalOut ledL( PTB8);
kamorei 0:8b8f5dba70e9 6 DigitalOut ledR( PTE5);
kamorei 0:8b8f5dba70e9 7 BusOut ledLL( PTB8, PTB9);
kamorei 0:8b8f5dba70e9 8 BusOut ledRR( PTE5, PTE4); //わざと右から書いてます
kamorei 0:8b8f5dba70e9 9
kamorei 0:8b8f5dba70e9 10 AnalogIn sensorR( PTB1);
kamorei 0:8b8f5dba70e9 11 AnalogIn sensorL( PTB3);
kamorei 0:8b8f5dba70e9 12 AnalogIn sensorCR( PTB0);
kamorei 0:8b8f5dba70e9 13 AnalogIn sensorCL( PTB2);
kamorei 0:8b8f5dba70e9 14
kamorei 0:8b8f5dba70e9 15 //モータ1
kamorei 0:8b8f5dba70e9 16 BusOut Mlefti(PTA1, PTA2);
kamorei 0:8b8f5dba70e9 17 PwmOut Mleftp(PTD4);
kamorei 0:8b8f5dba70e9 18 //モータ2
kamorei 0:8b8f5dba70e9 19 BusOut Mrighti(PTC0, PTC7);
kamorei 0:8b8f5dba70e9 20 PwmOut Mrightp(PTA12);
kamorei 0:8b8f5dba70e9 21
kamorei 0:8b8f5dba70e9 22 float white = 0.6, black = 0.08, gray = 0.3; //値をぶち込む
kamorei 0:8b8f5dba70e9 23 float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
kamorei 0:8b8f5dba70e9 24 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:8b8f5dba70e9 25 float kp = 0.5; //Pゲイン
kamorei 0:8b8f5dba70e9 26 float pr, pl;
kamorei 0:8b8f5dba70e9 27 int i = 0, j = 0;
kamorei 0:8b8f5dba70e9 28
kamorei 0:8b8f5dba70e9 29 void turn_right(){
kamorei 0:8b8f5dba70e9 30 Mlefti = 2;
kamorei 0:8b8f5dba70e9 31 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 32 Mrighti = 1;
kamorei 0:8b8f5dba70e9 33 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 34 ledR = 1;
kamorei 0:8b8f5dba70e9 35 ledL = 0;
kamorei 0:8b8f5dba70e9 36 }
kamorei 0:8b8f5dba70e9 37
kamorei 0:8b8f5dba70e9 38 void turn_left(){
kamorei 0:8b8f5dba70e9 39 Mrighti = 2;
kamorei 0:8b8f5dba70e9 40 Mrightp = 0.7f;
kamorei 0:8b8f5dba70e9 41 Mlefti = 1;
kamorei 0:8b8f5dba70e9 42 Mleftp = 0.7f;
kamorei 0:8b8f5dba70e9 43 ledR = 0;
kamorei 0:8b8f5dba70e9 44 ledL = 1;
kamorei 0:8b8f5dba70e9 45 }
kamorei 0:8b8f5dba70e9 46
kamorei 0:8b8f5dba70e9 47 void go_straight( float r, float l){
kamorei 0:8b8f5dba70e9 48 Mrighti = 2;
kamorei 0:8b8f5dba70e9 49 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 50 Mlefti = 2;
kamorei 0:8b8f5dba70e9 51 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 52 ledR = 0;
kamorei 0:8b8f5dba70e9 53 ledL = 0;
kamorei 0:8b8f5dba70e9 54 }
kamorei 0:8b8f5dba70e9 55
kamorei 0:8b8f5dba70e9 56 void go_back( float r, float l){
kamorei 0:8b8f5dba70e9 57 Mrighti = 1;
kamorei 0:8b8f5dba70e9 58 Mrightp = (kp * pr + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 59 Mlefti = 1;
kamorei 0:8b8f5dba70e9 60 Mleftp = (kp * pl + 0.5) * 1.0f;
kamorei 0:8b8f5dba70e9 61 ledR = 0;
kamorei 0:8b8f5dba70e9 62 ledL = 0;
kamorei 0:8b8f5dba70e9 63 }
kamorei 0:8b8f5dba70e9 64
kamorei 0:8b8f5dba70e9 65 void stop_point(){
kamorei 0:8b8f5dba70e9 66 Mrighti = 0;
kamorei 0:8b8f5dba70e9 67 Mlefti = 0;
kamorei 0:8b8f5dba70e9 68 ledRR = 2;
kamorei 0:8b8f5dba70e9 69 ledLL = 2;
kamorei 0:8b8f5dba70e9 70 }
kamorei 0:8b8f5dba70e9 71
kamorei 0:8b8f5dba70e9 72 int main() {
kamorei 0:8b8f5dba70e9 73 go_straight();
kamorei 0:8b8f5dba70e9 74 wait(0.2);
kamorei 0:8b8f5dba70e9 75 while( i < 3){
kamorei 0:8b8f5dba70e9 76 while(1) {
kamorei 0:8b8f5dba70e9 77 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 78 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 79 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 80 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 81
kamorei 0:8b8f5dba70e9 82 pr = (sensor[2] - black) / (white - black);
kamorei 0:8b8f5dba70e9 83 pl = (sensor[1] - black) / (white - black);
kamorei 0:8b8f5dba70e9 84
kamorei 0:8b8f5dba70e9 85 if( sensor[2] <= gray)
kamorei 0:8b8f5dba70e9 86 turn_right();
kamorei 0:8b8f5dba70e9 87 else if( sensor[1] <= gray)
kamorei 0:8b8f5dba70e9 88 turn_left();
kamorei 0:8b8f5dba70e9 89 else if( sensor[1] <= gray && sensor[2] <= gray){
kamorei 0:8b8f5dba70e9 90 if(ledR == 1)
kamorei 0:8b8f5dba70e9 91 turn_right;
kamorei 0:8b8f5dba70e9 92 else
kamorei 0:8b8f5dba70e9 93 turn_left;
kamorei 0:8b8f5dba70e9 94 }
kamorei 0:8b8f5dba70e9 95 else
kamorei 0:8b8f5dba70e9 96 go_straight( pr, pl);
kamorei 0:8b8f5dba70e9 97 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 0:8b8f5dba70e9 98 stop_point();
kamorei 0:8b8f5dba70e9 99 break;
kamorei 0:8b8f5dba70e9 100 }
kamorei 0:8b8f5dba70e9 101 }
kamorei 0:8b8f5dba70e9 102 i++;
kamorei 0:8b8f5dba70e9 103 if( i < 3){
kamorei 0:8b8f5dba70e9 104 /* while(1){
kamorei 0:8b8f5dba70e9 105 sensor[0] = sensorL.read();
kamorei 0:8b8f5dba70e9 106 sensor[1] = sensorCL.read();
kamorei 0:8b8f5dba70e9 107 sensor[2] = sensorCR.read();
kamorei 0:8b8f5dba70e9 108 sensor[3] = sensorR.read();
kamorei 0:8b8f5dba70e9 109
kamorei 0:8b8f5dba70e9 110 pr = (sensor[2] - black) / (white - black);
kamorei 0:8b8f5dba70e9 111 pl = (sensor[1] - black) / (white - black);
kamorei 0:8b8f5dba70e9 112
kamorei 0:8b8f5dba70e9 113 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 0:8b8f5dba70e9 114 stop_point();
kamorei 0:8b8f5dba70e9 115 break;
kamorei 0:8b8f5dba70e9 116 }
kamorei 0:8b8f5dba70e9 117 else
kamorei 0:8b8f5dba70e9 118 go_back( pr, pl);
kamorei 0:8b8f5dba70e9 119 }
kamorei 0:8b8f5dba70e9 120 */
kamorei 0:8b8f5dba70e9 121 for( j = 0; j < 2;){
kamorei 0:8b8f5dba70e9 122 turn_left();
kamorei 0:8b8f5dba70e9 123 if( sensor[2] <= black)
kamorei 0:8b8f5dba70e9 124 j++;
kamorei 0:8b8f5dba70e9 125 }
kamorei 0:8b8f5dba70e9 126 }
kamorei 0:8b8f5dba70e9 127 }
kamorei 0:8b8f5dba70e9 128 }