Revision 4:ca2e863a3d71, committed 2018-09-12

Comitter:

Shunsaku

Date:

Wed Sep 12 02:37:14 2018 +0000

Parent:

3:117706ca64d9

Commit message:

9/12

Changed in this revision

diff -r 117706ca64d9 -r ca2e863a3d71 main.cpp
--- a/main.cpp	Wed Sep 05 07:40:36 2018 +0000
+++ b/main.cpp	Wed Sep 12 02:37:14 2018 +0000
@@ -4,10 +4,10 @@
 #include <math.h>
 
 #define MEASURE_TIME 0.05
-#define WHEEL_WIDTH 202
+#define WHEEL_WIDTH 210
+#define pi 3.141592
 
 DigitalOut cylinder(p9);
-DigitalOut cylindert(p10);
 AnalogIn LBR1(p17);
 AnalogIn LBR2(p18);
 AnalogIn LBR3(p19);
@@ -24,12 +24,18 @@
 Timer t1;
 
 int old3,old4,now3,now4 = 0;
-double Kp = ;
-double Kd = ;
-double Threshold1 = ;
-double Threshold2 = ;
-double Threshold3 = ;
-double Threshold4 = ;
+double Kp = 1.0;//要調整
+double Kd = 1000;//要調整
+double Threshold1 = 0.5;//要調整
+double Threshold2 = 0.5;//要調整
+double Threshold3 = 0.5;//要調整
+double Threshold4 = 0.5;//要調整
+int s12open = 1000;//要修正
+int s12close = 1000;//要修正
+int s3up = 1000;//要修正
+int s3down = 1000;//要修正
+double turnDistance = WHEEL_WIDTH*pi*0.5;
+
 void linetrace(double basePower);
 void move(double RP,double LP,double RD,double LD);
 
@@ -39,63 +45,37 @@
     servo3.period_ms(20);
     motor1.period_us(50);
     motor2.period_us(50);
-    servo12.period_ms(50);
-    servo3.period_ms(50);
-    servo12.pulsewidth_us();
-    servo3.pulsewidth_us();
+    servo12.pulsewidth_us(s12close);
+    servo3.pulsewidth_us(s3down);
+    cylinder = 0;
     while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
         linetrace(0.5);
     }
+    servo12.pulsewidth_us(s12open);
     while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
         linetrace(0.15);
     }
     t1.start();
     while(t1.read() < 2) { //ラインによる位置調整
-        double kp = -;
+        double kp = -1.0;//修正しろ
         motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
         motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
         motor1 = fabs(kp*(LBR3.read() - Threshold3));
         motor2 = fabs(kp*(LBR4.read() - Threshold4));
     }
     t1.reset();
-    servo12.pulsewidth_us();//つかんで
+    servo12.pulsewidth_us(s12close);//つかんで
     wait(1);
-    servo3.pulsewidth_us();//持ち上げて
+    servo3.pulsewidth_us(s3up);//持ち上げて
     wait(1);
-    servo12.pulsewidth_us();//入れる
+    servo12.pulsewidth_us(s12open);//入れる
     wait(1);
-    ec1.reset();
-    sc2.reset();
-    move()
-    while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
-        motor1cw = 0;
-        motor2cw = 1;
-        motor1 = 0.6;
-        motor2 = 0.6;
-    }
-    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;
-    }
-    //エアシリンダー
+    move(-0.6,-0.6,600,600);
+    move(-0.6,0,turnDistance,0);
+    move(0.6,0.6,600,600);
+    move(0.6,0,turnDistance,0);
+    move(0.6,0.6,300,300);
+    cylinder = 1;//エアシリンダー
 }
 
 
@@ -114,17 +94,18 @@
 
 
 
-void move(double RP,double LP,double RD,double LD)//右モーターパワー,左モーターパワー,右モーター移動距離mm,左モーター移動距離mm
+void move(double RP,double LP,double RD,double LD)//右モーターパワー(-1~1),左モーターパワー(-1~1),右モーター移動距離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
+    while(fabs(ec1.getDistance_mm()) > RD && fabs(ec2.getDistance_mm()) > LD) {
+        motor1cw = !signbit(RP); //モータ1の回転方向
+        motor2cw = !signbit(LP); //モータ2の回転方向
+        motor1 = fabs(RP); //モータ1への出力
+        motor2 = fabs(LP); //モータ2への出力
+    }
+    motor1 = 0;
+    motor2 = 0;
+}
\ No newline at end of file