Diff: main.cpp

Revision:

6:7afdc6a81566

Parent:

5:869dc702b923

Child:

7:24a3e797e7a8

diff -r 869dc702b923 -r 7afdc6a81566 main.cpp
--- a/main.cpp	Fri Jan 11 09:33:04 2019 +0000
+++ b/main.cpp	Fri Mar 08 07:29:25 2019 +0000
@@ -1,33 +1,32 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "Filter.h"
-#define SB_ADRS 132
-#define SABER_ADDR 128
-#define INT_TIME 0.02
-#define RESOLUTION 48
-#define MULTIPLU 4.0
+#include "string"
+#include "define.h"
+#include "RoboClaw.h"
 
 Ticker timer;
 Timer T;
-QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+/*QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
 QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
-QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc (p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);*/
+RoboClaw MD(115200,p9,p10);
 Serial Saber(p13,p14);
 Serial pc(USBTX,USBRX);
+RawSerial Master(p28,p27,115200);//(tx,rx,baud);
 Filter velfilter(INT_TIME);
 
-DigitalIn sw2(p25);
-DigitalIn sw1(p26);//モード切替
-DigitalOut fet1(p22);
-DigitalOut fet2(p21);
+DigitalOut fet1(p22);//shagai把持
+DigitalOut fet2(p21);//shagai押出
+DigitalOut fet3(p23);//shagaiハンド昇降
 
-DigitalIn limit1(p15);
+DigitalIn limit1(p15);//shagaiハンドlimit
 DigitalIn limit2(p16);
-
-DigitalIn SENS1(p18);
+//SENSのどっちか減るかも
+DigitalIn SENS1(p18);//shagai検出
 DigitalIn SENS2(p17);
 
-DigitalIn G_limit1(p9);
+DigitalIn G_limit1(p9);//gerege limit
 DigitalIn G_limit2(p10);
 
 int cmd,A;
@@ -39,10 +38,10 @@
 float Ksp2 = 6.5, Ksd2 = 0.4; 
 float Ksp3 = 6.5, Ksd3 = 0.4; 
 
-float encount,b_encount;
+//float encount,b_encount;
 
-int mode = 7;
-int cmd2 = 0;
+char mode = 0x00;
+/*int cmd2 = 0;
 int cmd3 = 0;
 
 float spd2=0;
@@ -53,58 +52,70 @@
 
 int tmp1;
 int tmp2;
-
-double filtered_ref_spd;
+*/
+double filtered_ref_qpps;
 
 int G_LIM1=0,G_LIM2=0;
 
 int G_cmd;
 
-int modee=2;
-int button_in_2=0; 
+int limit_MD(int qpps,int max_qpps){
+    if (qpps > max_qpps) qpps = max_qpps;
+    else if (-qpps < -max_qpps) qpps = -max_qpps;
+    return qpps;
+}
 
