Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termA

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@4:b81dc4282175, 2018-10-29 (annotated)

Committer:
kamorei
Date:
Mon Oct 29 16:21:58 2018 +0000
Revision:
4:b81dc4282175
Parent:
3:bc6c111b88da 
10/29 ???A??? P?????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 4:b81dc4282175 1 //直線だけP制御したら直線でブレなくなるかもしれないので作りました
kamorei 4:b81dc4282175 2
kamorei 0:0abe8038e775 3 #include "mbed.h"
kamorei 0:0abe8038e775 4
kamorei 0:0abe8038e775 5 DigitalOut ledL( PTB8);
kamorei 0:0abe8038e775 6 DigitalOut ledR( PTE5);
kamorei 0:0abe8038e775 7 BusOut ledLL( PTB8, PTB9);
kamorei 0:0abe8038e775 8 BusOut ledRR( PTE5, PTE4); //わざと右から書いてます
kamorei 0:0abe8038e775 9
kamorei 0:0abe8038e775 10 AnalogIn sensorR( PTB1);
kamorei 0:0abe8038e775 11 AnalogIn sensorL( PTB3);
kamorei 0:0abe8038e775 12 AnalogIn sensorCR( PTB0);
kamorei 0:0abe8038e775 13 AnalogIn sensorCL( PTB2);
kamorei 0:0abe8038e775 14
kamorei 0:0abe8038e775 15 //モータ1
kamorei 0:0abe8038e775 16 BusOut Mlefti(PTA1, PTA2);
kamorei 0:0abe8038e775 17 PwmOut Mleftp(PTD4);
kamorei 0:0abe8038e775 18 //モータ2
kamorei 0:0abe8038e775 19 BusOut Mrighti(PTC0, PTC7);
kamorei 0:0abe8038e775 20 PwmOut Mrightp(PTA12);
kamorei 0:0abe8038e775 21
kamorei 4:b81dc4282175 22 float white = 0.6, black = 0.08, gray = 0.1; //値をぶち込む
kamorei 1:5a97a5e4ee44 23 float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
kamorei 0:0abe8038e775 24 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 4:b81dc4282175 25 float kp = 0.85; //Pゲイン
kamorei 4:b81dc4282175 26 float pr, pl;
kamorei 0:0abe8038e775 27
kamorei 4:b81dc4282175 28 void go_straight_p(){
kamorei 4:b81dc4282175 29 if( pr < (gray - black) / (white - black)){
kamorei 4:b81dc4282175 30 Mrighti = 1;
kamorei 4:b81dc4282175 31 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 4:b81dc4282175 32 Mlefti = 2;
kamorei 4:b81dc4282175 33 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 4:b81dc4282175 34 ledR = 1;
kamorei 4:b81dc4282175 35 ledL = 0;
kamorei 4:b81dc4282175 36 } else if( pl < (gray - black) / (white - black)){
kamorei 4:b81dc4282175 37 Mrighti = 2;
kamorei 4:b81dc4282175 38 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 4:b81dc4282175 39 Mlefti = 1;
kamorei 4:b81dc4282175 40 Mleftp = (kp * pl + 0.3) * 1.0f;
kamorei 4:b81dc4282175 41 ledR = 0;
kamorei 4:b81dc4282175 42 ledL = 1;
kamorei 4:b81dc4282175 43 } else{
kamorei 4:b81dc4282175 44 Mrighti = 2;
kamorei 4:b81dc4282175 45 Mrightp = (kp * pr + 0.3) * 1.0f;
kamorei 4:b81dc4282175 46 Mlefti = 2;
kamorei 4:b81dc4282175 47 Mleftp = (kp * pl - 0.1) * 1.0f;
kamorei 4:b81dc4282175 48 ledR = 0;
kamorei 4:b81dc4282175 49 ledL = 0;
kamorei 4:b81dc4282175 50 }
kamorei 0:0abe8038e775 51 }
kamorei 0:0abe8038e775 52
kamorei 4:b81dc4282175 53 void go_straight_check(){ //モータドライバの調子の確認用
kamorei 0:0abe8038e775 54 Mrighti = 2;
kamorei 4:b81dc4282175 55 Mrightp = 0.8f;
kamorei 0:0abe8038e775 56 Mlefti = 2;
kamorei 4:b81dc4282175 57 Mleftp = 0.8f;
kamorei 0:0abe8038e775 58 }
kamorei 0:0abe8038e775 59
kamorei 0:0abe8038e775 60 void stop_point(){
kamorei 4:b81dc4282175 61 Mrighti = 1;
kamorei 4:b81dc4282175 62 Mrightp = 0.5f;
kamorei 4:b81dc4282175 63 Mlefti = 1;
kamorei 4:b81dc4282175 64 Mleftp = 0.5f;
kamorei 4:b81dc4282175 65 wait(0.05);
kamorei 0:0abe8038e775 66 Mrighti = 0;
kamorei 0:0abe8038e775 67 Mlefti = 0;
kamorei 0:0abe8038e775 68 ledRR = 2;
kamorei 0:0abe8038e775 69 ledLL = 2;
kamorei 0:0abe8038e775 70 }
kamorei 0:0abe8038e775 71
kamorei 0:0abe8038e775 72 int main() {
kamorei 4:b81dc4282175 73 go_straight_check();
kamorei 2:bbd10b5da0bf 74 wait(0.2);
kamorei 0:0abe8038e775 75 while(1) {
kamorei 0:0abe8038e775 76 sensor[0] = sensorL.read();
kamorei 0:0abe8038e775 77 sensor[1] = sensorCL.read();
kamorei 0:0abe8038e775 78 sensor[2] = sensorCR.read();
kamorei 0:0abe8038e775 79 sensor[3] = sensorR.read();
kamorei 0:0abe8038e775 80
kamorei 4:b81dc4282175 81 pr = (sensor[2] - black) / (white - black);
kamorei 4:b81dc4282175 82 pl = (sensor[1] - black) / (white - black);
kamorei 4:b81dc4282175 83
kamorei 4:b81dc4282175 84 go_straight_p();
kamorei 3:bc6c111b88da 85 if( sensor[0] <= black && sensor[3] <= blackR){
kamorei 1:5a97a5e4ee44 86 stop_point();
kamorei 1:5a97a5e4ee44 87 break;
kamorei 1:5a97a5e4ee44 88 }
kamorei 0:0abe8038e775 89 }
kamorei 4:b81dc4282175 90 }