Diff: main.cpp

Revision:

1:0fe1b3af8053

Parent:

0:260f1d62f487

Child:

2:d857188289be

--- a/main.cpp	Thu Aug 23 03:09:55 2018 +0000
+++ b/main.cpp	Wed Aug 29 06:31:52 2018 +0000
@@ -1,7 +1,9 @@
 #include "mbed.h"
-#include "math.h"
-DigitalIn cylinder(p9,PullUp);
-DigitalIn cylindert(p10,PullUp);
+#include "Math.h"
+#include "Ec.h"
+#include "SpeedController.h"
+DigitalOut cylinder(p9);
+DigitalOut cylindert(p10);
 DigitalIn encoder1a(p11);
 DigitalIn encoder1b(p12);
 DigitalIn encoder2a(p13);
@@ -14,13 +16,42 @@
 PwmOut servo3(p22);
 PwmOut motor1(p25);
 PwmOut motor2(p26);
-DigitalIn motor1cw(p27,PullUp);
-DigitalIn motor2cw(p28,PullUp);
+DigitalOut motor1cw(p27);
+DigitalOut motor2cw(p28);
 
-
-
+int old3,old4,now3,now4 = 0;
+double basePower;
+double Kp = ;
+double Kd = ;
+void linetrace;
 
 int main()
 {
+    motor1.period_us(50);
+    motor2.period_us(50);
+    motor1cw = 0;
+    motor2cw = 1;
+    basePower = 0.5;
+    while(LBR1.read() >  && LBR2.read() > ) {//ライントレース部分
+        linetrace();
+    }
     
+    while(/*条件をかけや*/) {//ラインによる位置調整
+        double Kp = ;
+        motor1cw = 0;
+        motor2cw = 1;
+        motor1 =
+            motor2 =
+    }
+}
+
+void linetrace()
+{
+    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;
 }
\ No newline at end of file