![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ほぼ完成
Fork of f3rc_3_auto_lintrace_905 by
Diff: main.cpp
- Revision:
- 3:117706ca64d9
- Parent:
- 2:d857188289be
- Child:
- 4:ca2e863a3d71
diff -r d857188289be -r 117706ca64d9 main.cpp --- a/main.cpp Tue Sep 04 07:43:54 2018 +0000 +++ b/main.cpp Wed Sep 05 07:40:36 2018 +0000 @@ -3,6 +3,9 @@ #include "SpeedController.h" #include <math.h> +#define MEASURE_TIME 0.05 +#define WHEEL_WIDTH 202 + DigitalOut cylinder(p9); DigitalOut cylindert(p10); AnalogIn LBR1(p17); @@ -18,8 +21,6 @@ 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; @@ -29,10 +30,13 @@ double Threshold2 = ; double Threshold3 = ; double Threshold4 = ; -void linetrace(basePower); +void linetrace(double basePower); +void move(double RP,double LP,double RD,double LD); int main() { + servo12.period_ms(20); + servo3.period_ms(20); motor1.period_us(50); motor2.period_us(50); servo12.period_ms(50); @@ -53,6 +57,7 @@ motor1 = fabs(kp*(LBR3.read() - Threshold3)); motor2 = fabs(kp*(LBR4.read() - Threshold4)); } + t1.reset(); servo12.pulsewidth_us();//つかんで wait(1); servo3.pulsewidth_us();//持ち上げて @@ -61,17 +66,36 @@ wait(1); ec1.reset(); sc2.reset(); - while(ec1.getCount() > 6778 || ec1.getCount() > 3245) { + move() + while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) { motor1cw = 0; motor2cw = 1; motor1 = 0.6; motor2 = 0.6; } - while(ec1.getCount() > 6778 || ec1.getCount() > 3245) { + while(ec1.getDistance_mm() > 3245) { motor2cw = 1; motor1 = 0; motor2 = 0.6; } + while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) { + motor1cw = 1; + motor2cw = 0; + motor1 = 0.6; + motor2 = 0.6; + } + while(ec1.getDistance_mm() > 3245) { + motor1cw = 1; + motor1 = 0; + motor2 = 0.6; + } + while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) { + motor1cw = 1; + motor2cw = 0; + motor1 = 0.6; + motor2 = 0.6; + } + //エアシリンダー } @@ -86,4 +110,21 @@ motor2cw = !signbit(basePower - steering);//モーターのパワーの符号 motor1 = fabs(basePower + steering); motor2 = fabs(basePower - steering); -} \ No newline at end of file +} + + + +void move(double RP,double LP,double RD,double LD)//右モーターパワー,左モーターパワー,右モーター移動距離mm,左モーター移動距離mm +{ + mortor1.period_us(50); + mortor2.period_us(50); + ec1.setDiameter_mm(150); + ec2.setDiameter_mm(150); + ec1.reset(); + ec2.reset(); + while(fabs(ec1.getDistance_mm) <= RD && fabs(EC2_distance) <= LD) { + motor1cw = signbit(RP); //モータ1の回転方向 + motor2cw = signbit(LP); //モータ2の回転方向 + motor1 = RP; //モータ1への出力 + motor2 = LP; //モータ2への出力 + } \ No newline at end of file