Diff: main.cpp

Revision:

4:dfb5f4860097

Parent:

3:f3ef2e7a9407

Child:

5:7d50cbf6674b

--- a/main.cpp	Tue Sep 04 03:35:45 2018 +0000
+++ b/main.cpp	Tue Sep 04 07:00:33 2018 +0000
@@ -2,12 +2,8 @@
 #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);
+DigitalOut cylinder(p9);
+DigitalOut cylindert(p10);
 AnalogIn LBR1(p17,PullUp);
 AnalogIn LBR2(p18,PullUp);
 AnalogIn LBR3(p19,PullUp);
@@ -16,23 +12,23 @@
 PwmOut servo3(p22);
 PwmOut motor1(p25);
 PwmOut motor2(p26);
-DigitalIn motor1cw(p27,PullUp);
-DigitalIn motor2cw(p28,PullUp);
+DigitalOut motor1cw(p27);
+DigitalOut motor2cw(p28);
 
-EC wheel_1 (p11,p12,/*z相*/,/*分解能*/,0.05)
-EC wheel_2 (p13,p14,/*z相*/,/*分解能*/,0.05)
+EC ec1 (p12,p11,NC,500,0.05)
+EC ec2 (p14,p13,NC,500,0.05)
 
 /*直進用*/
-void straight(int l)  //l=移動距離(mm)
+void straight(int x)  //x=車輪の移動距離(mm)
 {
     mortor1.period_us(50);
     mortor2.period_us(50);
-    wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
-    wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
-    double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
-    double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
-    
-    while(EC1_distance <= l && EC2_distance <= l) {
+    ec1.setDiameter_mm(150);
+    ec2.setDiameter_mm(150);
+    double EC1_distance = ec1.getDistance_mm ();
+    double EC2_distance = ec2.getDistance_mm ();
+
+    while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) {
         motor1 = ;
         motor2 = ;
         motor1cw = ;
@@ -41,16 +37,16 @@
 }
 
 /*後退用*/
-void back(int l)  //l=移動距離(mm)
+void back(int x)  //x=車輪の移動距離(mm)
 {
     mortor1.period_us(50);
     mortor2.period_us(50);
-    wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
-    wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
-    double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
-    double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
-    
-    while(EC1_distance <= l && EC2_distance <= l) {
+    ec1.setDiameter_mm(150);
+    ec2.setDiameter_mm(150);
+    double EC1_distance = ec1.getDistance_mm ();
+    double EC2_distance = ec2.getDistance_mm ();
+
+    while(fabs(EC1_distance) <= l && fabs(EC2_distance) <= l) {
         motor1 = ;
         motor2 = ;
         motor1cw = ;
@@ -59,16 +55,16 @@
 }
 
 /*右回転*/
-void right(int l)  //l=ecの移動距離(mm)
+void right(int x)  //x=車輪の移動距離(mm)
 {
     mortor1.period_us(50);
     mortor2.period_us(50);
-    wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
-    wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
-    double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
-    double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
-    
-    while(EC1_distance <= l && EC2_distance <= -1*l) {
+    ec1.setDiameter_mm(150);
+    ec2.setDiameter_mm(150);
+    double EC1_distance = ec1.getDistance_mm ();
+    double EC2_distance = ec2.getDistance_mm ();
+
+    while(fabs(EC1_distance) <= l && fabs(EC2_distance) <= l) {
         motor1 = ;
         motor2 = ;
         motor1cw = ;
@@ -77,16 +73,16 @@
 }
 
 /*左回転*/
-void left(int l)  //l=ecの移動距離(mm)
+void left(int x)  //x=車輪の移動距離(mm)
 {
     mortor1.period_us(50);
     mortor2.period_us(50);
-    wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
-    wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
-    double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
-    double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
-    
-    while(EC1_distance <= -1*l && EC2_distance <= l) {
+    ec1.setDiameter_mm(150);
+    ec2.setDiameter_mm(150);
+    double EC1_distance = ec1.getDistance_mm ();
+    double EC2_distance = ec2.getDistance_mm ();
+
+    while(fabs(EC1_distance) <= l && fabs(EC2_distance) <= l) {
         motor1 = ;
         motor2 = ;
         motor1cw = ;
@@ -94,5 +90,6 @@
     }
 }
 
-int main(){
-    
\ No newline at end of file
+int main()
+{
+}
\ No newline at end of file