Diff: main.cpp

Revision:

0:56c3d27ab161

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 27 01:51:51 2018 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "define.h" //ステータス用
+
+Ticker timer;
+Timer T;
+
+QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+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);
+DigitalIn limit1(p15);
+DigitalIn limit2(p16);
+DigitalIn sensor1(p18);
+DigitalIn sensor2(p17);
+
+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 = 1;
+    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_ang = 0.0;
+    float encount_rot = 0.0;
+    float Ksp = 0.005, Ksd = 0.0015;    //速度係数
+    float Kp=5.0, Ki=0.01, Kd=0.1;             //角度係数
+    //float Kp_V=1, Kd_V=0;               //角速度係数
+    float ppr = 1.0;
+    static float pre_angle = 0.0;
+    static float pre_angleE = 0.0;
+    static float pre_spd = 0.0;
+    static float pre_spdE = 0.0;
+    static float ref_angle = 0.0;
+    static float ref_spd = 0.0;
+    static float angle_I = 0.0;
+    static int cmd_spd = 0;
+    static int cmd_ang = 0;
+    static int mode = 0;
+    static int lim_cmdA = 20;
+    static int lim_cmdS = 127;
+    static int pre_encount = 0;
+    
+    int sw_point = Button();
+        
+    int encount2 = Enc2.getPulses();
+    int encount3 = Enc3.getPulses();
+    
+    encount_ang = Enc1.getPulses()- pre_encount;
+    if (encount2 > encount3) encount_rot = encount2;
+    else encount_rot = encount3;
+    
+    float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0;   
+    float ang_spd =(angle - pre_angle)/INT_TIME;
+    
+    float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; 
+    float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU);
+    
+    float angle_P = (ref_angle - angle);
+    float angle_D=(angle_P - pre_angleE)/INT_TIME;
+    angle_I += (angle_P + pre_angleE)*INT_TIME/2.0;
+    
+    cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki);
+    
+    float spd_e = ref_spd - spd;
+    float spd_D = (spd_e - pre_spdE)/INT_TIME;
+    cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd);
+    
+    if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA;
+    if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA;
+    
+    if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS;
+    if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS;
+    
+    if (sw_point != HIGH) switch (mode) {
+        
+        case 0:
+            ref_angle = 0;
+            ref_spd = 0.0;
+            fet2 = OFF;
+            if (sw_point == RISE) mode = 1;
+            break;
+            
+        case 1:
+            cmd_ang = 20;
+            if (limit1.read()) {
+                cmd_ang = 0;
+                pre_encount = Enc1.getPulses();
+                if (sw_point == RISE) mode = 2;
+            }
+            break;
+        
+        case 2:
+            ref_angle = -125;
+            if (sw_point == RISE) mode = 3;
+            break;
+            
+        case 3:
+            if (!sensor1 && !sensor2) {
+                fet1 = ON;
+                if (sw_point == RISE) mode = 4;
+                }
+            break;
+        
+        case 4:
+            ref_angle = 0;
+                 //if(99<=angle||angle<=101){
+            if (sw_point == RISE) {
+                cmd_ang = 0;
+                pre_encount = Enc1.getPulses(); //スイッチを離したところを初期値に
+                mode = 5;                       //して無理くり止めてます
+                }
+            break;
+        
+        case 5:
+            fet1 = OFF;
+            ref_spd = 30.0;
+            if (sw_point == RISE) mode = 6;
+            break;
+        
+        case 6:
+            fet2 = ON;
+            if (sw_point == RISE) mode = 0;
+            break;
+        }
+        
+    if (!sw2.read()) {
+        cmd_spd = 0;
+        cmd_ang = 0;
+        }
+        
+    if (cmd_ang >= 0) {
+        Saber.putc(SB_ADRS_A);
+        Saber.putc(1);
+        Saber.putc(cmd_ang);
+        Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111);
+        } 
+    else {
+        Saber.putc(SB_ADRS_A);
+        Saber.putc(0);
+        Saber.putc(abs(cmd_ang));
+        Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111);
+        }
+        
+    if (cmd_spd >= 0) {
+        Saber.putc(SB_ADRS_B);
+        Saber.putc(1);
+        Saber.putc(cmd_spd);
+        Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111);
+        }
+    else {
+        Saber.putc(SB_ADRS_B);
+        Saber.putc(0);
+        Saber.putc(abs(cmd_spd));
+        Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111);
+        }
+        
+        pre_spd = spd;
+        pre_spdE = spd_e;
+        pre_angle = angle;
+        pre_angleE = angle_P;
+}
+
+int main() {
+    Saber.baud(115200);
+    pc.baud(9600);
+    timer.attach(timer_warikomi,INT_TIME);
+    
+    while(1) {
+    }
+}
\ No newline at end of file