Diff: main.cpp

Revision:

3:117706ca64d9

Parent:

2:d857188289be

Child:

4:ca2e863a3d71

--- 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