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             
00102             return_zero = 0;                                                    // 停止回0
00103             c_lock = 2;                                                         // 电机位置锁无效
00104 //            send_enable = 0;                                                    // main不发送CAN位置命令
00105             break;
00106         case('m'):
00107             command.printf("\n\rPF entering PC motor mode\r");
00108 //            EnterMotorMode(&PF_can);
00109            
00110 
00111             return_zero = 0;
00112             c_lock = 1;                                                         // 电机位置锁定 
00113 //            send_enable = 1;                                                    // main发送CAN位置命令
00114             break;
00115         case('M'):
00116             command.printf("\n\rPF entering BOARD motor mode\r");
00117 //            EnterMotorMode(&PF_can);
00118             
00119             return_zero = 0;
00120             c_lock = 0;                                                         // 电机位置解锁
00121 //            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定 
00122             break;
00123         case('z'):
00124             command.printf("\n\rPF zeroing\r");
00125 //            Zero(&PF_can);
00126             
00127             return_zero = 0; 
00128             c_lock = 2;
00129 //            send_enable = 0;
00130             break;
00131         case('r'):
00132             command.printf("\n\rPF return zero\r");
00133 //            EnterMotorMode(&PF_can);
00134             
00135             return_zero = 1;
00136             c_lock = 2;
00137 //            send_enable = 1;
00138 //            flag_gd = 1;
00139             break;      
00140     }
00141     /*
00142     switch(Suse[5])
00143     {
00144         case('e'):
00145             command.printf("DF exiting motor mode\n\r");
00146             ExitMotorMode(&DF_can);
00147             c_lock = 1;                                                         // 电机位置解锁
00148             send_enable = 0;                                                    // main不发送CAN位置命令
00149             break;
00150         case('m'):
00151             command.printf("DF entering PC motor mode\n\r");
00152             EnterMotorMode(&DF_can);
00153             c_lock = 1;                                                         // 电机位置锁定
00154             send_enable = 1;                                                    // main发送CAN位置命令
00155             break;
00156         case('M'):
00157             command.printf("DF entering BOARD motor mode\n\r");
00158             EnterMotorMode(&DF_can);
00159             c_lock = 0;                                                         // 电机位置解锁
00160             send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定
00161             break;
00162         case('z'):
00163             command.printf("DF zeroing\n\r");
00164             Zero(&DF_can);
00165             break;   
00166     }
00167     */
00168     
00169     wait_ms(2);
00170     
00171 //    sf_m_c = 0;                                                                 // 恢复接收模式
00172     wait_us(200);
00173     
00174     
00175     
00176     // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
00177     WriteAll();
00178     //wait(1.0f);                        
00179     
00180     // 解除串口控制，缓冲区清0
00181     command_control_flag = 0;
00182     for(unsigned int i = 0; i < 10; i++)
00183     {
00184         Suse[i] = 0;      
00185     }
00186 }
00187 
00188 
00189 
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