Diff: towelest.cpp

Revision:

0:0797b0e47e6d

Child:

1:c6950f45b03c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/towelest.cpp	Thu Aug 29 05:14:22 2019 +0000
@@ -0,0 +1,77 @@
+#include "towelest.h"
+
+towelest::towelest(Serial* RS485):
+enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING),
+pid(4.0, 0, 0.001, 0.01),
+LimitSW1(PB_5),
+LimitSW2(PB_4),
+pc(USBTX,USBRX,115200)
+{
+    Motor = new ikarashiMDC*[3];
+    Motor[0] = new ikarashiMDC(2,0,SM,RS485);
+    Motor[1] = new ikarashiMDC(2,1,SM,RS485);
+    Motor[2] = new ikarashiMDC(2,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);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(0);
+
+}
+
+void towelest::open()
+{
+    if(LimitSW1.read() == 1) {
+        Motor[0]->setSpeed(0);
+        Motor[1]->setSpeed(0);
+    } else if(LimitSW1.read() == 0) {
+        Motor[0]->setSpeed(-0.2);
+        Motor[1]->setSpeed(0);
+    }
+}
+
+void towelest::lift()
+{
+    if(LimitSW2.read() == 1) {
+        Motor[0]->setSpeed(0);
+        Motor[1]->setSpeed(0);
+    } else if(LimitSW2.read() == 0) {
+        Motor[0]->setSpeed(0);
+        Motor[1]->setSpeed(0.8);
+    }
+}
+
+void towelest::drop()
+{
+    timer.start();
+    time = timer.read();
+    if(time <= 3) {
+        pid.setSetPoint(-800);
+    } else {
+        pid.setSetPoint(0);
+        timer.stop();
+        timer.reset();
+    }
+}
+
+void towelest::loop()
+{
+    while(true) {
+        RotationDistance = (float)enc1.getPulses();
+        pid.setProcessValue(RotationDistance);
+        compute = pid.compute();
+        Motor[2]->setSpeed(compute);
+        Limit1 = LimitSW1.read();
+        Limit2 = LimitSW2.read();
+        pc.printf("%d  %d  %f \n\r",Limit1,Limit2,RotationDistance);
+        
+    }
+}
+