-int Button() {
-    
-    int button_in = sw1.read();
-    
-    static int pre_button = 0;
-    static int sw_state = 3;
-    
-    if(button_in && pre_button)sw_state = 0;
-    if(!button_in && !pre_button)sw_state = 3;
-    if(button_in && !pre_button)sw_state = 1;
-    if(!button_in && pre_button)sw_state = 2;
-    
-    pre_button = button_in;
-    
-    return sw_state;
+void robo_serial(int adrs, int qpps1, int qpps2){
+    MD.SpeedM1(adrs,qpps1);
+    MD.SpeedM2(adrs,qpps2);
 }
-int Button2() {//スイッチのノイズをとる関数
-    
-    button_in_2 = sw2.read();
-    
-    static int pre_button_2 = 0;
-    static int sw_state_2 = 3;
-    
-    if(button_in_2 && pre_button_2)sw_state_2 = 0;
-    if(!button_in_2 && !pre_button_2)sw_state_2 = 3;
-    if(button_in_2 && !pre_button_2)sw_state_2 = 1;
-    if(!button_in_2 && pre_button_2)sw_state_2 = 2;
-    
-    pre_button_2 = button_in_2;
-    
-    return sw_state_2;
+
+void Saber_Serial (int adrs, int rot, int cmd){
+    Saber.putc(adrs);
+    Saber.putc(rot); 
+    Saber.putc(abs(cmd));
+    Saber.putc((adrs + rot + abs(cmd)) & 0b01111111);
 }
 
+/*void Slave_tx(char tx_mode){                                //処理の終了を送る
+    Master.printf("%c",tx_mode);
+    Master.printf("%c",tx_mode);
+    Master.printf("\n");
+}*/
+
+void Slave_rx() {                                           //処理内容を受け取る
+    static string rx_mode = "";
+    char rx_c = Master.getc();
+    
+    if (rx_c == '\n') {
+        if (rx_mode.size() == 2){
+        if (rx_mode[0] == rx_mode[1]){
+        mode = rx_mode[1];       //モード切替
+        }}
+        rx_mode = "";
+        }
+    
+    else {
+        if (rx_mode.size() > 2) rx_mode = "";
+        rx_mode += rx_c;
+        }
+        //pc.printf("%x\n",mode);
+}
 
 void timer_warikomi()
 {
+    static int qpps1 = 0;
+    static int qpps2 = 0;
+    static int ref_qpps1 = 0;
+    static int ref_qpps2 = 0;
 
     LIM1=!limit1.read();
     LIM2=!limit2.read();
     S1=SENS1.read();
     S2=SENS2.read();
-    encount=Enc.getPulses()-b_encount;
+    static char slave_mode = 0x00;
+    static int spd_count = 0;
+    /*encount=Enc.getPulses()-b_encount;
 
     float ppr = 1.0;
     
@@ -119,7 +130,8 @@
     static int lim_cmd2 = 87;
     static int lim_cmd3 = 92;
     
-    int sw_point = Button();
+    static int count = 0;
+    static int count3 = 0;
     
     angle=(float)(encount)*(360.0/48.0)/4.0;   
     SOKUDO=(angle-pre_angle)/INT_TIME;
@@ -156,178 +168,125 @@
     if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
     
     if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
-    if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
+    if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;*/
+    
+    
     
-    if(sw_point != 0) switch(mode){
-          case 0:
-              goal_D=0;
-                if(sw_point==2)mode=1;
+    G_LIM1=!G_limit1.read();//pullupなので逆
+    G_LIM2=!G_limit2.read();//pullupなので逆
+    
+    switch (mode) {
+        case 0x99:
                 break;
-          case 1:
-              cmd=-15;
-                if(sw_point==2)mode=2;
-                if(LIM2==1){
-                        cmd=0;
-                        b_encount=Enc.getPulses();
-                        }
-                        break;
-          case 2:
-              goal_D=125;
-                 if(sw_point==2)mode=3;
-                 if(angle>=120)cmd=0;
-                 if(S1==0&&S2==0){
-                fet1=1;
-                A=1;
-                }
-                 break;
-               
-          /*case 3:
-                if(sw_point==2)mode=4;
+                
+        case 0x10://初期化処理
+                ref_qpps1 = 0.0;
+                ref_qpps2 = 0.0;
+                
+                break;
+                
+        case 0x20://ハンド降下
+                //goal_D=125;
                 if(S1==0&&S2==0){
                 fet1=1;
                 A=1;
                 }
-                break;*/
+                break;
+                 
+        case 0x30://ハンド上昇
+                
+                break;
+                
+        case 0x40://geregeハンド
+                G_cmd=120;
+                if(G_LIM1){
+                    G_cmd=0;
+                }
+                break;
                 
-          case 3:
-                goal_D=0;
-                    if(sw_point==2){
-                    cmd=0;
-                    b_encount=Enc.getPulses();
-                    mode=4;
+        case 0x50://geregeハンド
+                G_cmd=-120;
+                if(G_LIM2){
+                    G_cmd=0;
+                slave_mode = 0x55;
+                }
+                break;
+               
+        case 0x60://shagai発射
+                ref_qpps1 = REF_QPPS1;
+                ref_qpps2 = REF_QPPS2;
+                if (qpps1 == REF_QPPS1 && qpps2 == REF_QPPS2) {
+                    spd_count++;
                 }
                 break;
-          case 4:
-                fet1=0;
-                if(sw_point==2)mode=5;
+        
+        case 0x90://全停止
+                ref_qpps1 = 0.0;
+                ref_qpps2 = 0.0;
+                //cmd = 0;
+                G_cmd = 0;
                 break;
-           
-          case 5:
-               ref_spd = 26.0;
-               if (sw_point == 2) mode = 6;
-               break;
-        
-          case 6:
-               fet2 = 0;
-               if (sw_point == 2) mode = 7;
-               break;
-        
-          case 7:
-               ref_spd = 0.0;
-               fet2 = 1;
-               if (sw_point == 2) mode = 0;
-               if (spd3<=5.0) {
-                   cmd2 = 0;
-                   cmd3 = 0;
-                   }
-               break;
-          }
-          
-          G_LIM1=!G_limit1.read();//pullupなので逆
-    G_LIM2=!G_limit2.read();//pullupなので逆
-    
-    int sw_point2 = Button2();//スイッチの関数からリターン
+                
+        default://何もしない
+                break;
+        }
         
-    if(sw_point2 != 0) switch(modee){
-        case(0):  
-        G_cmd=120;
-        if(G_LIM1==1){
-            G_cmd=0;
-                }
-        if (sw_point2 == 2) modee = 1;
-
-        break;
-        
-        case(1):
-        G_cmd=-120;
-        if (sw_point2 == 2) modee = 2;
-        if(G_LIM2==1){
-            G_cmd=0;
-            }
-        break;
-        
-        case(2)://モータ停止と押し上げ機構の降下
-        G_cmd=0;
-        if (sw_point2 == 2) modee = 0;
-        break;
-        }
+    //Slave_tx(slave_mode);
+    //二次遅れ系 
+    if(qpps1>=QPPS_HIGH_RANGE1 && ref_qpps1 == REF_QPPS1) qpps1=ref_qpps1;
+    else if(qpps1<=QPPS_HIGH_STOP && ref_qpps1 == 0) qpps1=0;
+    else qpps1 = velfilter.SecondOrderLag((double)ref_qpps1);
+    if(qpps2>=QPPS_HIGH_RANGE2 && ref_qpps2 == REF_QPPS2) qpps2=ref_qpps2;
+    else if(qpps2<=QPPS_HIGH_STOP && ref_qpps2 == 0) qpps2=0;
+    else qpps2 = velfilter.SecondOrderLag((double)ref_qpps2);
+    //ローラー強制停止
+    /*if (qpps <= 5.0 && ref_qpps == 0) {
+        cmd3 = 0;
+    }*/
+    //自動押し出し
+    if (spd_count >= 40){
+        fet2 = 0;
+        if (slave_mode != 0x60) spd_count = 0;
+    }
+    /*
+    if(angle>=124 && goal_D >= 120){
+        cmd=0;
+        goal_D = angle;
+    }
     
-          
-          if(filtered_ref_spd>=25.5&&mode==5){
-          filtered_ref_spd=26;
-    }else if(filtered_ref_spd>=25.5&&mode==6){
-          filtered_ref_spd=26;
-    }else if(filtered_ref_spd<=5.0&&mode==7){
-          filtered_ref_spd=0;
-    }else if(mode==5||mode==6||mode==7){
-          filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
+    if(angle <= 2 && goal_D <= 10) {
+        cmd = 0;
+        goal_D = angle;
     }
     
     if(cmd>20) cmd=20;
-    if(cmd<-15)cmd=-15; 
+    if(cmd<-15)cmd=-15;
     
-    int F=1,FF=0;//向き
+    if(cmd>=0) Saber_Serial (SABER_ADDR, 1, cmd);
+    else Saber_Serial (SABER_ADDR, 0, cmd);
+    */
+    qpps1 = limit_MD( qpps1, MAX_QPPS1);
+    qpps2 = limit_MD( qpps2, MAX_QPPS2);
     
-    if(cmd>=0) {
-        Saber.putc(SABER_ADDR);
-        Saber.putc(F); 
-        Saber.putc(abs(cmd));
-        Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111); 
-    } else {
-        Saber.putc(SABER_ADDR);
-        Saber.putc(FF); 
-        Saber.putc(abs(cmd));
-        Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111); 
-    }
+    robo_serial(ROBO_ADRS, qpps1, qpps2);
     
-    if (cmd2 > 0) {
-        Saber.putc(SB_ADRS);
-        Saber.putc(4);
-        Saber.putc(cmd2);
-        Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111);
-        }
-    else {
-        Saber.putc(SB_ADRS);
-        Saber.putc(5);
-        Saber.putc(abs(cmd2));
-        Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111);
-        }
+    if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd);
+    else Saber_Serial (SABER_ADDR, 5, G_cmd);
+    /*
+    if (cmd3 > 0) Saber_Serial (SB_ADRS, 0, cmd3);
+    else Saber_Serial (SB_ADRS, 1, cmd3);
+    
+    if (cmd2 > 0) Saber_Serial (SB_ADRS, 4, cmd2);
+    else Saber_Serial (SB_ADRS, 5, cmd2);*/
         
