ROBOSTEP_LIBRARY
/
f3rc_3_auto_lintrace_912
ほぼ完成
Fork of f3rc_3_auto_lintrace_905 by
main.cpp@1:0fe1b3af8053, 2018-08-29 (annotated)
- Committer:
- Shunsaku
- Date:
- Wed Aug 29 06:31:52 2018 +0000
- Revision:
- 1:0fe1b3af8053
- Parent:
- 0:260f1d62f487
- Child:
- 2:d857188289be
???????????EC2????????????cpp????????????????; ????????????Math????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Shunsaku | 0:260f1d62f487 | 1 | #include "mbed.h" |
Shunsaku | 1:0fe1b3af8053 | 2 | #include "Math.h" |
Shunsaku | 1:0fe1b3af8053 | 3 | #include "Ec.h" |
Shunsaku | 1:0fe1b3af8053 | 4 | #include "SpeedController.h" |
Shunsaku | 1:0fe1b3af8053 | 5 | DigitalOut cylinder(p9); |
Shunsaku | 1:0fe1b3af8053 | 6 | DigitalOut cylindert(p10); |
Shunsaku | 0:260f1d62f487 | 7 | DigitalIn encoder1a(p11); |
Shunsaku | 0:260f1d62f487 | 8 | DigitalIn encoder1b(p12); |
Shunsaku | 0:260f1d62f487 | 9 | DigitalIn encoder2a(p13); |
Shunsaku | 0:260f1d62f487 | 10 | DigitalIn encoder2b(p14); |
Shunsaku | 0:260f1d62f487 | 11 | AnalogIn LBR1(p17,PullUp); |
Shunsaku | 0:260f1d62f487 | 12 | AnalogIn LBR2(p18,PullUp); |
Shunsaku | 0:260f1d62f487 | 13 | AnalogIn LBR3(p19,PullUp); |
Shunsaku | 0:260f1d62f487 | 14 | AnalogIn LBR4(p20,PullUp); |
Shunsaku | 0:260f1d62f487 | 15 | PwmOut servo12(p21); |
Shunsaku | 0:260f1d62f487 | 16 | PwmOut servo3(p22); |
Shunsaku | 0:260f1d62f487 | 17 | PwmOut motor1(p25); |
Shunsaku | 0:260f1d62f487 | 18 | PwmOut motor2(p26); |
Shunsaku | 1:0fe1b3af8053 | 19 | DigitalOut motor1cw(p27); |
Shunsaku | 1:0fe1b3af8053 | 20 | DigitalOut motor2cw(p28); |
Shunsaku | 0:260f1d62f487 | 21 | |
Shunsaku | 1:0fe1b3af8053 | 22 | int old3,old4,now3,now4 = 0; |
Shunsaku | 1:0fe1b3af8053 | 23 | double basePower; |
Shunsaku | 1:0fe1b3af8053 | 24 | double Kp = ; |
Shunsaku | 1:0fe1b3af8053 | 25 | double Kd = ; |
Shunsaku | 1:0fe1b3af8053 | 26 | void linetrace; |
Shunsaku | 0:260f1d62f487 | 27 | |
Shunsaku | 0:260f1d62f487 | 28 | int main() |
Shunsaku | 0:260f1d62f487 | 29 | { |
Shunsaku | 1:0fe1b3af8053 | 30 | motor1.period_us(50); |
Shunsaku | 1:0fe1b3af8053 | 31 | motor2.period_us(50); |
Shunsaku | 1:0fe1b3af8053 | 32 | motor1cw = 0; |
Shunsaku | 1:0fe1b3af8053 | 33 | motor2cw = 1; |
Shunsaku | 1:0fe1b3af8053 | 34 | basePower = 0.5; |
Shunsaku | 1:0fe1b3af8053 | 35 | while(LBR1.read() > && LBR2.read() > ) {//ライントレース部分 |
Shunsaku | 1:0fe1b3af8053 | 36 | linetrace(); |
Shunsaku | 1:0fe1b3af8053 | 37 | } |
Shunsaku | 0:260f1d62f487 | 38 | |
Shunsaku | 1:0fe1b3af8053 | 39 | while(/*条件をかけや*/) {//ラインによる位置調整 |
Shunsaku | 1:0fe1b3af8053 | 40 | double Kp = ; |
Shunsaku | 1:0fe1b3af8053 | 41 | motor1cw = 0; |
Shunsaku | 1:0fe1b3af8053 | 42 | motor2cw = 1; |
Shunsaku | 1:0fe1b3af8053 | 43 | motor1 = |
Shunsaku | 1:0fe1b3af8053 | 44 | motor2 = |
Shunsaku | 1:0fe1b3af8053 | 45 | } |
Shunsaku | 1:0fe1b3af8053 | 46 | } |
Shunsaku | 1:0fe1b3af8053 | 47 | |
Shunsaku | 1:0fe1b3af8053 | 48 | void linetrace() |
Shunsaku | 1:0fe1b3af8053 | 49 | { |
Shunsaku | 1:0fe1b3af8053 | 50 | old3 = now3; |
Shunsaku | 1:0fe1b3af8053 | 51 | old4 = now4; |
Shunsaku | 1:0fe1b3af8053 | 52 | now3 = 1000*LBR3.read(); |
Shunsaku | 1:0fe1b3af8053 | 53 | now4 = 1000*LBR4.read(); |
Shunsaku | 1:0fe1b3af8053 | 54 | double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4)); |
Shunsaku | 1:0fe1b3af8053 | 55 | motor1 = basePower + steering; |
Shunsaku | 1:0fe1b3af8053 | 56 | motor2 = basePower - steering; |
Shunsaku | 0:260f1d62f487 | 57 | } |