Diff: main.cpp

Revision:

0:da183064372e

Child:

1:0ff59878537a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 03 05:00:04 2018 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "Math.h"
+#include "Ec.h"
+#include "SpeedController.h"
+DigitalIn cylinder(p9,PullUp);
+DigitalIn cylindert(p10,PullUp);
+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);
+PwmOut servo12(p21);
+PwmOut servo3(p22);
+PwmOut motor1(p25);
+PwmOut motor2(p26);
+DigitalIn motor1cw(p27,PullUp);
+DigitalIn motor2cw(p28,PullUp);
+
+EC wheel_1 (p11,p12,/*z相*/,/*分解能*/,0.05)
+EC wheel_2 (p13,p14,/*z相*/,/*分解能*/,0.05)
+//SpeedControl (p11,p12,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも
+//SpeedControl (p13,p14,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも
+
+/*直進*/
+void straight(double t,int l)  //t=時間 l=移動距離
+{
+    mortor1.period_us(50);
+    mortor2.period_us(50);
+    double EC1_omega = wheel_1.getOmega();
+    double EC2_omega = wheel_2.getOmega();
+    double L_1 = EC1_omega*"ecの半径"*t;
+    double L_2 = EC2_omega*"ecの半径"*t;
+    while(L_1 <= l && L_2 <= l) {
+        motor1 = ;
+        motor2 = ;
+        motor1cw = ;
+        motor2cw = ;
+    }
+}
+
+/*右回転*/
+void right(double t,int l)  //t=時間 l=機体の中心を回転中心としたときのエンコーダの移動距離
+{
+    mortor1.period_us(50);
+    mortor2.period_us(50);
+    double EC1_omega = wheel_1.getOmega();
+    double EC2_omega = wheel_2.getOmega();
+    double L_1 = EC1_omega*"ecの半径"*t;
+    double L_2 = EC2_omega*"ecの半径"*t;
+    while(L_1 <= l && L_2 <= -1*l) {
+        motor1 = ;
+        motor2 = ;
+        motor1cw = ;
+        motor2cw = ;
+    }
+}
+
+/*左回転*/
+void left(double t,int l)  //t=時間 l=l=機体の中心を回転中心としたときのエンコーダの移動距離
+{
+    mortor1.period_us(50);
+    mortor2.period_us(50);
+    double EC1_omega = wheel_1.getOmega();
+    double EC2_omega = wheel_2.getOmega();
+    double L_1 = EC1_omega*"ecの半径"*t;
+    double L_2 = EC2_omega*"ecの半径"*t;
+    while(L_1 <= -1*l && L_2 <= l) {
+        motor1 = ;
+        motor2 = ;
+        motor1cw = ;
+        motor2cw = ;
+    }
+}
\ No newline at end of file