mbed_robotcar
/
pid
p制御(失敗)
main.cpp
- Committer:
- yangtzuli
- Date:
- 2020-08-31
- Revision:
- 0:2c87f9a531df
File content as of revision 0:2c87f9a531df:
#include "mbed.h" #include "rtos.h" #include <stdint.h> #include "platform/mbed_thread.h" RawSerial pc(USBTX, USBRX); AnalogIn ss1(p20); // ライントレースセンサ(左) AnalogIn ss2(p19); // ライントレースセンサ AnalogIn ss3(p18); // ライントレースセンサ AnalogIn ss4(p17); // ライントレースセンサ AnalogIn ss5(p16); // ライントレースセンサ(右) PwmOut motorR2(p21); // 右モーター後退 PwmOut motorR1(p22); // 右モーター前進 PwmOut motorL2(p23); // 左モーター後退 PwmOut motorL1(p24); // 左モーター前進 void trace(); /* ライントレーススレッド */ void trace(){ while(1){ /* 各センサー値読み取り */ float sensor1 = ss1; float sensor2 = ss2; float sensor3 = ss3; float sensor4 = ss4; float sensor5 = ss5; float speed; pc.printf("%f %f %f %f %f\r\n",sensor1, sensor2, sensor3, sensor4, sensor5); speed=(0.3-sensor3)*0.33; pc.printf("%f\r\n",speed); if((sensor2-sensor4)>0.01||(sensor1-sensor4)>0.01){ motorR1 = 0; motorR2 = 0.3 + speed; motorL1 = 0.3 - speed; motorL2 = 0; }else if((sensor4-sensor2)>=0.01||(sensor5-sensor2)>=0.01){ motorR1 = 0; motorR2 = 0.3 - speed; motorL1 = 0.3 + speed; motorL2 = 0; }else{ } /*if(sensor2>=sensor4){ motorR1 = 0; motorR2 = 0.6 + 制御量; motorL1 = 0.6 - 制御量; motorL2 = 0; } else if(sensor2<sensor4){ motorR1 = 0; motorR2 = 0.6 - 制御量; motorL1 = 0.6 + 制御量; motorL2 = 0; }*/ } } int main(){ trace(); }