mbed_robotcar / Mbed OS pid

p制御（失敗）

main.cpp@0:2c87f9a531df, 2020-08-31 (annotated)

Committer:
yangtzuli
Date:
Mon Aug 31 07:54:01 2020 +0000
Revision:
0:2c87f9a531df
P

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yangtzuli 0:2c87f9a531df 1 #include "mbed.h"
yangtzuli 0:2c87f9a531df 2 #include "rtos.h"
yangtzuli 0:2c87f9a531df 3 #include <stdint.h>
yangtzuli 0:2c87f9a531df 4 #include "platform/mbed_thread.h"
yangtzuli 0:2c87f9a531df 5
yangtzuli 0:2c87f9a531df 6 RawSerial pc(USBTX, USBRX);
yangtzuli 0:2c87f9a531df 7
yangtzuli 0:2c87f9a531df 8 AnalogIn ss1(p20); // ライントレースセンサ(左)
yangtzuli 0:2c87f9a531df 9 AnalogIn ss2(p19); // ライントレースセンサ
yangtzuli 0:2c87f9a531df 10 AnalogIn ss3(p18); // ライントレースセンサ
yangtzuli 0:2c87f9a531df 11 AnalogIn ss4(p17); // ライントレースセンサ
yangtzuli 0:2c87f9a531df 12 AnalogIn ss5(p16); // ライントレースセンサ(右)
yangtzuli 0:2c87f9a531df 13 PwmOut motorR2(p21); // 右モーター後退
yangtzuli 0:2c87f9a531df 14 PwmOut motorR1(p22); // 右モーター前進
yangtzuli 0:2c87f9a531df 15 PwmOut motorL2(p23); // 左モーター後退
yangtzuli 0:2c87f9a531df 16 PwmOut motorL1(p24); // 左モーター前進
yangtzuli 0:2c87f9a531df 17
yangtzuli 0:2c87f9a531df 18
yangtzuli 0:2c87f9a531df 19 void trace();
yangtzuli 0:2c87f9a531df 20
yangtzuli 0:2c87f9a531df 21 /* ライントレーススレッド */
yangtzuli 0:2c87f9a531df 22 void trace(){
yangtzuli 0:2c87f9a531df 23 while(1){
yangtzuli 0:2c87f9a531df 24 /* 各センサー値読み取り */
yangtzuli 0:2c87f9a531df 25 float sensor1 = ss1;
yangtzuli 0:2c87f9a531df 26 float sensor2 = ss2;
yangtzuli 0:2c87f9a531df 27 float sensor3 = ss3;
yangtzuli 0:2c87f9a531df 28 float sensor4 = ss4;
yangtzuli 0:2c87f9a531df 29 float sensor5 = ss5;
yangtzuli 0:2c87f9a531df 30 float speed;
yangtzuli 0:2c87f9a531df 31 pc.printf("%f %f %f %f %f\r\n",sensor1, sensor2, sensor3, sensor4, sensor5);
yangtzuli 0:2c87f9a531df 32 speed=(0.3-sensor3)*0.33;
yangtzuli 0:2c87f9a531df 33 pc.printf("%f\r\n",speed);
yangtzuli 0:2c87f9a531df 34 if((sensor2-sensor4)>0.01||(sensor1-sensor4)>0.01){
yangtzuli 0:2c87f9a531df 35 motorR1 = 0;
yangtzuli 0:2c87f9a531df 36 motorR2 = 0.3 + speed;
yangtzuli 0:2c87f9a531df 37 motorL1 = 0.3 - speed;
yangtzuli 0:2c87f9a531df 38 motorL2 = 0;
yangtzuli 0:2c87f9a531df 39 }else if((sensor4-sensor2)>=0.01||(sensor5-sensor2)>=0.01){
yangtzuli 0:2c87f9a531df 40 motorR1 = 0;
yangtzuli 0:2c87f9a531df 41 motorR2 = 0.3 - speed;
yangtzuli 0:2c87f9a531df 42 motorL1 = 0.3 + speed;
yangtzuli 0:2c87f9a531df 43 motorL2 = 0;
yangtzuli 0:2c87f9a531df 44 }else{
yangtzuli 0:2c87f9a531df 45
yangtzuli 0:2c87f9a531df 46 }
yangtzuli 0:2c87f9a531df 47 /*if(sensor2>=sensor4){
yangtzuli 0:2c87f9a531df 48 motorR1 = 0;
yangtzuli 0:2c87f9a531df 49 motorR2 = 0.6 + 制御量;
yangtzuli 0:2c87f9a531df 50 motorL1 = 0.6 - 制御量;
yangtzuli 0:2c87f9a531df 51 motorL2 = 0;
yangtzuli 0:2c87f9a531df 52 }
yangtzuli 0:2c87f9a531df 53 else if(sensor2<sensor4){
yangtzuli 0:2c87f9a531df 54 motorR1 = 0;
yangtzuli 0:2c87f9a531df 55 motorR2 = 0.6 - 制御量;
yangtzuli 0:2c87f9a531df 56 motorL1 = 0.6 + 制御量;
yangtzuli 0:2c87f9a531df 57 motorL2 = 0;
yangtzuli 0:2c87f9a531df 58 }*/
yangtzuli 0:2c87f9a531df 59 }
yangtzuli 0:2c87f9a531df 60
yangtzuli 0:2c87f9a531df 61 }
yangtzuli 0:2c87f9a531df 62 int main(){
yangtzuli 0:2c87f9a531df 63 trace();
yangtzuli 0:2c87f9a531df 64 }