Diff: towelest.cpp

Revision:

3:63bb3e19c7eb

Parent:

2:623fb33351af

Child:

4:416d194973a3

--- a/towelest.cpp	Fri Sep 13 04:38:47 2019 +0000
+++ b/towelest.cpp	Tue Sep 17 02:10:43 2019 +0000
@@ -1,21 +1,20 @@
 #include "towelest.h"
 
-towelest::towelest():
-    serial(mdTX,mdRX,115200),
-    pid(4.0, 0, 0.001, 0.01),
-    LimitSW1(schmitt_trigger_0),
-    LimitSW2(schmitt_trigger_1)
+towelest::towelest(Serial* RS485,int address,PinName Limit0,PinName Limit1,PinName Limit2):
+pid(4.0, 0, 0.001, 0.01),
+LimitSW0(Limit0),
+LimitSW1(Limit1),
+LimitSW2(Limit2)
 {
-    Motor[0] = new ikarashiMDC(2,0,SM,&serial);
-    Motor[1] = new ikarashiMDC(2,2,SM,&serial);
-    Motor[2] = new ikarashiMDC(2,1,SM,&serial);
-
+    Motor = new ikarashiMDC*[3];
+    Motor[0] = new ikarashiMDC(address,0,SM,RS485);
+    Motor[1] = new ikarashiMDC(address,1,SM,RS485);
+    Motor[2] = new ikarashiMDC(address,2,SM,RS485);      
+      
     Motor[0]->braking = true;
     Motor[1]->braking = true;
     Motor[2]->braking = true;
-
-    thread.start(callback(this, &towelest::loop));
-
+    
     pid.setInputLimits(-1800, 1800);
     pid.setOutputLimits(-0.6,0.6);
     pid.setBias(0.0);
@@ -26,10 +25,10 @@
 
 void towelest::open()
 {
-    if(LimitSW1.read() == 1) {
+    if(LimitSW0.read() == 1){
         Motor[0]->setSpeed(0);
         Motor[1]->setSpeed(0);
-    } else if(LimitSW1.read() == 0) {
+    }else{
         Motor[0]->setSpeed(-0.2);
         Motor[1]->setSpeed(0);
     }
@@ -42,15 +41,26 @@
     
 void towelest::lift()
 {
-    if(LimitSW2.read() == 1) {
+    if(LimitSW1.read() == 1){
         Motor[0]->setSpeed(0);
         Motor[1]->setSpeed(0);
-    } else if(LimitSW2.read() == 0) {
+    }else{
         Motor[0]->setSpeed(0);
         Motor[1]->setSpeed(0.8);
     }
 }
 
+void towelest::descent()
+{
+    if(LimitSW2.read() == 1){
+        Motor[0]->setSpeed(0);
+        Motor[1]->setSpeed(0);
+    }else{
+        Motor[0]->setSpeed(0);
+        Motor[1]->setSpeed(-0.8);
+    }
+}
+
 void towelest::drop()
 {
     timer.start();
@@ -69,15 +79,12 @@
     loli = Loli;
 }
 
-void towelest::loop()
+void towelest::pid_compute()
 {
-    while(true) {
-        pid.setProcessValue(loli);
-        compute = pid.compute();
-        Motor[2]->setSpeed(compute);
-        Limit1 = LimitSW1.read();
-        Limit2 = LimitSW2.read();
-        //pc.printf("%d  %d  %f \n\r",Limit1,Limit2,RotationDistance);
-    }
+    pid.setProcessValue(loli);
+    compute = pid.compute();
+    Motor[2]->setSpeed(compute);
+        //pc.printf("%d  %d  %f \n\r",Limit1,Limit2,Limit3,RotationDistance);
 }
 
+