Diff: main.cpp

Revision:

2:d857188289be

Parent:

1:0fe1b3af8053

Child:

3:117706ca64d9

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