data_command.cpp

00001 #include "data_command.h"
00002 #include "data_pc.h"
00003 #include "CAN.h"
00004 
00005 Serial      command(PA_9, PA_10);
00006 
00007 
00008 int enabled = 0;                                                                // 进入电机模式标志位
00009 int counter = 0;                                                                // 输出计数器
00010 int command_control_flag = 0;                                                   // 命令帧标志位
00011 float SP_pf = 0, SP_df = 0;
00012 int c_lock = 2;                                                                 // 位置锁定标志位 
00013 int return_zero = 0;                                                            // 电机回0标志位
00014 
00015 unsigned int flag_c_A = 0, flag_c_B = 0;
00016 unsigned int Snum_c = 0;
00017 uint16_t Sget[10] = {0};                                                    
00018 uint16_t Suse[10] = {0};
00019 
00020 
00021 void serial_command_isr()
00022 {
00023     while(command.readable())
00024     {
00025         uint8_t c = command.getc();
00026         if(c == 'A')
00027         {
00028             flag_c_A = 1;
00029             flag_c_B = 0;
00030             Snum_c   = 0;
00031             for(unsigned int i = 0; i < 10; i++)
00032             {
00033                 Sget[i] = 0;      
00034             }
00035             
00036             break;  
00037         }
00038         if(c == 'B')
00039         {
00040             flag_c_B = 1;
00041         }
00042     
00043         if(flag_c_A == 1)
00044         {
00045             if((flag_c_B != 1) && (Snum_c < 10))
00046             {
00047                 Sget[Snum_c] = c;    
00048             }
00049             
00050             Snum_c++;
00051             
00052             if((flag_c_B == 1) && (Snum_c != 11))
00053             {                        
00054                 flag_c_A = 0;
00055                 flag_c_B = 0;
00056                 Snum_c   = 0;     
00057             }
00058             
00059             if((flag_c_B == 1) && (Snum_c == 11))
00060             {
00061                 flag_c_A = 0;
00062                 flag_c_B = 0;
00063                 Snum_c   = 0; 
00064                 
00065                 for(unsigned int i = 0; i < 10; i++)
00066                 {
00067                     Suse[i] = Sget[i];
00068                 }  
00069                 
00070                 command_control_flag = 1;                                       // 命令帧接收成功
00071                 
00072                 pc.printf("%c%c%c%c%c%c%c%c%c%c\n\r", Suse[0], Suse[1], Suse[2], Suse[3], Suse[4], Suse[5], Suse[6], Suse[7], Suse[8], Suse[9]);
00073             }
00074         }
00075     }    
00076 }
00077 
00078 
00079 void command_control()
00080 {
00081     SP_pf = 10*(Suse[2] - '0') + 1*(Suse[3] - '0') + 0.1*(Suse[4] - '0');
00082     SP_df = 10*(Suse[7] - '0') + 1*(Suse[8] - '0') + 0.1*(Suse[9] - '0');
00083     if(Suse[1] == '+')
00084         SP_pf = 0 + SP_pf;
00085     if(Suse[1] == '-')
00086         SP_pf = 0 - SP_pf;
00087     if(Suse[6] == '+')
00088         SP_df = 0 + SP_df;
00089     if(Suse[6] == '-')
00090         SP_df = 0 - SP_df;
00091     
00092     
00093     // 切换为发送模式
00094     wait_us(200);
00095     
00096     switch(Suse[0])
00097     {
00098         case('e'):
00099             command.printf("\n\rPF exiting motor mode\r");
00100 //          ExitMotorMode(&PF_can);
00101             ExitMotorMode(&knee_txMsg);           
00102             ExitMotorMode(&ankle_txMsg);
00103             return_zero = 0;                                                    // 停止回0
00104             c_lock = 2;                                                         // 电机位置锁无效
00105 //            send_enable = 0;                                                    // main不发送CAN位置命令
00106             break;
00107         case('m'):
00108             command.printf("\n\rPF entering PC motor mode\r");
00109 //            EnterMotorMode(&PF_can);
00110             EnterMotorMode(&knee_txMsg);           
00111             EnterMotorMode(&ankle_txMsg);
00112 
00113             return_zero = 0;
00114             c_lock = 1;                                                         // 电机位置锁定 
00115 //            send_enable = 1;                                                    // main发送CAN位置命令
00116             break;
00117         case('M'):
00118             command.printf("\n\rPF entering BOARD motor mode\r");
00119 //            EnterMotorMode(&PF_can);
00120             EnterMotorMode(&knee_txMsg);           
00121             EnterMotorMode(&ankle_txMsg);
00122             return_zero = 0;
00123             c_lock = 0;                                                         // 电机位置解锁
00124 //            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定 
00125             break;
00126         case('z'):
00127             command.printf("\n\rPF zeroing\r");
00128 //            Zero(&PF_can);
00129             Zero(&knee_txMsg);
00130             Zero(&ankle_txMsg);
00131             return_zero = 0; 
00132             c_lock = 2;
00133 //            send_enable = 0;
00134             break;
00135         case('r'):
00136             command.printf("\n\rPF return zero\r");
00137 //            EnterMotorMode(&PF_can);
00138             EnterMotorMode(&knee_txMsg);           
00139             EnterMotorMode(&ankle_txMsg);
00140             return_zero = 1;
00141             c_lock = 2;
00142 //            send_enable = 1;
00143 //            flag_gd = 1;
00144             break;      
00145     }
00146     /*
00147     switch(Suse[5])
00148     {
00149         case('e'):
00150             command.printf("DF exiting motor mode\n\r");
00151             ExitMotorMode(&DF_can);
00152             c_lock = 1;                                                         // 电机位置解锁
00153             send_enable = 0;                                                    // main不发送CAN位置命令
00154             break;
00155         case('m'):
00156             command.printf("DF entering PC motor mode\n\r");
00157             EnterMotorMode(&DF_can);
00158             c_lock = 1;                                                         // 电机位置锁定
00159             send_enable = 1;                                                    // main发送CAN位置命令
00160             break;
00161         case('M'):
00162             command.printf("DF entering BOARD motor mode\n\r");
00163             EnterMotorMode(&DF_can);
00164             c_lock = 0;                                                         // 电机位置解锁
00165             send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定
00166             break;
00167         case('z'):
00168             command.printf("DF zeroing\n\r");
00169             Zero(&DF_can);
00170             break;   
00171     }
00172     */
00173     
00174     wait_ms(2);
00175     
00176 //    sf_m_c = 0;                                                                 // 恢复接收模式
00177     wait_us(200);
00178     
00179     
00180     
00181     // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
00182     WriteAll();
00183     //wait(1.0f);                        
00184     
00185     // 解除串口控制，缓冲区清0
00186     command_control_flag = 0;
00187     for(unsigned int i = 0; i < 10; i++)
00188     {
00189         Suse[i] = 0;      
00190     }
00191 }
00192 
00193 
00194 
00195 
00196 
00197 
00198 
00199 
00200 
00201 
00202 
00203 
00204 
00205 
00206 
00207 
00208 
00209 
00210 
00211 
00212 
00213 
00214 
00215 
00216 
00217 
00218 
00219 
00220 
00221 
00222 
00223 
00224 
00225 
00226 
00227 
00228 
00229 
00230 
00231 
00232