mbed_robotcar
/
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?
User | Revision | Line number | New 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 | } |