Revision 0:890f901f4898, committed 2018-10-04

Comitter:

tanabe2000

Date:

Thu Oct 04 11:43:22 2018 +0000

Child:

1:6cdfc10c7b1d

Commit message:

?????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/petbottleLoading.cpp	Thu Oct 04 11:43:22 2018 +0000
@@ -0,0 +1,130 @@
+#include "petbottleLoading.h"
+
+petbottleLoading::petbottleLoading():
+    loadingPid(loadingKP, loadingKI, loadingKD, RATE),
+    loadingencoder(loadingencoder1_1, loadingencoder1_2 ,NC, 2048, QEI::X4_ENCODING),
+    RS485control(PA_4),
+    RS485(MDC_TX,MDC_RX,115200),
+    debug(USBTX, USBRX),
+    limitSW1_(limitswich1_1),
+    limitSW2_(limitswich1_2),
+    limitSW3_(limitswich1_3)
+{
+    petbottledrop  = new ikarashiMDC(&RS485control,1,0,SM,&RS485);
+    petbotleslider = new ikarashiMDC(&RS485control,1,1,SM,&RS485);
+    petbottleIn    = new ikarashiMDC(&RS485control,1,2,SM,&RS485);
+    petbottledrop->braking = true;/*変更*/
+    petbotleslider->braking = true;/*変更*/
+    petbottleIn->braking = true;/*変更*/
+    limitSW1_.mode(PullUp);
+    limitSW2_.mode(PullUp);
+    limitSW3_.mode(PullUp);
+
+    loadingPid.setMode(AUTO_MODE);
+    loadingPid.setInputLimits(-10, 2200);
+    loadingPid.setOutputLimits(-0.9, 0.9);
+    loadingPid.setBias(0.0);   // outputの変わり目
+    limitSwFlag[2] = 0;
+    limitSwFlag2[2] = 0;
+    limitSwFlag[0] = 0;
+    limitSwFlag2[0] = 0;
+    sw5flag = 0;
+    pt.start();
+
+}
+
+
+void petbottleLoading::petbottlemode(int loadingmode)
+{
+    nowPals = loadingencoder.getPulses();
+    loadingPid.setSetPoint(loadingDistance);// 目標値
+    loadingPid.setProcessValue(nowPals);
+    Output_PID = -1.0*loadingPid.compute();
+    pdt = pt.read();
+    limitSW1 = limitSW1_.read();
+    limitSW2 = limitSW2_.read();
+    limitSW3 = limitSW3_.read();
+//    debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3);
+debug.printf("nowPals<%d>\r\n", nowPals);
+
+    if(loadingmode == 0) {
+        loadingDistance = 0;
+        petbottledropPwm = 0.0;
+        petbottleInPwm = 0.0;
+    } else if(loadingmode == 1) {
+        limitSwFlag[2] = 0;
+        limitSwFlag2[2] = 0;
+        loadingDistance = 954;
+        if((nowPals - pullDistanceOfset) > 700) {
+            if(limitSW1 == 1 && limitSwFlag[0] == 0) {
+                limitSwFlag[0] = 1;
+                petbottledropPwm = 1.0;
+            }
+            if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
+                limitSwFlag[0] = 1;
+                limitSwFlag2[0] = 1;
+                petbottledropPwm = 1.0;
+            }
+            if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
+            if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
+        }
+    } else if(loadingmode == 2) {
+        limitSwFlag[2] = 0;
+        limitSwFlag2[2] = 0;
+        loadingDistance = 1854;
+        if((nowPals - pullDistanceOfset) > 700) {
+            if(limitSW1 == 1 && limitSwFlag[0] == 0) {
+                limitSwFlag[0] = 1;
+      
+                petbottledropPwm = 1.0;
+            }
+            if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
+                limitSwFlag[0] = 1;
+                limitSwFlag2[0] = 1;
+                petbottledropPwm = 1.0;
+            }
+            if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
+            if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
+        }
+    } else if(loadingmode == 3) {
+        if(limitSW3 ==1 && limitSwFlag[2] == 0) limitSwFlag[2] = 1,limitSwFlag2[2] = 0 ;
+        if(limitSW3 ==0 && limitSwFlag[2] == 1) limitSwFlag2[2] = 1;
+        if(limitSwFlag[2] == 1 && limitSwFlag2[2] == 1) {
+            if(limitSW3 ==1) {
+                petbottleInPwm = 0.0;
+                loadingDistance = 0;
+                petbottleInPwm = 0.0;
+                if((nowPals - pullDistanceOfset) < 25) {
+                    if(limitSW1 == 1 && limitSwFlag[0] == 1) {
+                        limitSwFlag[0] = 0;
+                        petbottledropPwm = 1.0;
+                    }
+                    if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) {
+                        limitSwFlag[0] = 0;
+                        limitSwFlag2[0] = 0;
+                        petbottledropPwm = 1.0;
+                    }
+                    if(limitSW1 ==0 && limitSwFlag[0] == 0) limitSwFlag2[0] = 0;
+                    if((limitSwFlag[0] == 0) && (limitSwFlag2[0] == 0) && (limitSW1 == 1))petbottledropPwm = 0.0;
+                }
+            }
+        } else {
+            if((nowPals - pullDistanceOfset) > 700)petbottleInPwm = -1.0;
+        }
+    } else {
+        petbottledropPwm = 0.0;
+        petbottleInPwm = 0.0;
+    }
+    petbottledrop->setSpeed(petbottledropPwm);
+    petbotleslider->setSpeed(Output_PID);
+    petbottleIn->setSpeed(petbottleInPwm);
+}
+
+int petbottleLoading::LoadingEncoder(){
+    return nowPals - pullDistanceOfset;
+    }
+
+int petbottleLoading::state()
+{
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/petbottleLoading.h	Thu Oct 04 11:43:22 2018 +0000
@@ -0,0 +1,75 @@
+#ifndef PETBOTTLE_LOADING_H
+#define PETBOTTLE_LOADING_H
+#include "ikarashiMDC.h"
+#include "PID.h"
+#include "QEI.h"
+#include"pin_config.h"
+
+#define loadingKP 8.0
+#define loadingKI 0
+#define loadingKD 0.0000001
+#define RATE 0.01
+
+
+/**
+ * 装填・発射機構用クラス
+ */
+class petbottleLoading
+{
+public:
+    /**
+    * デフォルトコンストラクタ
+    */
+    petbottleLoading();
+
+    /**
+     *
+     * @0:最初の位置
+     * @1:発射機構１まで移動モード
+     * @2:発射機構２まで移動モード
+     * @3:装填して原点に戻りボトルを落とすモード
+     */
+    void petbottlemode(int loadingmode);
+
+    /**
+     *装填機構状態表示
+     * @0 : 装填中～～
+     * @1 : 装填完了！
+     */
+    int state();
+    
+    int LoadingEncoder();
+
+
+private:
+    /*装填機構*/
+    void loadingasenble();
+    ikarashiMDC* petbottledrop;
+    ikarashiMDC* petbotleslider;
+    ikarashiMDC* petbottleIn;
+    Serial RS485;
+    DigitalOut RS485control;
+
+    double petbottledropPwm, petbotlesliderPwm, petbottleInPwm;
+    PID loadingPid;
+    QEI loadingencoder;
+    InterruptIn limitSW1_,limitSW2_,limitSW3_;
+    int limitSW1,limitSW2,limitSW3;
+    bool limitSwFlag[3],limitSwFlag2[3], limitStatus[3];
+    int nowPals, loadingDistance, pullDistanceOfset;
+    double Output_PID;
+    int loadingmode;
+    bool sw5flag;
+    
+    /*受信系*/
+    bool receiveSuccessed;
+    Serial debug;
+    Timer pt;
+    float pdt;
+
+
+};
+
+
+
+#endif