Revision 3:6ae90807f164, committed 2019-08-30

Comitter:

ec30109b

Date:

Fri Aug 30 05:51:16 2019 +0000

Parent:

2:521f83b5d6e5

Commit message:

2019NHK_teamA_outo_sheets

Changed in this revision

diff -r 521f83b5d6e5 -r 6ae90807f164 sheets.cpp
--- a/sheets.cpp	Thu Aug 29 05:11:38 2019 +0000
+++ b/sheets.cpp	Fri Aug 30 05:51:16 2019 +0000
@@ -2,6 +2,7 @@
 
 sheets::sheets(Serial* RS485):
 enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING),
+pid(4.0, 0, 0.001, 0.01),
 port_a(PB_5),
 port_b(PB_4),
 LimitSW1(PC_13),
@@ -15,6 +16,12 @@
     Motor[1]->braking = true;
     thread.start(callback(this, &sheets::loop));
     
+    pid.setInputLimits(-6000, 6000);
+    pid.setOutputLimits(-0.5,0.5);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(0);
+    
     port_a.write(true);
     port_b.write(false);
 }
@@ -43,27 +50,17 @@
     port_b.write(false);
 }
 
-void sheets::adjust1(int height1)
+void sheets::set_adjust(int height)
 {
-    ideal_height1 = RotationDistance - height1;
-    if(RotationDistance < ideal_height1 + 20 && RotationDistance > ideal_height1 - 20){
-        Motor[0]->setSpeed(0);
-    }else{
-        Motor[0]->setSpeed(-0.3);
-    }
-    Motor[1]->setSpeed(0);
-    port_a.write(true);
-    port_b.write(false);
+    pid.setSetPoint(height);
 }   
 
-void sheets::adjust2(int height2)
+void sheets::compute_adjust()
 {
-    ideal_height2 = RotationDistance - height2;
-    if(RotationDistance < ideal_height2 + 20 && RotationDistance > ideal_height2 - 20){
-        Motor[0]->setSpeed(0);
-    }else{
-        Motor[0]->setSpeed(-0.3);
-    }
+    pid.setProcessValue(RotationDistance);
+    compute = pid.compute();
+    Motor[0]->setSpeed(compute);
+    
     Motor[1]->setSpeed(0);
     port_a.write(true);
     port_b.write(false);
@@ -104,6 +101,14 @@
     RotationDistance = Loli;
 }
 
+void sheets::stop()
+{
+    Motor[0]->setSpeed(0);
+    Motor[1]->setSpeed(0);
+    port_a.write(true);
+    port_b.write(false);
+}
+
 void sheets::loop()
 {
     while(true) {

diff -r 521f83b5d6e5 -r 6ae90807f164 sheets.h
--- a/sheets.h	Thu Aug 29 05:11:38 2019 +0000
+++ b/sheets.h	Fri Aug 30 05:51:16 2019 +0000
@@ -4,6 +4,7 @@
 #include "mbed.h"
 #include "ikarashiMDC.h"
 #include "QEI.h"
+#include "PID.h"
 
 const int RS485_BAUD = 115200;
 
@@ -13,12 +14,13 @@
     sheets(Serial* RS485);
     void lift();
     void expansion();
-    void adjust1(int height1);
-    void adjust2(int height2);
+    void set_adjust(int height1);
+    void compute_adjust();
     void descent();
     void air_open();
     void air_close();
     void setPulse(int Loli);
+    void stop();                 
     void loop();
     int Limit1;
     int Limit2;
@@ -28,15 +30,14 @@
     ikarashiMDC **Motor;
     Thread thread;
     QEI enc1;
+    PID pid;
     DigitalOut port_a;
     DigitalOut port_b;
     DigitalIn LimitSW1;
     DigitalIn LimitSW2;
     DigitalIn LimitSW3;
     int RotationDistance;
-    int ideal_height1;
-    int ideal_height2;
-    int height1;
-    int height2;
+    int height;
+    double compute;
 };
 #endif
\ No newline at end of file