main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "Filter.h"
00004 #include "string"
00005 #include "define.h"
00006 #include "RoboClaw.h"
00007 
00008 Ticker timer;
00009 Timer T;
00010 
00011 RoboClaw MD(115200,p9,p10);
00012 Serial Saber(p13,p14);
00013 Serial pc(USBTX,USBRX);
00014 RawSerial Master(p28,p27,115200);//(tx,rx,baud);
00015 Filter velfilter(INT_TIME);
00016 
00017 DigitalOut fet1(p22);//shagai把持
00018 DigitalOut fet2(p21);//shagai押出
00019 DigitalOut fet3(p23);//shagaiハンド昇降
00020 
00021 DigitalIn limit1(p15);//shagaiハンドlimit
00022 DigitalIn limit2(p16);
00023 //SENSのどっちか減るかも
00024 DigitalIn SENS1(p18);//shagai検出
00025 DigitalIn SENS2(p17);
00026 
00027 DigitalIn G_limit1(p9);//gerege limit
00028 DigitalIn G_limit2(p10);
00029 
00030 int cmd,A;
00031 int LIM1,LIM2;
00032 int S1,S2;
00033 
00034 char mode = 0x00;
00035 
00036 double filtered_ref_qpps;
00037 
00038 int G_LIM1=0,G_LIM2=0;
00039 
00040 int G_cmd;
00041 
00042 int limit_MD(int qpps,int max_qpps){
00043     if (qpps > max_qpps) qpps = max_qpps;
00044     else if (-qpps < -max_qpps) qpps = -max_qpps;
00045     return qpps;
00046 }
00047 
00048 void robo_serial(int adrs, int qpps1, int qpps2){
00049     MD.SpeedM1(adrs,qpps1);
00050     MD.SpeedM2(adrs,qpps2);
00051 }
00052 
00053 void Saber_Serial (int adrs, int rot, int cmd){
00054     Saber.putc(adrs);
00055     Saber.putc(rot); 
00056     Saber.putc(abs(cmd));
00057     Saber.putc((adrs + rot + abs(cmd)) & 0b01111111);
00058 }
00059 
00060 /*void Slave_tx(char tx_mode){                                //処理の終了を送る
00061     Master.printf("%c",tx_mode);
00062     Master.printf("%c",tx_mode);
00063     Master.printf("\n");
00064 }*/
00065 
00066 void Slave_rx() {                                           //処理内容を受け取る
00067     static string rx_mode = "";
00068     char rx_c = Master.getc();
00069     
00070     if (rx_c == '\n') {
00071         if (rx_mode.size() == 2){
00072         if (rx_mode[0] == rx_mode[1]){
00073         mode = rx_mode[1];       //モード切替
00074         }}
00075         rx_mode = "";
00076         }
00077     
00078     else {
00079         if (rx_mode.size() > 2) rx_mode = "";
00080         rx_mode += rx_c;
00081         }
00082         //pc.printf("%x\n",mode);
00083 }
00084 
00085 void timer_warikomi()
00086 {
00087     static int qpps1 = 0;
00088     static int qpps2 = 0;
00089     static int ref_qpps1 = 0;
00090     static int ref_qpps2 = 0;
00091 
00092     LIM1=!limit1.read();
00093     LIM2=!limit2.read();
00094     S1=SENS1.read();
00095     S2=SENS2.read();
00096     static char slave_mode = 0x00;
00097     static int spd_count = 0;
00098     
00099     G_LIM1=!G_limit1.read();//pullupなので逆
00100     G_LIM2=!G_limit2.read();//pullupなので逆
00101     
00102     switch (mode) {
00103         case 0x99:
00104                 break;
00105                 
00106         case 0x10://初期化処理
00107                 ref_qpps1 = 0.0;
00108                 ref_qpps2 = 0.0;
00109                 
00110                 break;
00111                 
00112         case 0x20://ハンド降下
00113                 //goal_D=125;
00114                 if(S1==0&&S2==0){
00115                 fet1=1;
00116                 A=1;
00117                 }
00118                 break;
00119                  
00120         case 0x30://ハンド上昇
00121                 
00122                 break;
00123                 
00124         case 0x40://geregeハンド
00125                 G_cmd=120;
00126                 if(G_LIM1){
00127                     G_cmd=0;
00128                 }
00129                 break;
00130                 
00131         case 0x50://geregeハンド
00132                 G_cmd=-120;
00133                 if(G_LIM2){
00134                     G_cmd=0;
00135                 slave_mode = 0x55;
00136                 }
00137                 break;
00138                
00139         case 0x60://shagai発射
00140                 ref_qpps1 = REF_QPPS1;
00141                 ref_qpps2 = REF_QPPS2;
00142                 if (qpps1 == REF_QPPS1 && qpps2 == REF_QPPS2) {
00143                     spd_count++;
00144                 }
00145                 break;
00146         
00147         case 0x90://全停止
00148                 ref_qpps1 = 0.0;
00149                 ref_qpps2 = 0.0;
00150                 G_cmd = 0;
00151                 break;
00152                 
00153         default://何もしない
00154                 break;
00155         }
00156         
00157     //Slave_tx(slave_mode);
00158     //二次遅れ系 
00159     if(qpps1>=QPPS_HIGH_RANGE1 && ref_qpps1 == REF_QPPS1) qpps1=ref_qpps1;
00160     else if(qpps1<=QPPS_HIGH_STOP && ref_qpps1 == 0) qpps1=0;
00161     else qpps1 = velfilter.SecondOrderLag((double)ref_qpps1);
00162     if(qpps2>=QPPS_HIGH_RANGE2 && ref_qpps2 == REF_QPPS2) qpps2=ref_qpps2;
00163     else if(qpps2<=QPPS_HIGH_STOP && ref_qpps2 == 0) qpps2=0;
00164     else qpps2 = velfilter.SecondOrderLag((double)ref_qpps2);
00165     //ローラー強制停止
00166     /*if (qpps <= 5.0 && ref_qpps == 0) {
00167         cmd3 = 0;
00168     }*/
00169     //自動押し出し
00170     if (spd_count >= 40){
00171         fet2 = 0;
00172         if (slave_mode != 0x60) spd_count = 0;
00173     }
00174     
00175     qpps1 = limit_MD( qpps1, MAX_QPPS1);
00176     qpps2 = limit_MD( qpps2, MAX_QPPS2);
00177     
00178     robo_serial(ROBO_ADRS, qpps1, qpps2);
00179     
00180     if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd);
00181     else Saber_Serial (SABER_ADDR, 5, G_cmd);
00182     
00183     }
00184 
00185 
00186 int main() {
00187     Saber.baud(19200);
00188     pc.baud(9600);
00189     fet1=0;
00190     fet2=1;
00191     fet3=0;
00192     
00193     G_limit1.mode( PullUp );    //  内蔵プルアップを使う
00194     G_limit2.mode( PullUp ); 
00195     
00196     velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
00197     
00198     Master.attach(&Slave_rx, RawSerial::RxIrq);
00199     timer.attach(timer_warikomi,INT_TIME);
00200     
00201     while(1) {
00202         pc.printf("%lf\n", filtered_ref_qpps);
00203     }
00204 }