Shao Rui
/
SPIne_referenceTraj_DEMO
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 00002 00003 #include "mbed.h" 00004 #include "math_ops.h" 00005 #include <cstring> 00006 #include "leg_message.h" 00007 #include "referenceTraj.h" 00008 #include "FastMath.h" //hjb added 00009 //#include "rtos.h" //hjb added 00010 00011 //Thread thread; //hjb added 00012 // length of receive/transmit buffers 00013 #define RX_LEN 66 00014 #define TX_LEN 66 00015 00016 // length of outgoing/incoming messages 00017 #define DATA_LEN 30 00018 #define CMD_LEN 66 00019 00020 // Master CAN ID /// 00021 #define CAN_ID 0x0 00022 00023 00024 /// Value Limits /// 00025 #define P_MIN -12.5f 00026 #define P_MAX 12.5f 00027 #define V_MIN -45.0f 00028 #define V_MAX 45.0f 00029 #define KP_MIN 0.0f 00030 #define KP_MAX 500.0f 00031 #define KD_MIN 0.0f 00032 #define KD_MAX 5.0f 00033 //#define T_MIN -18.0f 00034 //#define T_MAX 18.0f 00035 00036 #define T_MIN -3.0f //hjb changed test 00037 #define T_MAX 3.0f 00038 #define SPI_TIMEOUT 1000 // hjb added for spi time out 00039 00040 /// Joint Soft Stops /// 00041 #define A_LIM_P 5.0f//1.5f 00042 #define A_LIM_N -5.0f//-1.5f 00043 #define H_LIM_P 5.0f 00044 #define H_LIM_N -5.0f 00045 #define K_LIM_P 0.2f 00046 #define K_LIM_N 7.7f 00047 #define KP_SOFTSTOP 0.0f//100.0f 00048 #define KD_SOFTSTOP 0.4f; 00049 00050 #define ENABLE_CMD 0xFFFF 00051 #define DISABLE_CMD 0x1F1F 00052 00053 spi_data_t spi_data; // data from spine to up 00054 spi_command_t spi_command; // data from up to spine 00055 spi_command_t *mspi_command=&spi_command; // data from up to spine 00056 00057 // spi buffers 00058 uint16_t rx_buff[RX_LEN]; 00059 uint16_t tx_buff[TX_LEN]; 00060 00061 DigitalOut led(PC_5); 00062 00063 00064 Serial pc(PA_2, PA_3); 00065 CAN can1(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name 00066 CAN can2(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name 00067 00068 CANMessage rxMsg1, rxMsg2; 00069 CANMessage txMsg1, txMsg2; 00070 CANMessage a1_can, a2_can, h1_can, h2_can, k1_can, k2_can; //TX Messages 00071 int ledState; 00072 Ticker sendCAN; 00073 volatile int counter = 0; 00074 volatile bool msgAvailable = false; 00075 Ticker loop; 00076 00077 int spi_enabled = 0; 00078 InterruptIn cs(PA_4); 00079 //**//DigitalIn estop(PA_14); 00080 DigitalIn estop(PB_15); 00081 //SPISlave spi(PA_7, PA_6, PA_5, PA_4); 00082 00083 00084 leg_state l1_state, l2_state;; 00085 leg_control l1_control, l2_control; 00086 00087 uint16_t x = 0; 00088 uint16_t x2 = 0; 00089 uint16_t count = 0; 00090 uint16_t counter2 = 0; 00091 00092 int control_mode = 1; 00093 int is_standing = 0; 00094 int enabled = 0; 00095 00096 // generates fake spi data from spi command 00097 void test_control(); 00098 void control(); 00099 00100 //==========hjb=======//=================================================================================// 00101 /////////////////////////////////////////////////////// 00102 #define MULTI200 1 00103 #define MAX_TRACK 10240*5 00104 #define MIN_SAMPLE 0.001 00105 float TimeSample, TimeSample2; 00106 #define TS (MULTI200 * MIN_SAMPLE) // Sample Time 00107 #define SP 0.2 // Interval 00108 00109 float Fz_mea;//2.7 00110 float trajRef[5][9], trajRef2[5][9]; // ref. Traj. 0..2 -> q_d, 3..5-> v_d, 6..8->a_d. 00111 REF ref[5]; 00112 /////////////////////////////////////////////////////// 00113 //++++++++ FOURARM +++++++++++++++++++++++// 00114 float g_ArmSpeed = 250;//2.50f; //控制关节速度 00115 float g_ArmJointAngleRe[5]; 00116 float g_ArmJointAngleDe[5]; 00117 float g_ArmJointAngleLineDe[5]; 00118 float g_ArmJointAngleDe_Past[5] = {0,0,0,0,0}; 00119 float g_ArmJointAngleDe_PastP[5] = {0,0,0,0,0}; 00120 float g_ArmTrajRef[15]; 00121 float g_ArmTrajLineRef[15]; 00122 int g_ArmInit=TRUE; 00123 static int Control_Flag = 0; 00124 static int timeout = 0; 00125 00126 //++++++++ FOURARM +++++++++++++// 00127 /////////////////////////////////////////////////////// 00128 //++++++++ FOURARM +++++++++++++++++++++++// 00129 #define ARMSP 0.200 // Interval 00130 #define ARMJOINTSPEED 2.5f //关节速度定义 2.5f 00131 #define ARMJOINTACC 0.100f //关节加速度时间 00132 00133 //++++++++ FOURARM +++++++++++++++++++++++// 00134 ARMREF g_ArmRef; 00135 //++++++++ FOURARM +++++++++++++// 00136 //================HJB=============added========== 00137 using namespace FastMath; 00138 volatile float Init_pos = 0; 00139 volatile float Init_pos1 = 0;//shaorui add 00140 volatile float Pmag = 1; 00141 volatile float Pmag1 = 1;//shaorui add 00142 volatile float Pmag_last = 1; 00143 volatile float Pmag_last1 = 1;//shaorui add 00144 volatile float Tperiod = 25; 00145 volatile float Tperiod1= 25;//shaorui add 00146 volatile float p_des_HJB=0; 00147 volatile float p_des_shaorui=0;//shaorui add 00148 volatile float v_des_HJB=0; 00149 volatile float v_des_shaorui=0;//shaorui add 00150 volatile int count_shaorui= 0; 00151 volatile int count_HJB= 0; //shaorui add 00152 //===================HJB end=================== 00153 00154 00155 //==========hjb=======//=================================================================================// 00156 00157 00158 00159 00160 /// CAN Command Packet Structure /// 00161 /// 16 bit position command, between -4*pi and 4*pi 00162 /// 12 bit velocity command, between -30 and + 30 rad/s 00163 /// 12 bit kp, between 0 and 500 N-m/rad 00164 /// 12 bit kd, between 0 and 100 N-m*s/rad 00165 /// 12 bit feed forward torque, between -18 and 18 N-m 00166 /// CAN Packet is 8 8-bit words 00167 /// Formatted as follows. For each quantity, bit 0 is LSB 00168 /// 0: [position[15-8]] 00169 /// 1: [position[7-0]] 00170 /// 2: [velocity[11-4]] 00171 /// 3: [velocity[3-0], kp[11-8]] 00172 /// 4: [kp[7-0]] 00173 /// 5: [kd[11-4]] 00174 /// 6: [kd[3-0], torque[11-8]] 00175 /// 7: [torque[7-0]] 00176 00177 void pack_cmd(CANMessage * msg, joint_control joint){ 00178 00179 /// limit data to be within bounds /// 00180 //float p_des = fminf(fmaxf(P_MIN, uint_to_float(joint.p_des, P_MIN, P_MAX, 16)), P_MAX); 00181 //float v_des = fminf(fmaxf(V_MIN, uint_to_float(joint.v_des, V_MIN, V_MAX, 12)), V_MAX); 00182 //float p_des = fminf(fmaxf(P_MIN, p_des_HJB), P_MAX); 00183 //float v_des = fminf(fmaxf(V_MIN, v_des_HJB), V_MAX); 00184 float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX); 00185 float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX); 00186 float kp = fminf(fmaxf(KP_MIN, uint_to_float(joint.kp, KP_MIN, KP_MAX, 12)), KP_MAX); 00187 float kd = fminf(fmaxf(KD_MIN, uint_to_float(joint.kd, KD_MIN, KD_MAX, 12)), KD_MAX); 00188 float t_ff = fminf(fmaxf(T_MIN, uint_to_float(joint.t_ff, T_MIN, T_MAX, 12)), T_MAX); 00189 /// convert floats to unsigned ints /// 00190 uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); 00191 uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); 00192 uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); 00193 uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); 00194 uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12); 00195 /// pack ints into the can buffer /// 00196 msg->data[0] = p_int>>8; 00197 msg->data[1] = p_int&0xFF; 00198 msg->data[2] = v_int>>4; 00199 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); 00200 msg->data[4] = kp_int&0xFF; 00201 msg->data[5] = kd_int>>4; 00202 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8); 00203 msg->data[7] = t_int&0xff; 00204 ///printf("p:%d v:%d kp:%d kd:%d t:%d ",(uint16_t)p_int, (uint16_t)v_int, kp_int, kd_int, t_int); 00205 00206 } 00207 00208 /// CAN Reply Packet Structure /// 00209 /// 16 bit position, between -4*pi and 4*pi 00210 /// 12 bit velocity, between -30 and + 30 rad/s 00211 /// 12 bit current, between -40 and 40; 00212 /// CAN Packet is 5 8-bit words 00213 /// Formatted as follows. For each quantity, bit 0 is LSB 00214 /// 0: [position[15-8]] 00215 /// 1: [position[7-0]] 00216 /// 2: [velocity[11-4]] 00217 /// 3: [velocity[3-0], current[11-8]] 00218 /// 4: [current[7-0]] 00219 00220 void unpack_reply(CANMessage msg, leg_state * leg){ 00221 /// unpack ints from can buffer /// 00222 uint16_t id = msg.data[0]; 00223 uint16_t p_int = (msg.data[1]<<8)|msg.data[2]; 00224 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4); 00225 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; 00226 /// convert uints to floats /// 00227 float p = uint_to_float(p_int, P_MIN, P_MAX, 16); 00228 float v = uint_to_float(v_int, V_MIN, V_MAX, 12); 00229 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12); 00230 00231 if(id==1){ 00232 leg->a.p = p; 00233 leg->a.v = v; 00234 leg->a.t = t; 00235 //printf("p=%.3f v=%.3f i=%.3f", p, v, t); 00236 } 00237 else if(id==2){ 00238 leg->h.p = p; 00239 leg->h.v = v; 00240 leg->h.t = t; 00241 } 00242 else if(id==3){ 00243 leg->k.p = p; 00244 leg->k.v = v; 00245 leg->k.t = t; 00246 } 00247 00248 } 00249 00250 void rxISR1() { 00251 can1.read(rxMsg1); // read message into Rx message storage 00252 printf("%c\n",rxMsg1.id); //shaorui add 00253 unpack_reply(rxMsg1, &l1_state); 00254 } 00255 void rxISR2(){ 00256 can2.read(rxMsg2); 00257 unpack_reply(rxMsg2, &l2_state); 00258 } 00259 void PackAll(){ 00260 pack_cmd(&a1_can, l1_control.a); 00261 pack_cmd(&a2_can, l2_control.a); 00262 pack_cmd(&h1_can, l1_control.h); 00263 pack_cmd(&h2_can, l2_control.h); 00264 pack_cmd(&k1_can, l1_control.k); 00265 pack_cmd(&k2_can, l2_control.k); 00266 00267 } 00268 void WriteAll(){ 00269 //toggle = 1; 00270 can1.write(a1_can); 00271 wait(.0001); 00272 can2.write(a2_can); 00273 wait(.0002); 00274 can1.write(h1_can); 00275 wait(.0001); 00276 can2.write(h2_can); 00277 wait(.0002); 00278 00279 can1.write(k1_can); 00280 wait(.0001); 00281 can2.write(k2_can); 00282 //wait(.0001); 00283 00284 //toggle = 0; 00285 } 00286 00287 void sendCMD(){ 00288 Control_Flag = 1; 00289 //=========================hjb ==============================================================// 00290 //#define PI 3.1415926f 00291 //static int init = TRUE; 00292 //static double Tacc[3]={0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S 00293 //static float MAX_STEP[3]={12.0, 12.0, 12.0}; // max step limitation 12 degree 00294 //control(); 00295 00296 00297 WriteAll(); 00298 00299 00300 } 00301 00302 00303 00304 00305 void Zero(CANMessage * msg){ 00306 msg->data[0] = 0xFF; 00307 msg->data[1] = 0xFF; 00308 msg->data[2] = 0xFF; 00309 msg->data[3] = 0xFF; 00310 msg->data[4] = 0xFF; 00311 msg->data[5] = 0xFF; 00312 msg->data[6] = 0xFF; 00313 msg->data[7] = 0xFE; 00314 WriteAll(); 00315 } 00316 00317 void EnterMotorMode(CANMessage * msg){ 00318 msg->data[0] = 0xFF; 00319 msg->data[1] = 0xFF; 00320 msg->data[2] = 0xFF; 00321 msg->data[3] = 0xFF; 00322 msg->data[4] = 0xFF; 00323 msg->data[5] = 0xFF; 00324 msg->data[6] = 0xFF; 00325 msg->data[7] = 0xFC; 00326 //WriteAll(); 00327 } 00328 00329 void ExitMotorMode(CANMessage * msg){ 00330 msg->data[0] = 0xFF; 00331 msg->data[1] = 0xFF; 00332 msg->data[2] = 0xFF; 00333 msg->data[3] = 0xFF; 00334 msg->data[4] = 0xFF; 00335 msg->data[5] = 0xFF; 00336 msg->data[6] = 0xFF; 00337 msg->data[7] = 0xFD; 00338 //WriteAll(); 00339 } 00340 void serial_isr(){ 00341 /// handle keyboard commands from the serial terminal /// 00342 while(pc.readable()){ 00343 char c = pc.getc(); 00344 led = !led; 00345 switch(c){ 00346 case(27): 00347 //loop.detach(); 00348 printf("\n\r exiting motor mode \n\r"); 00349 ExitMotorMode(&a1_can); 00350 ExitMotorMode(&a2_can); 00351 ExitMotorMode(&h1_can); 00352 ExitMotorMode(&h2_can); 00353 ExitMotorMode(&k1_can); 00354 ExitMotorMode(&k2_can); 00355 enabled = 0; 00356 break; 00357 case('m'): 00358 printf("\n\r entering motor mode \n\r"); 00359 EnterMotorMode(&a1_can); 00360 EnterMotorMode(&a2_can); 00361 EnterMotorMode(&h1_can); 00362 EnterMotorMode(&h2_can); 00363 EnterMotorMode(&k1_can); 00364 EnterMotorMode(&k2_can); 00365 //WriteAll(); //hjb added 00366 //wait(.5); //hjb delete 00367 enabled = 1; 00368 00369 break; 00370 case('s'): 00371 printf("\n\r standing \n\r"); 00372 counter2 = 0; 00373 is_standing = 1; 00374 //stand(); 00375 break; 00376 case('z'): 00377 printf("\n\r zeroing \n\r"); 00378 Zero(&a1_can); 00379 Zero(&a2_can); 00380 Zero(&h1_can); 00381 Zero(&h2_can); 00382 Zero(&k1_can); 00383 Zero(&k2_can); 00384 break; 00385 } 00386 } 00387 WriteAll(); 00388 00389 00390 } 00391 00392 uint16_t xor_checksum(uint16_t* data, size_t len) 00393 { 00394 uint16_t t = 0; 00395 for(int i = 0; i < len; i++) 00396 t = t ^ data[i]; 00397 return t; 00398 } 00399 00400 void spi_isr(void) 00401 { 00402 //led = !led; // HJB added 00403 timeout = 0; 00404 GPIOC->ODR |= (1 << 8); 00405 GPIOC->ODR &= ~(1 << 8); 00406 int bytecount = 0; 00407 // tx_buff[0]=0x1111; //hjb added 00408 SPI1->DR = tx_buff[0]; 00409 while(cs == 0) { 00410 if(SPI1->SR&0x1) { 00411 rx_buff[bytecount] = SPI1->DR; 00412 bytecount++; 00413 if(bytecount<TX_LEN) { 00414 SPI1->DR = tx_buff[bytecount]; 00415 } 00416 } 00417 00418 } 00419 00420 // after reading, save into spi_command 00421 // should probably check checksum first! 00422 //uint16_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32); //===hjb del ==== 00423 uint16_t calc_checksum = xor_checksum(rx_buff,32); 00424 // for(int i = 0; i < CMD_LEN; i++) 00425 // { 00426 // ((uint16_t*)(&spi_command))[i] = rx_buff[i]; 00427 //} 00428 spi_command.q_des_abad[0] = rx_buff[0]; 00429 spi_command.q_des_hip[0] = rx_buff[2]; 00430 spi_command.q_des_knee[0] = rx_buff[4]; 00431 00432 spi_command.qd_des_abad[0] = rx_buff[6]; 00433 spi_command.qd_des_hip[0] = rx_buff[8]; 00434 spi_command.qd_des_knee[0] = rx_buff[10]; 00435 00436 spi_command.kp_abad[0] = rx_buff[12]; 00437 spi_command.kp_hip[0] = rx_buff[14]; 00438 spi_command.kp_knee[0] = rx_buff[16]; 00439 00440 spi_command.kd_abad[0] = rx_buff[18]; 00441 spi_command.kd_hip[0] = rx_buff[20]; 00442 spi_command.kd_knee[0] = rx_buff[22]; 00443 00444 spi_command.tau_abad_ff[0] = rx_buff[24]; 00445 spi_command.tau_hip_ff[0] = rx_buff[26]; 00446 spi_command.tau_knee_ff[0] = rx_buff[28]; 00447 00448 spi_command.flags[0] = rx_buff[30]; 00449 00450 spi_command.q_des_abad[1] = rx_buff[1]; 00451 spi_command.q_des_hip[1] = rx_buff[3]; 00452 spi_command.q_des_knee[1] = rx_buff[5]; 00453 00454 spi_command.qd_des_abad[1] = rx_buff[7]; 00455 spi_command.qd_des_hip[1] = rx_buff[9]; 00456 spi_command.qd_des_knee[1] = rx_buff[11]; 00457 00458 spi_command.kp_abad[1] = rx_buff[13]; 00459 spi_command.kp_hip[1] = rx_buff[15]; 00460 spi_command.kp_knee[1] = rx_buff[17]; 00461 00462 spi_command.kd_abad[1] = rx_buff[19]; 00463 spi_command.kd_hip[1] = rx_buff[21]; 00464 spi_command.kd_knee[1] = rx_buff[23]; 00465 00466 spi_command.tau_abad_ff[1] = rx_buff[25]; 00467 spi_command.tau_hip_ff[1] = rx_buff[27]; 00468 spi_command.tau_knee_ff[1] = rx_buff[29]; 00469 00470 spi_command.flags[1] = rx_buff[31]; 00471 00472 spi_command.checksum = rx_buff[32]; 00473 // run control, which fills in tx_buff for the next iteration 00474 if(calc_checksum != spi_command.checksum){ 00475 spi_data.flags[1] = 0xdead;} 00476 00477 //test_control(); 00478 //spi_data.q_abad[0] = 13.0f; 00479 //control(); 00480 //PackAll(); //hjb cancel 00481 //WriteAll(); //hjb cancel 00482 //sendCMD(); //hjb added 00483 00484 //for (int i=0; i<TX_LEN; i++) { 00485 // printf("%d ", rx_buff[i]); 00486 // printf("R= %.3f",*(&spi_command)[i]); 00487 // } 00488 //printf("\n\r"); 00489 00490 } 00491 00492 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ //**// exceed limit edge, using the impedance control 00493 if((state.p)>=limit_p){ 00494 //control->p_des = limit_p; 00495 control->v_des = 0.0f; 00496 control->kp = 0; 00497 control->kd = KD_SOFTSTOP; 00498 control->t_ff += KP_SOFTSTOP*(limit_p - state.p); 00499 //control->t_ff = KP_SOFTSTOP*(limit_p - state.p); //hjb changed 00500 return 1; 00501 } 00502 else if((state.p)<=limit_n){ 00503 //control->p_des = limit_n; 00504 control->v_des = 0.0f; 00505 control->kp = 0; 00506 control->kd = KD_SOFTSTOP; 00507 control->t_ff += KP_SOFTSTOP*(limit_n - state.p); 00508 //control->t_ff = KP_SOFTSTOP*(limit_n - state.p); //hjb changed 00509 return 1; 00510 } 00511 return 0; 00512 00513 } 00514 00515 00516 void control() 00517 { 00518 spi_data.q_abad[0] = l1_state.a.p; 00519 spi_data.q_hip[0] = l1_state.h.p; 00520 spi_data.q_knee[0] = l1_state.k.p; 00521 spi_data.qd_abad[0] = l1_state.a.v; 00522 spi_data.qd_hip[0] = l1_state.h.v; 00523 spi_data.qd_knee[0] = l1_state.k.v; 00524 00525 spi_data.q_abad[1] = l2_state.a.p; 00526 spi_data.q_hip[1] = l2_state.h.p; 00527 spi_data.q_knee[1] = l2_state.k.p; 00528 spi_data.qd_abad[1] = l2_state.a.v; 00529 spi_data.qd_hip[1] = l2_state.h.v; 00530 spi_data.qd_knee[1] = l2_state.k.v; 00531 00532 tx_buff[0] = float_to_uint(spi_data.q_abad[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[0]); 00533 tx_buff[1] = float_to_uint(spi_data.q_abad[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[1]); 00534 tx_buff[2] = float_to_uint(spi_data.q_hip[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[0]); 00535 tx_buff[3] = float_to_uint(spi_data.q_hip[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[1]); 00536 tx_buff[4] = float_to_uint(spi_data.q_knee[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[0]); 00537 tx_buff[5] = float_to_uint(spi_data.q_knee[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[1]); 00538 00539 tx_buff[6] = float_to_uint(spi_data.qd_abad[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[0]); 00540 tx_buff[7] = float_to_uint(spi_data.qd_abad[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[1]); 00541 tx_buff[8] = float_to_uint(spi_data.qd_hip[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[0]); 00542 tx_buff[9] = float_to_uint(spi_data.qd_hip[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[1]); 00543 tx_buff[10] = float_to_uint(spi_data.qd_knee[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[0]); 00544 tx_buff[11] = float_to_uint(spi_data.qd_knee[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[1]); 00545 00546 tx_buff[12] = (uint16_t)(spi_data.flags[0]); 00547 tx_buff[13] = (uint16_t)(spi_data.flags[1]); 00548 spi_data.checksum = xor_checksum(tx_buff,14); 00549 00550 tx_buff[14] = (spi_data.checksum)&0xFFFF; 00551 00552 //=================================================hjb added==========================// 00553 static double g_ArmTacc[5] = {0.060, 0.060, 0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S 00554 static float g_ArmMAX_STEP[5] = {20.0, 20.0, 20.0, 20.0, 20.0}; 00555 //static float Accr1 = 0.0,Accr2 = 0.0,Accr3 = 0.0,Accr4 = 0.0,Accr5 = 0.0; 00556 int k =0; 00557 for (k=0; k<5; k++) g_ArmTacc[k] = ARMJOINTACC; //0.030f; 00558 for (k=0; k<5; k++) g_ArmMAX_STEP[k] = g_ArmSpeed*2.0f*g_ArmTacc[k]; 00559 timeout++; 00560 if((timeout > SPI_TIMEOUT) && (SPI_TIMEOUT > 0)){ 00561 enabled = 0; 00562 memset(&l1_control, 0, sizeof(l1_control)); 00563 memset(&l2_control, 0, sizeof(l2_control)); 00564 ExitMotorMode(&a1_can); 00565 //can1.write(a1_can); 00566 //wait(0.0001); 00567 ExitMotorMode(&a2_can); 00568 //can2.write(a2_can); 00569 //wait(0.0001); 00570 ExitMotorMode(&k1_can); 00571 //can1.write(k1_can); 00572 //wait(0.0001); 00573 ExitMotorMode(&k2_can); 00574 //can2.write(k2_can); 00575 //wait(0.0001); 00576 ExitMotorMode(&h1_can); 00577 //can1.write(h1_can); 00578 //wait(0.0001); 00579 ExitMotorMode(&h2_can); 00580 //can2.write(k2_can); 00581 //printf("Time out\n\r"); 00582 spi_data.flags[0] = 0xdead; 00583 spi_data.flags[1] = 0xdead; 00584 led = 1; // HJB added 00585 return; 00586 } 00587 else 00588 { 00589 if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){ 00590 //===============================================HJB added====================================================// 00591 Init_pos = spi_data.q_abad[0]; //==== initial the first position 00592 count_HJB = 0; //for trajectory hjb 00593 count_shaorui = 0; //for trajectory 00594 enabled = 1; 00595 EnterMotorMode(&a1_can); 00596 can1.write(a1_can); 00597 wait(0.0001); 00598 EnterMotorMode(&a2_can); 00599 can2.write(a2_can); 00600 wait(0.0001); 00601 EnterMotorMode(&k1_can); 00602 can1.write(k1_can); 00603 wait(0.0001); 00604 EnterMotorMode(&k2_can); 00605 can2.write(k2_can); 00606 wait(0.0001); 00607 EnterMotorMode(&h1_can); 00608 can1.write(h1_can); 00609 wait(0.0001); 00610 EnterMotorMode(&h2_can); 00611 can2.write(h2_can); 00612 wait(0.0001); 00613 printf("e\n\r"); 00614 return; 00615 } 00616 else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ //**//exit motor mode 00617 enabled = 0; 00618 ExitMotorMode(&a1_can); 00619 //can1.write(a1_can); 00620 ExitMotorMode(&a2_can); 00621 //can2.write(a2_can); 00622 ExitMotorMode(&h1_can); 00623 //can1.write(h1_can); 00624 ExitMotorMode(&h2_can); 00625 //can2.write(h2_can); 00626 ExitMotorMode(&k1_can); 00627 //can1.write(k1_can); 00628 ExitMotorMode(&k2_can); 00629 //can2.write(k2_can); 00630 printf("x\n\r"); 00631 return; 00632 } 00633 } 00634 if(enabled){ 00635 if(estop==0){ 00636 //printf("estopped!!!!\n\r"); 00637 //enabled = 0; //hjb added 00638 memset(&l1_control, 0, sizeof(l1_control)); 00639 memset(&l2_control, 0, sizeof(l2_control)); 00640 ExitMotorMode(&a1_can); 00641 //can1.write(a1_can); 00642 ExitMotorMode(&a2_can); 00643 //can2.write(a2_can); 00644 ExitMotorMode(&h1_can); 00645 //can1.write(h1_can); 00646 ExitMotorMode(&h2_can); 00647 //can2.write(h2_can); 00648 ExitMotorMode(&k1_can); 00649 //can1.write(k1_can); 00650 ExitMotorMode(&k2_can); 00651 //can2.write(k2_can); 00652 00653 spi_data.flags[0] = 0xdead; 00654 spi_data.flags[1] = 0xdead; 00655 led = 1; // HJB added 00656 00657 return; 00658 } 00659 00660 00661 00662 else{ 00663 led = 0; 00664 memset(&l1_control, 0, sizeof(l1_control)); 00665 memset(&l2_control, 0, sizeof(l2_control)); 00666 00667 g_ArmJointAngleDe[0] =uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); //input angle from platform 00668 g_ArmJointAngleDe[1] =uint_to_float(spi_command.q_des_hip[0], P_MIN, P_MAX, 16);//spi_command.q_des_hip[0]; 00669 g_ArmJointAngleDe[2] =uint_to_float(spi_command.q_des_knee[0], P_MIN, P_MAX, 16);//spi_command.q_des_knee[0]; 00670 g_ArmJointAngleDe[3] =uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16); ; 00671 g_ArmJointAngleDe[4] =uint_to_float(spi_command.q_des_hip[1], P_MIN, P_MAX, 16); 00672 00673 g_ArmJointAngleRe[0] = l1_state.a.p; 00674 g_ArmJointAngleRe[1] = l1_state.h.p; 00675 g_ArmJointAngleRe[2] = l1_state.k.p; 00676 g_ArmJointAngleRe[3] = l2_state.a.p; 00677 g_ArmJointAngleRe[4] = l2_state.h.p; 00678 arm_reference_trajectory(g_ArmInit, TS, ARMSP, g_ArmTacc, &g_ArmRef, g_ArmMAX_STEP, g_ArmJointAngleRe, g_ArmJointAngleDe, g_ArmTrajRef); 00679 00680 g_ArmInit = FALSE; 00681 00682 l1_control.a.p_des = g_ArmTrajRef[0]; 00683 l1_control.h.p_des = g_ArmTrajRef[1]; 00684 l1_control.k.p_des = g_ArmTrajRef[2]; 00685 l2_control.a.p_des = g_ArmTrajRef[3]; 00686 l2_control.h.p_des = g_ArmTrajRef[4]; 00687 00688 l1_control.a.v_des = g_ArmTrajRef[5]; 00689 l1_control.h.v_des = g_ArmTrajRef[6]; 00690 l1_control.k.v_des = g_ArmTrajRef[7]; 00691 l2_control.a.v_des = g_ArmTrajRef[8]; 00692 l2_control.h.v_des = g_ArmTrajRef[9]; 00693 00694 //========================================HJB added for trajectory input=========================================// 00695 Pmag = uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); 00696 Tperiod = uint_to_float(spi_command.qd_des_abad[0], V_MIN, V_MAX, 12);//; 00697 if (Pmag != Pmag_last){count_HJB = 0;} 00698 Pmag_last = Pmag; 00699 Init_pos = 0; 00700 p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count_HJB/(Tperiod*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000)); 00701 v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count_HJB/(Tperiod*1000))/Tperiod; 00702 l1_control.a.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16); 00703 l1_control.a.v_des = v_des_HJB; 00704 if(count_HJB>=(Tperiod*1000)) { 00705 count_HJB = 0; 00706 } 00707 count_HJB++; 00708 //========================================HJB end=========================================// 00709 00710 00711 //========================================shaorui added for trajectory input(leg2)=========================================// 00712 Pmag1 = uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16); 00713 Tperiod1 = uint_to_float(spi_command.qd_des_abad[1], V_MIN, V_MAX, 12);//; 00714 if (Pmag1 != Pmag_last1){count_shaorui = 0;} 00715 Pmag_last1 = Pmag1; 00716 Init_pos1 = 0; 00717 p_des_shaorui = Init_pos1 + Pmag1*FastSin(2*PI*count_shaorui/(Tperiod1*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000)); 00718 v_des_shaorui = 2*PI*Pmag1*FastCos(2*PI*count_shaorui/(Tperiod1*1000))/Tperiod1; 00719 l1_control.a.p_des = p_des_shaorui;//uint_to_float(p_des_HJB, -15.f, 15.f, 16); 00720 l1_control.a.v_des = v_des_shaorui; 00721 if(count_shaorui>=(Tperiod*1000)) { 00722 count_shaorui= 0; 00723 } 00724 count_shaorui++; 00725 //========================================shaorui end=========================================// 00726 00727 00728 00729 00730 //l1_control.a.p_des = spi_command.q_des_abad[0]; 00731 //l1_control.a.v_des = spi_command.qd_des_abad[0]; 00732 l1_control.a.kp = spi_command.kp_abad[0]; 00733 l1_control.a.kd = spi_command.kd_abad[0]; 00734 l1_control.a.t_ff = spi_command.tau_abad_ff[0]; 00735 00736 //l1_control.h.p_des = spi_command.q_des_hip[0]; 00737 //l1_control.h.v_des = spi_command.qd_des_hip[0]; 00738 l1_control.h.kp = spi_command.kp_hip[0]; 00739 l1_control.h.kd = spi_command.kd_hip[0]; 00740 l1_control.h.t_ff = spi_command.tau_hip_ff[0]; 00741 00742 //l1_control.k.p_des = spi_command.q_des_knee[0]; 00743 //l1_control.k.v_des = spi_command.qd_des_knee[0]; 00744 l1_control.k.kp = spi_command.kp_knee[0]; 00745 l1_control.k.kd = spi_command.kd_knee[0]; 00746 l1_control.k.t_ff = spi_command.tau_knee_ff[0]; 00747 00748 //l2_control.a.p_des = spi_command.q_des_abad[1]; 00749 //l2_control.a.v_des = spi_command.qd_des_abad[1]; 00750 l2_control.a.kp = spi_command.kp_abad[1]; 00751 l2_control.a.kd = spi_command.kd_abad[1]; 00752 l2_control.a.t_ff = spi_command.tau_abad_ff[1]; 00753 00754 //l2_control.h.p_des = spi_command.q_des_hip[1]; 00755 //l2_control.h.v_des = spi_command.qd_des_hip[1]; 00756 l2_control.h.kp = spi_command.kp_hip[1]; 00757 l2_control.h.kd = spi_command.kd_hip[1]; 00758 l2_control.h.t_ff = spi_command.tau_hip_ff[1]; 00759 00760 l2_control.k.p_des = spi_command.q_des_knee[1]; 00761 l2_control.k.v_des = spi_command.qd_des_knee[1]; 00762 l2_control.k.kp = spi_command.kp_knee[1]; 00763 l2_control.k.kd = spi_command.kd_knee[1]; 00764 l2_control.k.t_ff = spi_command.tau_knee_ff[1]; 00765 00766 ///printf("ap=%d akp=%d at=%d \n\r",(uint16_t)l1_control.a.p_des,(uint16_t)l1_control.a.kp,(uint16_t)l1_control.a.t_ff); 00767 00768 spi_data.flags[0] = 0; 00769 spi_data.flags[1] = 0; 00770 //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); //hjb cancelled 00771 //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled 00772 ////spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2; 00773 //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); //hjb cancelled 00774 //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled 00775 ////spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2; 00776 00777 //spi_data.flags[0] = 0xbeef; 00778 //spi_data.flags[1] = 0xbeef; 00779 PackAll(); 00780 //WriteAll(); 00781 } 00782 00783 00784 } 00785 00786 //spi_data.checksum = xor_checksum((uint16_t*)&spi_data,14); 00787 // for(int i = 0; i < DATA_LEN; i++){ 00788 // tx_buff[i] = ((uint16_t*)(&spi_data))[i];} 00789 00790 } 00791 00792 00793 void test_control() 00794 { 00795 00796 for(int i = 0; i < 2; i++) 00797 { 00798 spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f; 00799 spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f; 00800 spi_data.q_hip[i] = spi_command.q_des_hip[i] + 1.f; 00801 00802 spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f; 00803 spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f; 00804 spi_data.qd_hip[i] = spi_command.qd_des_hip[i] + 1.f; 00805 //printf("%d %d %f %.3f %.3f %.3f\n\r",(uint16_t)(spi_command.q_des_abad[i]),(uint16_t)spi_command.q_des_knee[i],spi_command.q_des_hip[i],spi_data.qd_abad[i],spi_data.qd_knee[i],spi_data.qd_hip[i]); 00806 } 00807 00808 //spi_data.flags[0] = 0xdead; 00809 //spi_data.flags[1] = 0xbeef; 00810 00811 spi_data.flags[0] = 2; 00812 spi_data.flags[1] = 2; 00813 00814 // only do first 56 bytes of message. 00815 //spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14); 00816 00817 tx_buff[0] = (uint16_t)(spi_data.q_abad[0]); 00818 tx_buff[1] = (uint16_t)(spi_data.q_abad[1]); 00819 tx_buff[2] = (uint16_t)(spi_data.q_hip[0]); 00820 tx_buff[3] = (uint16_t)(spi_data.q_hip[1]); 00821 tx_buff[4] = (uint16_t)(spi_data.q_knee[0]); 00822 tx_buff[5] = (uint16_t)(spi_data.q_knee[1]); 00823 00824 tx_buff[6] = (uint16_t)(spi_data.qd_abad[0]); 00825 tx_buff[7] = (uint16_t)(spi_data.qd_abad[1]); 00826 tx_buff[8] = (uint16_t)(spi_data.qd_hip[0]); 00827 tx_buff[9] = (uint16_t)(spi_data.qd_hip[1]); 00828 tx_buff[10] = (uint16_t)(spi_data.qd_knee[0]); 00829 tx_buff[11] = (uint16_t)(spi_data.qd_knee[1]); 00830 00831 tx_buff[12] = (uint16_t)(spi_data.flags[0]); 00832 tx_buff[13] = (uint16_t)(spi_data.flags[1]); 00833 spi_data.checksum = xor_checksum(tx_buff,14); 00834 00835 tx_buff[14] = (spi_data.checksum)&0xFFFF; 00836 00837 00838 for(int i = 0; i < DATA_LEN; i++) 00839 { // tx_buff[i] = ((uint16_t*)(&spi_data))[i]; 00840 printf("%d ", tx_buff[i]); 00841 } 00842 int testchecksum = xor_checksum((uint16_t*)&spi_data,14); 00843 00844 printf("%d %d\n\r",testchecksum,spi_data.checksum); 00845 00846 } 00847 00848 void init_spi(void){ 00849 SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4); 00850 spi->format(16, 0); 00851 spi->frequency(12000000); 00852 spi->reply(0x0); 00853 cs.fall(&spi_isr); 00854 printf("done\n\r"); 00855 } 00856 00857 00858 int main() { 00859 //wait(.5); 00860 //led = 1; 00861 //**// pc.baud(921600); 00862 pc.baud(460800); //115200 00863 pc.attach(&serial_isr); 00864 estop.mode(PullUp); 00865 //spi.format(16, 0); 00866 //spi.frequency(1000000); 00867 //spi.reply(0x0); 00868 //cs.fall(&spi_isr); 00869 00870 can1.frequency(1000000); // set bit rate to 1Mbps 00871 can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler 00872 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter 00873 can2.frequency(1000000); // set bit rate to 1Mbps 00874 can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler 00875 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter 00876 memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t)); 00877 memset(&spi_data, 0, sizeof(spi_data_t)); 00878 memset(&spi_command,0,sizeof(spi_command_t)); 00879 00880 00881 NVIC_SetPriority(TIM5_IRQn, 1); 00882 NVIC_SetPriority(CAN1_RX0_IRQn, 4); 00883 NVIC_SetPriority(CAN2_RX0_IRQn, 3); 00884 /* 00885 //=============================hjb===============================================================// 00886 //ISR Setup 00887 #define CAN_ARR 0x56 /// timer autoreload value 0x8CA 00888 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ 00889 00890 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt 00891 TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up 00892 TIM1->CR1 |= TIM_CR1_UDIS; 00893 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on, 00894 TIM1->RCR |= 0x001; // update event once per up/down count of tim1 00895 TIM1->EGR |= TIM_EGR_UG; 00896 00897 //PWM Setup 00898 00899 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock 00900 TIM1->ARR = CAN_ARR; // set auto reload, 40 khz 00901 TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. 00902 TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 00903 00904 //++++++++ FOURARM ++++++++++++++++++++++// 00905 //---------------------------------------------------------- 00906 00907 int i = 0; 00908 for (i=0; i<5; i++) 00909 { 00910 for (k=0; k<3; k++) trajRef[i][k] = 0; 00911 for (k=0; k<3; k++) trajRef2[i][k] = 0; 00912 } 00913 //---------------------------------------------------------- 00914 //---------------------------------------------------------- 00915 for (i=0; i<5; i++) 00916 { 00917 for (k=0; k<3; k++) 00918 { 00919 ref[i].T1[k] = 0; // ?????????? time t // 00920 ref[i].t1[k] = 0; // ?????????? time t // 00921 ref[i].t[k] = 0; // current time t // 00922 ref[i].M[k] = 0; // Desire Position C // 00923 ref[i].N[k] = 0; // Desire Position C // 00924 ref[i].L[k] = 0; // Last Position B // 00925 ref[i].det_M[k] = 0; // C - B // 00926 ref[i].det_L[k] = 0; // A - B // 00927 } 00928 } 00929 00930 //---------------------------------------------------------- 00931 */ 00932 int i, k; 00933 00934 //++++++++++++++++++ FOURARM ++++++++++++++++++++// 00935 for (i=0; i<5; i++) 00936 { 00937 g_ArmTrajRef[i] = 0.0f; 00938 g_ArmJointAngleRe[i] = 0.0f; 00939 g_ArmJointAngleDe[i] = 0.0f; 00940 } 00941 00942 for (k=0; k<5; k++) 00943 { 00944 g_ArmRef.T1[k] = 0.0f; // ?????????? time t // 00945 g_ArmRef.t1[k] = 0.0f; // ?????????? time t // 00946 g_ArmRef.t[k] = 0.0f; // current time t // 00947 g_ArmRef.M[k] = 0.0f; // Desire Position C // 00948 g_ArmRef.N[k] = 0.0f; // Desire Position C // 00949 g_ArmRef.L[k] = 0.0f; // Last Position B // 00950 g_ArmRef.det_M[k] = 0.0f; // C - B // 00951 g_ArmRef.det_L[k] = 0.0f; // A - B // 00952 } 00953 00954 00955 //++++++++++++++++++ FOURARM ++++++++++++++++++++// 00956 //=============================hjb===============================================================// 00957 00958 00959 printf("\n\r SPIne\n\r"); 00960 //printf("%d\n\r", RX_ID << 18); 00961 00962 a1_can.len = 8; //transmit 8 bytes 00963 a2_can.len = 8; //transmit 8 bytes 00964 h1_can.len = 8; 00965 h2_can.len = 8; 00966 k1_can.len = 8; 00967 k2_can.len = 8; 00968 rxMsg1.len = 6; //receive 6 bytes 00969 rxMsg2.len = 6; //receive 6 bytes 00970 00971 a1_can.id = 0x1; 00972 a2_can.id = 0x1; 00973 h1_can.id = 0x2; 00974 h2_can.id = 0x2; 00975 k1_can.id = 0x3; 00976 k2_can.id = 0x3; 00977 00978 pack_cmd(&a1_can, l1_control.a); 00979 pack_cmd(&a2_can, l2_control.a); 00980 pack_cmd(&h1_can, l1_control.h); 00981 pack_cmd(&h2_can, l2_control.h); 00982 pack_cmd(&k1_can, l1_control.k); 00983 pack_cmd(&k2_can, l2_control.k); 00984 WriteAll(); 00985 00986 00987 // SPI doesn't work if enabled while the CS pin is pulled low 00988 // Wait for CS to not be low, then enable SPI 00989 if(!spi_enabled){ 00990 while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);} 00991 init_spi(); 00992 spi_enabled = 1; 00993 } 00994 loop.attach(&sendCMD, .001); //========hjb added========// 00995 /*if(spi_enabled){ 00996 loop.attach(&sendCMD, .001); //============hjb added===========// 00997 } 00998 */ 00999 while(1) { 01000 //counter++; 01001 //can2.read(rxMsg2); 01002 //unpack_reply(rxMsg2, &l2_state); 01003 //can1.read(rxMsg1); // read message into Rx message storage 01004 //unpack_reply(rxMsg1, &l1_state); 01005 01006 if(Control_Flag){ 01007 control(); 01008 counter ++; 01009 01010 01011 if(counter>100){ 01012 01013 //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", l1_control.a.p_des*Deg_rad, l1_state.a.p*Deg_rad, l1_control.h.p_des*Deg_rad, l1_state.h.p*Deg_rad, l1_control.a.v_des*Deg_rad, l1_state.a.v*Deg_rad, l1_control.h.v_des*Deg_rad, l1_state.h.v*Deg_rad); 01014 // printf("%.3f %.3f %.3f %.3f %d\n\r", p_des_HJB*Deg_rad, l1_state.a.p*Deg_rad, v_des_HJB*Deg_rad, l1_state.a.v*Deg_rad, count_HJB);//shaorui delete 01015 01016 //printf("%.3f %.3f\n\r", l1_control.h.p_des, g_ArmJointAngleDe[1]); 01017 counter = 0 ; 01018 } 01019 Control_Flag =0; 01020 } 01021 //wait_us(10); 01022 01023 } 01024 01025 01026 } 01027 01028 01029 01030 01031
Generated on Fri Jul 15 2022 03:16:42 by
1.7.2