ROBOSTEP_LIBRARY
/
f3rc_3_auto_lintrace_912
ほぼ完成
Fork of f3rc_3_auto_lintrace_905 by
Diff: main.cpp
- Revision:
- 2:d857188289be
- Parent:
- 1:0fe1b3af8053
- Child:
- 3:117706ca64d9
diff -r 0fe1b3af8053 -r d857188289be main.cpp --- a/main.cpp Wed Aug 29 06:31:52 2018 +0000 +++ b/main.cpp Tue Sep 04 07:43:54 2018 +0000 @@ -1,17 +1,14 @@ #include "mbed.h" -#include "Math.h" -#include "Ec.h" +#include "EC.h" #include "SpeedController.h" +#include <math.h> + DigitalOut cylinder(p9); DigitalOut cylindert(p10); -DigitalIn encoder1a(p11); -DigitalIn encoder1b(p12); -DigitalIn encoder2a(p13); -DigitalIn encoder2b(p14); -AnalogIn LBR1(p17,PullUp); -AnalogIn LBR2(p18,PullUp); -AnalogIn LBR3(p19,PullUp); -AnalogIn LBR4(p20,PullUp); +AnalogIn LBR1(p17); +AnalogIn LBR2(p18); +AnalogIn LBR3(p19); +AnalogIn LBR4(p20); PwmOut servo12(p21); PwmOut servo3(p22); PwmOut motor1(p25); @@ -19,39 +16,74 @@ DigitalOut motor1cw(p27); DigitalOut motor2cw(p28); +Ec ec1(p11,p12,NC,500,MEASURE_TIME);//(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) +Ec ec2(p14,p13,NC,500,MEASURE_TIME); +servo12.period_ms(20); +servo3.period_ms(20); +Timer t1; + int old3,old4,now3,now4 = 0; -double basePower; double Kp = ; double Kd = ; -void linetrace; +double Threshold1 = ; +double Threshold2 = ; +double Threshold3 = ; +double Threshold4 = ; +void linetrace(basePower); int main() { motor1.period_us(50); motor2.period_us(50); - motor1cw = 0; - motor2cw = 1; - basePower = 0.5; - while(LBR1.read() > && LBR2.read() > ) {//ライントレース部分 - linetrace(); + servo12.period_ms(50); + servo3.period_ms(50); + servo12.pulsewidth_us(); + servo3.pulsewidth_us(); + while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1 + linetrace(0.5); + } + while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2 + linetrace(0.15); } - - while(/*条件をかけや*/) {//ラインによる位置調整 - double Kp = ; + t1.start(); + while(t1.read() < 2) { //ラインによる位置調整 + double kp = -; + motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆 + motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆 + motor1 = fabs(kp*(LBR3.read() - Threshold3)); + motor2 = fabs(kp*(LBR4.read() - Threshold4)); + } + servo12.pulsewidth_us();//つかんで + wait(1); + servo3.pulsewidth_us();//持ち上げて + wait(1); + servo12.pulsewidth_us();//入れる + wait(1); + ec1.reset(); + sc2.reset(); + while(ec1.getCount() > 6778 || ec1.getCount() > 3245) { motor1cw = 0; motor2cw = 1; - motor1 = - motor2 = + motor1 = 0.6; + motor2 = 0.6; + } + while(ec1.getCount() > 6778 || ec1.getCount() > 3245) { + motor2cw = 1; + motor1 = 0; + motor2 = 0.6; } } -void linetrace() + +void linetrace(double basePower) { 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; + motor1cw = !signbit(basePower + steering);//モーターのパワーの符号 + motor2cw = !signbit(basePower - steering);//モーターのパワーの符号 + motor1 = fabs(basePower + steering); + motor2 = fabs(basePower - steering); } \ No newline at end of file