Diff: main.cpp

Revision:

2:55c616d2e0fe

Parent:

1:86c4c38abe40

Child:

3:7a608fbd3bcd

diff -r 86c4c38abe40 -r 55c616d2e0fe main.cpp
--- a/main.cpp	Sat Apr 27 04:28:33 2019 +0000
+++ b/main.cpp	Sat Apr 27 05:41:33 2019 +0000
@@ -10,6 +10,7 @@
 void pid(double,double);
 void out_lo(double);
 void out_li(double);
+void reset();
 
 ////////////定数
 int solution=1000;
@@ -43,8 +44,9 @@
 {
 
     setup();
-    
+    reset();
     while(1) {
+       
        can_send();
        pid(target_lo,target_li);
         wait(0.01);
@@ -61,8 +63,8 @@
     motor_li_b.period_us(100);
 
     hand.mode(PullUp);
-    switch2.mode(PullUp);
-    switch3.mode(PullUp);
+    switch_lo.mode(PullUp);
+    switch_li.mode(PullUp);
     switch4.mode(PullUp);
 
 
@@ -91,6 +93,20 @@
     else led4=0;
 }
 
+void reset()
+{
+    while(switch_lo.read()) {
+        out_lo(0.05);
+    }
+     ec_lo.reset();
+     out_lo(0);
+    while(switch_li.read()) {
+        out_li(0.05);
+    }
+    
+     ec_li.reset();
+     out_li(0);
+}
 
 void out_lo(double duty)
 {