-    if (cmd3 > 0) {
-        Saber.putc(SB_ADRS);
-        Saber.putc(0);
-        Saber.putc(cmd3);
-        Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111);
-        }
-    else {
-        Saber.putc(SB_ADRS);
-        Saber.putc(1);
-        Saber.putc(abs(cmd3));
-        Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
-        }
-    if (G_cmd > 0) {
-        Saber.putc(SABER_ADDR);
-        Saber.putc(4);
-        Saber.putc(G_cmd);
-        Saber.putc((SABER_ADDR + 4 + G_cmd) & 0b01111111);
-        }
-    else {
-        Saber.putc(SABER_ADDR);
-        Saber.putc(5);
-        Saber.putc(abs(G_cmd));
-        Saber.putc((SABER_ADDR + 5 + abs(G_cmd)) & 0b01111111);
-        }
-        
-    pre_spd2 = rot_sp2;
-    pre_err2 = spd_err2;
-    pre_spd3 = rot_sp3;
-    pre_err3 = spd_err3;
-        
-        
-    pre_e_D=e_D;
-    pre_angle=angle;
-    pre_e_V=e_V;
-    B_SA1=SA1;
+    /*pre_spd2  = rot_sp2;
+    pre_err2  = spd_err2;
+    pre_spd3  = rot_sp3;
+    pre_err3  = spd_err3;
+    pre_e_D   = e_D;
+    pre_angle = angle;
+    pre_e_V   = e_V;
+    B_SA1     = SA1;*/
     }
 
 
@@ -336,16 +295,17 @@
     pc.baud(9600);
     fet1=0;
     fet2=1;
+    fet3=0;
     
     G_limit1.mode( PullUp );    //  内蔵プルアップを使う
     G_limit2.mode( PullUp ); 
     
     velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
     
+    Master.attach(&Slave_rx, RawSerial::RxIrq);
     timer.attach(timer_warikomi,INT_TIME);
     
     while(1) {
-        //pc.printf("%d\n",mode);
-        pc.printf("spd2%f\t spd3 %f\n",spd2,spd3);
+        pc.printf("%lf\n", filtered_ref_qpps);
     }
 }