Diff: main.cpp

Revision:

0:5b33e3942d43

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 27 01:34:33 2018 +0000
@@ -0,0 +1,105 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "define.h" //ステータス用
+
+Ticker timer;
+Timer T;
+
+QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+DigitalIn sw1(p26);
+DigitalIn sw2(p25);
+DigitalOut fet1(p21);
+DigitalOut fet2(p22);
+Serial Saber(p13,p14);
+Serial pc(USBTX,USBRX);
+
+int Button() {
+    
+    int button_in = sw1.read();
+    static int pre_button = 0;
+    static int sw_state = LOW;
+    
+    if(button_in && pre_button)sw_state = HIGH;
+    if(!button_in && !pre_button)sw_state = LOW;
+    if(button_in && !pre_button)sw_state = FALL;
+    if(!button_in && pre_button)sw_state = RISE;
+    
+    pre_button = button_in;
+    
+    return sw_state;
+}
+
+void timer_warikomi()
+{
+    float encount_rot = 0.0;
+    float Ksp = 0.005, Ksd = 0.0015;    
+    float ppr = 1.0;
+    static float pre_spd = 0.0;
+    static float pre_err = 0.0;
+    static float ref_spd = 0.0;
+    static int cmd = 0;
+    static int mode = 2;
+    static int lim_cmd = 127;
+    
+    int sw_point = Button();
+    
+    if(sw_point != HIGH) switch(mode){
+        case(0):    
+        ref_spd = 30.0;
+        if (sw_point == RISE) mode = 1;
+        break;
+        
+        case(1):
+        fet2 = ON;
+        if (sw_point == RISE) mode = 2;
+        break;
+        
+        case(2):
+        ref_spd = 0.0;
+        fet2 = OFF;
+        if (sw_point == RISE) mode = 0;
+        break;
+        }
+        
+    int encount2 = Enc2.getPulses();
+    int encount3 = Enc3.getPulses();
+    
+    if (encount2 > encount3) encount_rot = encount2;
+    else encount_rot = encount3;
+    
+    float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; 
+    float spd = (rot_sp - pre_spd)/INT_TIME/(48*4);
+    
+    float spd_err = ref_spd - spd;
+    float spd_d = (spd_err - pre_err)/INT_TIME;
+    cmd += spd_err * Ksp + spd_d * Ksd;
+    
+    if (cmd > lim_cmd) cmd = lim_cmd;
+    if (-cmd < -lim_cmd) cmd = -lim_cmd;
+    
+    if (cmd > 0) {
+        Saber.putc(SB_ADRS);
+        Saber.putc(1);
+        Saber.putc(cmd);
+        Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
+        }
+    else {
+        Saber.putc(SB_ADRS);
+        Saber.putc(0);
+        Saber.putc(abs(cmd));
+        Saber.putc((SB_ADRS + 0 + abs(cmd)) & 0b01111111);
+        }
+        
+        pre_spd = spd;
+        pre_err = spd_err;
+}
+
+int main() {
+    Saber.baud(115200);
+    pc.baud(9600);
+    timer.attach(timer_warikomi,INT_TIME);
+    
+    while(1) {
+    }
+}