Diff: towelest.cpp

Revision:

2:623fb33351af

Parent:

1:c6950f45b03c

Child:

3:63bb3e19c7eb

--- a/towelest.cpp	Wed Sep 04 05:14:18 2019 +0000
+++ b/towelest.cpp	Fri Sep 13 04:38:47 2019 +0000
@@ -1,15 +1,14 @@
 #include "towelest.h"
 
 towelest::towelest():
-serial(mdTX,mdRX,115200),
-enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING),
-pid(4.0, 0, 0.001, 0.01),
-LimitSW1(PB_5),
-LimitSW2(PB_4)
+    serial(mdTX,mdRX,115200),
+    pid(4.0, 0, 0.001, 0.01),
+    LimitSW1(schmitt_trigger_0),
+    LimitSW2(schmitt_trigger_1)
 {
     Motor[0] = new ikarashiMDC(2,0,SM,&serial);
-    Motor[1] = new ikarashiMDC(2,1,SM,&serial);
-    Motor[2] = new ikarashiMDC(2,2,SM,&serial);
+    Motor[1] = new ikarashiMDC(2,2,SM,&serial);
+    Motor[2] = new ikarashiMDC(2,1,SM,&serial);
 
     Motor[0]->braking = true;
     Motor[1]->braking = true;
@@ -27,15 +26,20 @@
 
 void towelest::open()
 {
-    if(LimitSW1.read() == 1){
+    if(LimitSW1.read() == 1) {
         Motor[0]->setSpeed(0);
         Motor[1]->setSpeed(0);
-    }else if(LimitSW1.read() == 0){
+    } else if(LimitSW1.read() == 0) {
         Motor[0]->setSpeed(-0.2);
         Motor[1]->setSpeed(0);
     }
 }
-
+void towelest::close()
+{
+    Motor[0]->setSpeed(0.2);
+    Motor[1]->setSpeed(0);
+}
+    
 void towelest::lift()
 {
     if(LimitSW2.read() == 1) {
@@ -60,11 +64,15 @@
     }
 }
 
+void towelest::setPulse(int Loli)
+{
+    loli = Loli;
+}
+
 void towelest::loop()
 {
-    while(true){
-        RotationDistance = (float)enc1.getPulses();
-        pid.setProcessValue(RotationDistance);
+    while(true) {
+        pid.setProcessValue(loli);
         compute = pid.compute();
         Motor[2]->setSpeed(compute);
         Limit1 = LimitSW1.read();