![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ほぼ完成
Fork of f3rc_3_auto_lintrace_905 by
Diff: main.cpp
- Revision:
- 1:0fe1b3af8053
- Parent:
- 0:260f1d62f487
- Child:
- 2:d857188289be
diff -r 260f1d62f487 -r 0fe1b3af8053 main.cpp --- a/main.cpp Thu Aug 23 03:09:55 2018 +0000 +++ b/main.cpp Wed Aug 29 06:31:52 2018 +0000 @@ -1,7 +1,9 @@ #include "mbed.h" -#include "math.h" -DigitalIn cylinder(p9,PullUp); -DigitalIn cylindert(p10,PullUp); +#include "Math.h" +#include "Ec.h" +#include "SpeedController.h" +DigitalOut cylinder(p9); +DigitalOut cylindert(p10); DigitalIn encoder1a(p11); DigitalIn encoder1b(p12); DigitalIn encoder2a(p13); @@ -14,13 +16,42 @@ PwmOut servo3(p22); PwmOut motor1(p25); PwmOut motor2(p26); -DigitalIn motor1cw(p27,PullUp); -DigitalIn motor2cw(p28,PullUp); +DigitalOut motor1cw(p27); +DigitalOut motor2cw(p28); - - +int old3,old4,now3,now4 = 0; +double basePower; +double Kp = ; +double Kd = ; +void linetrace; int main() { + motor1.period_us(50); + motor2.period_us(50); + motor1cw = 0; + motor2cw = 1; + basePower = 0.5; + while(LBR1.read() > && LBR2.read() > ) {//ライントレース部分 + linetrace(); + } + while(/*条件をかけや*/) {//ラインによる位置調整 + double Kp = ; + motor1cw = 0; + motor2cw = 1; + motor1 = + motor2 = + } +} + +void linetrace() +{ + old3 = now3; + old4 = now4; + now3 = 1000*LBR3.read(); + now4 = 1000*LBR4.read(); + double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4)); + motor1 = basePower + steering; + motor2 = basePower - steering; } \ No newline at end of file