main.cpp

00001 #include "mbed.h"
00002 #include "define.h"
00003 #include "Sabertooth_Serial.h"
00004 #include "PID.h"
00005 #include "QEI.h"
00006 #include "Filter.h"
00007 #include "string"
00008 
00009 Ticker timer;
00010 Timer T;
00011 
00012 SaberSerial MD_S( 19200, p13, p14);
00013 RawSerial Master( p9, p10, 115200);
00014 RawSerial pc( USBTX, USBRX, 115200);
00015 
00016 PID pid1(INT_TIME);
00017 PID pid2(INT_TIME);
00018 
00019 Filter LPF1(INT_TIME);
00020 Filter LPF2(INT_TIME);
00021 
00022 QEI Enc1(p18,p15,NC,RESO,&T,QEI::X4_ENCODING);
00023 QEI Enc2(p17,p16,NC,RESO,&T,QEI::X4_ENCODING);
00024 //_____________________________↓機能設定
00025 
00026 DigitalIn sw1(p29);
00027 DigitalIn sw2(p30);
00028 //DigitalIn pe1(p19);
00029 //DigitalIn pe2(p20);
00030 
00031 DigitalOut fet1(p25);//CB回収
00032 DigitalOut fet2(p26);//つっかえ
00033 DigitalOut fet3(p27);//押出 上
00034 DigitalOut fet4(p28);//押出 下
00035 
00036 DigitalOut led1(LED1);
00037 DigitalOut led2(LED2);
00038 DigitalOut led3(LED3);
00039 DigitalOut led4(LED4);
00040 //_____________________________↓変数宣言
00041 
00042 string Master_str = "";
00043 char global_mode = 0b00000000;
00044 int gl_enc1 = 0, gl_enc2 = 0;
00045 double ref_spd1 = 0, ref_spd2 = 0, load_co = 0, recv_co = 0;
00046 double f_spd1 = 0, f_spd2 = 0;
00047 int gl_cmd1 = 0, gl_cmd2 = 0;
00048 int gl_move = 0, gl_channel = 0;
00049 
00050 void Master_send(char mode){
00051     Master.printf("%c\n",mode);
00052 }
00053 
00054 //______________________________↓受信割込み
00055 
00056 void Master_recv(){
00057     recv_co = 1;
00058     char Master_c = Master.getc();
00059     static char local_mode = 0b00000000;
00060     
00061     if(Master_c == '\n'){
00062         local_mode = Master_str[0];
00063         if(local_mode == EMERGENCY) {
00064             global_mode = EMERGENCY;
00065         }
00066         else {
00067             global_mode = local_mode;
00068         }
00069         Master_str = "";
00070     }
00071     else Master_str += Master_c;
00072 }
00073 //________________________________________↓速度制限・加減速制限
00074 
00075 double limit_MD(double cmd,double max_cmd){
00076     if (cmd > max_cmd) cmd = max_cmd;
00077     else if (cmd < -max_cmd) cmd = -max_cmd;
00078     return cmd;
00079 }
00080 
00081 //_____________________________↓主要処理(周期割込み)
00082 
00083 void timer_warikomi(){
00084     char mode = global_mode;
00085     int sw1_def = sw1.read(), sw2_def = sw2.read();
00086     int enc1 = Enc1.getPulses(), enc2 = Enc2.getPulses();
00087     static char send_mode = 0b00000000;
00088     static bool points1 = 0, points2 = 0, points3 = 0;
00089     static bool positions = 0;
00090     static bool pre_sw1 = 1, pre_sw2 = 1, move = 0, channel = 0;
00091     static int counter = 0, point_co = 0, p1_co = 0 ,p3_co = 0, p2_co = 0;
00092     static int pre_enc1 = 0, pre_enc2 = 0;
00093     static double cmd1 = 0, cmd2 = 0, up_cmd = 0;
00094     
00095     
00096     if(sw1_def == 0 && pre_sw1 == 1) move = !move;
00097     if(sw2_def == 0 && pre_sw2 == 1) channel = !channel;
00098     
00099     pre_sw1 = sw1_def;
00100     pre_sw2 = sw2_def;
00101     
00102     if(!recv_co);
00103     if(mode == EMERGENCY){
00104         f_spd1 = 0;
00105         f_spd2 = 0;
00106         up_cmd = 0;
00107         if(counter >= EMER_COUNT){
00108             led1 = !led1;
00109             led2 = !led2;
00110             led3 = !led3;
00111             led4 = !led4;
00112             counter = 0;
00113         }else counter++;
00114     }else if(mode & AUTOMATION){
00115         up_cmd = 0;
00116         led3 = 1;
00117         led4 = 1;
00118         points1   = (mode & POINTS_1);//20
00119         points2   = (mode & POINTS_2);//40
00120         points3   = (mode & POINTS_3);//10
00121         positions = (mode & POSITIONS);
00122         
00123         if(!points1 && !points2 && !points3){
00124             f_spd1 = 0;
00125             f_spd2 = 0;
00126         }
00127         if (points2){
00128             led4 = 1;
00129             f_spd1 = REF_SPD2_1;
00130             f_spd2 = REF_SPD2_2;
00131             send_mode |= POINTS_2;
00132             if(positions && (enc1 - pre_enc1) > (REF_SPD2_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
00133                 if(p1_co < 3){
00134                     point_co++;
00135                     if(point_co >= 20){
00136                         fet3 = F3_ON;
00137                         p1_co++;
00138                         point_co = 0;
00139                     }else fet3 = F3_OFF;
00140                 }else{
00141                     send_mode |= POSITIONS;
00142                     fet3 = F3_OFF;
00143                 }
00144             }else{
00145                 send_mode &= ~POINTS_1;
00146                 send_mode &= ~POSITIONS;
00147             }
00148         }if(points1){
00149             led4 = 1;
00150             f_spd1 = REF_SPD1_1;
00151             f_spd2 = REF_SPD1_2;
00152             send_mode |= POINTS_1;
00153             if(positions && (enc1 - pre_enc1) > (REF_SPD1_1 - 30) && (enc1 - pre_enc1) < (REF_SPD1_1 + 30)){
00154                 fet3 = F3_ON;
00155                 if(p2_co > 8) send_mode |= POSITIONS;
00156                 else p2_co++;
00157             }else{
00158                 fet3 = F3_OFF;
00159                 send_mode &= ~POINTS_3;
00160                 send_mode &= ~POSITIONS;
00161             }
00162         }if(points3){
00163             led4 = 1;
00164             fet2 = F2_OFF;
00165             fet3 = F3_OFF;
00166             f_spd1 = REF_SPD3_1;
00167             f_spd2 = REF_SPD3_2;
00168             send_mode |= POINTS_3;
00169             if(positions && (enc1 - pre_enc1) > (REF_SPD3_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
00170                 if(p3_co < 6){
00171                     if (point_co >= 20){
00172                         fet4 = F4_ON;
00173                         point_co = 0;
00174                         p3_co++;
00175                     }else {        
00176                         fet4 = F4_OFF;
00177                         send_mode &= ~POINTS_2;
00178                         send_mode &= ~POSITIONS;
00179                     }
00180                 }else{
00181                     if(p3_co > 15){
00182                         fet4 = F4_OFF;
00183                         send_mode |= POSITIONS;
00184                         f_spd1 = 0;
00185                         f_spd2 = 0;
00186                     }else p3_co++;
00187                 }
00188             }
00189         }else if(positions){
00190             led4 = 1;
00191             send_mode |= POSITIONS;
00192         }else if(!positions){
00193             send_mode &= ~POSITIONS;
00194         }else;
00195         
00196         Master_send(send_mode);
00197     }else{
00198         /*led1 = 0;
00199         led2 = 0;
00200         led3 = 0;
00201         led4 = 0;*/
00202         if(mode & CB_COLLECT){
00203             up_cmd = 50;
00204             fet1 = F1_ON;
00205         }else{
00206             fet1 = F1_OFF;
00207             up_cmd = 0;
00208         }
00209         if(move){
00210             led1 = 1;
00211             led2 = 0;
00212             f_spd1 = REF_SPD3_1;
00213             f_spd2 = REF_SPD3_2;
00214             if(channel) fet4 = F4_ON;
00215             else fet4 = F4_OFF;
00216         }else{
00217             led1 = 0;
00218             led2 = 1;
00219             f_spd1 = 0;
00220             f_spd2 = 0;
00221             if(channel) fet4 = F4_ON;
00222             else fet4 = F4_OFF;
00223         }
00224     }
00225     gl_enc1 = enc1 - pre_enc1;
00226     gl_enc2 = enc2 - pre_enc2;
00227     
00228     ref_spd1 = LPF1.LowPassFilter(f_spd1);
00229     ref_spd2 = LPF2.LowPassFilter(f_spd2);
00230     
00231     cmd1 += pid1.con(ref_spd1 - gl_enc1);
00232     cmd1 = limit_MD( cmd1, 120);
00233     if(f_spd1 == 0 && abs(cmd1) < 20) cmd1 = 0;
00234     if(cmd1 >= 0) MD_S.Serial(SB_ADRS1, 1, (int)(abs(cmd1)));
00235     else MD_S.Serial(SB_ADRS1, 0, (int)(abs(cmd1)));
00236         
00237     cmd2 += pid2.con(ref_spd2 - gl_enc2);
00238     cmd2 = limit_MD( cmd2, 120);
00239     if(f_spd2 == 0 && abs(cmd2) < 20) cmd2 = 0;
00240     if(cmd2 >= 0) MD_S.Serial(SB_ADRS1, 5, (int)(abs(cmd2)));
00241     else MD_S.Serial(SB_ADRS1, 6, (int)(abs(cmd2)));
00242     
00243     pre_enc1 = enc1;
00244     pre_enc2 = enc2;
00245     
00246     gl_cmd1 = cmd1;
00247     gl_cmd2 = cmd2;
00248     gl_move = move;
00249     gl_channel = channel;
00250     
00251     MD_S.Serial(SB_ADRS2, 5, up_cmd);
00252 }
00253 //__________________________________________
00254 
00255 int main() {
00256     fet1 = F1_OFF;
00257     fet2 = F2_OFF;
00258     fet3 = F3_OFF;
00259     fet4 = F4_ON;
00260     led1 = 0;
00261     led2 = 0;
00262     led3 = 0;
00263     led4 = 0;
00264     
00265     pid1.set(P1,NU,D1);
00266     pid2.set(P2,NU,D1);
00267     
00268     LPF1.setLowPassPara(2.0, 0);
00269     LPF2.setLowPassPara(2.0, 0);
00270     
00271     timer.attach(&timer_warikomi,INT_TIME);
00272     Master.attach(&Master_recv ,RawSerial::RxIrq);
00273     while(true) {
00274         /*static int buff1 = 0, buff2 = 0;
00275         buff1 = Enc1.getPulses();
00276         buff2 = Enc2.getPulses();*/
00277         pc.printf("ERR1:%d ERR2:%d CMD1:%d CMD2:%d MODE:%d CAHN:%d\n", gl_enc1, gl_enc2, gl_cmd1, gl_cmd2, int(global_mode), gl_channel);
00278         wait(0.1);
00279     }
00280 }