main.cpp

00001 
00002 
00003 #include "mbed.h"
00004 #include "math_ops.h"
00005 #include <cstring>
00006 #include "leg_message.h"
00007 
00008 // length of receive/transmit buffers
00009 #define RX_LEN 66
00010 #define TX_LEN 66
00011 
00012 // length of outgoing/incoming messages
00013 #define DATA_LEN 30
00014 #define CMD_LEN  66
00015 
00016 // Master CAN ID ///
00017 #define CAN_ID 0x0
00018 
00019 
00020 /// Value Limits ///
00021  #define P_MIN -12.5f
00022  #define P_MAX 12.5f
00023  #define V_MIN -65.0f
00024  #define V_MAX 65.0f
00025  #define KP_MIN 0.0f
00026  #define KP_MAX 500.0f
00027  #define KD_MIN 0.0f
00028  #define KD_MAX 5.0f
00029  #define T_MIN -18.0f
00030  #define T_MAX 18.0f
00031  
00032  /// Joint Soft Stops ///
00033  #define A_LIM_P 1.5f
00034  #define A_LIM_N -1.5f
00035  #define H_LIM_P 5.0f
00036  #define H_LIM_N -5.0f
00037  #define K_LIM_P 0.2f
00038  #define K_LIM_N 7.7f
00039  #define KP_SOFTSTOP 100.0f
00040  #define KD_SOFTSTOP 0.4f;
00041 
00042 #define ENABLE_CMD 0xFFFF
00043 #define DISABLE_CMD 0x1F1F
00044 
00045 spi_data_t spi_data; // data from spine to up
00046 spi_command_t spi_command; // data from up to spine
00047 
00048 // spi buffers
00049 uint16_t rx_buff[RX_LEN];
00050 uint16_t tx_buff[TX_LEN];
00051 
00052 DigitalOut led(PC_5);
00053 
00054 
00055 //Serial       pc(PA_2, PA_3);
00056 Serial       pc(PA_9, PA_10);
00057 CAN          can1(PB_8, PB_9, 1000000);  // CAN Rx pin name, CAN Tx pin name
00058 CAN          can2(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
00059 //CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
00060 //CAN          can2(PB_8, PB_9, 1000000);  // CAN Rx pin name, CAN Tx pin name
00061 
00062 CANMessage   rxMsg1, rxMsg2;
00063 CANMessage   txMsg1, txMsg2;
00064 CANMessage   a1_can, a2_can, h1_can, h2_can, k1_can, k2_can;    //TX Messages
00065 int                     ledState;
00066 Ticker                  sendCAN;
00067 int                     counter = 0;
00068 volatile bool           msgAvailable = false;
00069 Ticker loop;
00070 
00071 int spi_enabled = 0;
00072 InterruptIn cs(PA_4);
00073 //DigitalIn estop(PB_15);
00074 //SPISlave spi(PA_7, PA_6, PA_5, PA_4);
00075 
00076 
00077 leg_state l1_state, l2_state;;
00078 leg_control l1_control, l2_control;
00079 
00080 uint16_t x = 0;
00081 uint16_t x2 = 0;
00082 uint16_t count = 0;
00083 uint16_t counter2 = 0;
00084 
00085 int control_mode = 1;
00086 int is_standing = 0;
00087 int enabled = 0;
00088 
00089  // generates fake spi data from spi command
00090 void test_control();
00091 void control();
00092 
00093 
00094 /// CAN Command Packet Structure ///
00095 /// 16 bit position command, between -4*pi and 4*pi
00096 /// 12 bit velocity command, between -30 and + 30 rad/s
00097 /// 12 bit kp, between 0 and 500 N-m/rad
00098 /// 12 bit kd, between 0 and 100 N-m*s/rad
00099 /// 12 bit feed forward torque, between -18 and 18 N-m
00100 /// CAN Packet is 8 8-bit words
00101 /// Formatted as follows.  For each quantity, bit 0 is LSB
00102 /// 0: [position[15-8]]
00103 /// 1: [position[7-0]] 
00104 /// 2: [velocity[11-4]]
00105 /// 3: [velocity[3-0], kp[11-8]]
00106 /// 4: [kp[7-0]]
00107 /// 5: [kd[11-4]]
00108 /// 6: [kd[3-0], torque[11-8]]
00109 /// 7: [torque[7-0]]
00110 
00111 void pack_cmd(CANMessage * msg, joint_control joint){
00112      
00113      /// limit data to be within bounds ///
00114      float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
00115      float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
00116      float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
00117      float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
00118      float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
00119      /// convert floats to unsigned ints ///
00120      uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
00121      uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
00122      uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
00123      uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
00124      uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
00125      /// pack ints into the can buffer ///
00126      msg->data[0] = p_int>>8;                                       
00127      msg->data[1] = p_int&0xFF;
00128      msg->data[2] = v_int>>4;
00129      msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
00130      msg->data[4] = kp_int&0xFF;
00131      msg->data[5] = kd_int>>4;
00132      msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
00133      msg->data[7] = t_int&0xff;
00134      }
00135      
00136 /// CAN Reply Packet Structure ///
00137 /// 16 bit position, between -4*pi and 4*pi
00138 /// 12 bit velocity, between -30 and + 30 rad/s
00139 /// 12 bit current, between -40 and 40;
00140 /// CAN Packet is 5 8-bit words
00141 /// Formatted as follows.  For each quantity, bit 0 is LSB
00142 /// 0: [position[15-8]]
00143 /// 1: [position[7-0]] 
00144 /// 2: [velocity[11-4]]
00145 /// 3: [velocity[3-0], current[11-8]]
00146 /// 4: [current[7-0]]
00147 
00148 void unpack_reply(CANMessage msg, leg_state * leg){
00149     /// unpack ints from can buffer ///
00150     uint16_t id = msg.data[0];
00151     uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
00152     uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
00153     uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
00154     /// convert uints to floats ///
00155     float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
00156     float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
00157     float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
00158     
00159     if(id==1){
00160         leg->a.p = p;
00161         leg->a.v = v;
00162         leg->a.t = t;
00163         }
00164     else if(id==2){
00165         leg->h.p = p;
00166         leg->h.v = v;
00167         leg->h.t = t;
00168         }
00169     else if(id==3){
00170         leg->k.p = p;
00171         leg->k.v = v;
00172         leg->k.t = t;
00173         }
00174     } 
00175 
00176  void rxISR1() {
00177     can1.read(rxMsg1);                    // read message into Rx message storage
00178     unpack_reply(rxMsg1, &l1_state);
00179 }
00180 void rxISR2(){
00181     can2.read(rxMsg2);
00182     unpack_reply(rxMsg2, &l2_state);
00183     }
00184 void PackAll(){
00185     pack_cmd(&a1_can, l1_control.a); 
00186     pack_cmd(&a2_can, l2_control.a); 
00187     pack_cmd(&h1_can, l1_control.h); 
00188     pack_cmd(&h2_can, l2_control.h); 
00189     pack_cmd(&k1_can, l1_control.k); 
00190     pack_cmd(&k2_can, l2_control.k); 
00191     
00192     }
00193 void WriteAll(){
00194     //toggle = 1;
00195     can1.write(a1_can);
00196     wait(.00002);
00197     can2.write(a2_can);
00198     wait(.00002);
00199     can1.write(h1_can);
00200     wait(.00002);
00201     can2.write(h2_can);
00202     wait(.00002);
00203     can1.write(k1_can);
00204     wait(.00002);
00205     can2.write(k2_can);
00206     wait(.00002);
00207     //toggle = 0;
00208 
00209 
00210 //    //toggle = 1;
00211 //    can1.write(a1_can);
00212 //    wait(.00002);
00213 //    can1.write(h1_can);
00214 //    wait(.00002);
00215 //    can1.write(k1_can);
00216 //    wait(.00002);
00217 //    
00218 //    can2.write(a2_can);
00219 //    wait(.00002);    
00220 //    can2.write(h2_can);
00221 //    wait(.00002);
00222 //    can2.write(k2_can);
00223 //    wait(.00002);
00224 //    //toggle = 0;
00225 
00226     }
00227 
00228 void sendCMD(){
00229     counter ++;
00230 
00231     PackAll();
00232 
00233     if(counter>100){
00234         printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
00235         counter = 0 ;
00236         }
00237     
00238     WriteAll();
00239     
00240     }
00241 
00242 
00243 
00244     
00245 void Zero(CANMessage * msg){
00246     msg->data[0] = 0xFF;
00247     msg->data[1] = 0xFF;
00248     msg->data[2] = 0xFF;
00249     msg->data[3] = 0xFF;
00250     msg->data[4] = 0xFF;
00251     msg->data[5] = 0xFF;
00252     msg->data[6] = 0xFF;
00253     msg->data[7] = 0xFE;
00254     WriteAll();
00255     }
00256 
00257 void EnterMotorMode(CANMessage * msg){
00258     msg->data[0] = 0xFF;
00259     msg->data[1] = 0xFF;
00260     msg->data[2] = 0xFF;
00261     msg->data[3] = 0xFF;
00262     msg->data[4] = 0xFF;
00263     msg->data[5] = 0xFF;
00264     msg->data[6] = 0xFF;
00265     msg->data[7] = 0xFC;
00266     //WriteAll();
00267     }
00268     
00269 void ExitMotorMode(CANMessage * msg){
00270     msg->data[0] = 0xFF;
00271     msg->data[1] = 0xFF;
00272     msg->data[2] = 0xFF;
00273     msg->data[3] = 0xFF;
00274     msg->data[4] = 0xFF;
00275     msg->data[5] = 0xFF;
00276     msg->data[6] = 0xFF;
00277     msg->data[7] = 0xFD;
00278     //WriteAll();
00279     }
00280 void serial_isr(){
00281      /// handle keyboard commands from the serial terminal ///
00282      while(pc.readable()){
00283         char c = pc.getc();
00284         //led = !led;
00285         switch(c){
00286             case(27):
00287                 //loop.detach();
00288                 printf("\n\r exiting motor mode \n\r");
00289                 ExitMotorMode(&a1_can);
00290                 ExitMotorMode(&a2_can);
00291                 ExitMotorMode(&h1_can);
00292                 ExitMotorMode(&h2_can);
00293                 ExitMotorMode(&k1_can);
00294                 ExitMotorMode(&k2_can);
00295                 enabled = 0;
00296                 break;
00297             case('m'):
00298                 printf("\n\r entering motor mode \n\r");
00299                 EnterMotorMode(&a1_can);
00300                 EnterMotorMode(&a2_can);
00301                 EnterMotorMode(&h1_can);
00302                 EnterMotorMode(&h2_can);
00303                 EnterMotorMode(&k1_can);
00304                 EnterMotorMode(&k2_can);
00305                 wait(.5);
00306                 enabled = 1;
00307                 //loop.attach(&sendCMD, .001);
00308                 break;
00309             case('s'):
00310                 printf("\n\r standing \n\r");
00311                 counter2 = 0;
00312                 is_standing = 1;
00313                 //stand();
00314                 break;
00315             case('z'):
00316                 printf("\n\r zeroing \n\r");
00317                 Zero(&a1_can);
00318                 Zero(&a2_can);
00319                 Zero(&h1_can);
00320                 Zero(&h2_can);
00321                 Zero(&k1_can);
00322                 Zero(&k2_can);
00323                 break;
00324             }
00325         }
00326         WriteAll();
00327         
00328     }
00329     
00330 uint32_t xor_checksum(uint32_t* data, size_t len)
00331 {
00332     uint32_t t = 0;
00333     for(int i = 0; i < len; i++)   
00334         t = t ^ data[i];
00335     return t;
00336 }
00337 
00338 void spi_isr(void)
00339 {
00340     GPIOC->ODR |= (1 << 8);
00341     GPIOC->ODR &= ~(1 << 8);
00342     int bytecount = 0;
00343     SPI1->DR = tx_buff[0];
00344     while(cs == 0) {
00345         if(SPI1->SR&0x1) {
00346             rx_buff[bytecount] = SPI1->DR;
00347             bytecount++;
00348             if(bytecount<TX_LEN) {
00349                 SPI1->DR = tx_buff[bytecount];
00350             }
00351         }
00352 
00353     }
00354     
00355     // after reading, save into spi_command
00356     // should probably check checksum first!
00357     uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
00358     for(int i = 0; i < CMD_LEN; i++)
00359     {
00360         ((uint16_t*)(&spi_command))[i] = rx_buff[i];
00361     }
00362     
00363     // run control, which fills in tx_buff for the next iteration
00364     if(calc_checksum != spi_command.checksum){
00365         spi_data.flags[1] = 0xdead;}
00366         
00367     //test_control();
00368     //spi_data.q_abad[0] = 12.0f;
00369     control();
00370     PackAll();
00371     WriteAll();
00372 
00373 
00374     //for (int i = 0; i<TX_LEN; i++) {
00375      //   tx_buff[i] = 2*rx_buff[i];
00376     //}
00377 //    for (int i=0; i<TX_LEN; i++) {
00378 //        //printf("%d ", rx_buff[i]);
00379 //    }
00380     //printf("\n\r");
00381 }
00382 
00383 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
00384     if((state.p)>=limit_p){
00385         //control->p_des = limit_p;
00386         control->v_des = 0.0f;
00387         control->kp = 0;
00388         control->kd = KD_SOFTSTOP;
00389         control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
00390         return 1;
00391     }
00392     else if((state.p)<=limit_n){
00393         //control->p_des = limit_n;
00394         control->v_des = 0.0f;
00395         control->kp = 0;
00396         control->kd = KD_SOFTSTOP;
00397         control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
00398         return 1;
00399     }
00400     return 0;
00401     
00402     }
00403     
00404     
00405 void control()
00406 {
00407     
00408     if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
00409         enabled = 1;
00410         EnterMotorMode(&a1_can);
00411         can1.write(a1_can);
00412         EnterMotorMode(&a2_can);
00413         can2.write(a2_can);
00414         EnterMotorMode(&k1_can);
00415         can1.write(k1_can);
00416         EnterMotorMode(&k2_can);
00417         can2.write(k2_can);
00418         EnterMotorMode(&h1_can);
00419         can1.write(h1_can);
00420         EnterMotorMode(&h2_can);
00421         can2.write(h2_can);
00422         printf("e\n\r");
00423         return;
00424     }
00425     else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
00426          enabled = 0;
00427         ExitMotorMode(&a1_can);
00428         can1.write(a1_can);
00429         ExitMotorMode(&a2_can);
00430         can2.write(a2_can);
00431         ExitMotorMode(&h1_can);
00432         can1.write(h1_can);
00433         ExitMotorMode(&h2_can);
00434         can2.write(h2_can);
00435         ExitMotorMode(&k1_can);
00436         can1.write(k1_can);
00437         ExitMotorMode(&k2_can);
00438         can2.write(k2_can);
00439         printf("x\n\r");
00440         return;
00441         }
00442     
00443     spi_data.q_abad[0] = l1_state.a.p;
00444     spi_data.q_hip[0] = l1_state.h.p;
00445     spi_data.q_knee[0] = l1_state.k.p;
00446     spi_data.qd_abad[0] = l1_state.a.v;
00447     spi_data.qd_hip[0] = l1_state.h.v;
00448     spi_data.qd_knee[0] = l1_state.k.v;
00449     
00450     spi_data.q_abad[1] = l2_state.a.p;
00451     spi_data.q_hip[1] = l2_state.h.p;
00452     spi_data.q_knee[1] = l2_state.k.p;
00453     spi_data.qd_abad[1] = l2_state.a.v;
00454     spi_data.qd_hip[1] = l2_state.h.v;
00455     spi_data.qd_knee[1] = l2_state.k.v;
00456     
00457     
00458     
00459 //    if(estop==0){
00460 //        //printf("estopped!!!!\n\r");
00461 //        memset(&l1_control, 0, sizeof(l1_control));
00462 //        memset(&l2_control, 0, sizeof(l2_control));
00463 //        spi_data.flags[0] = 0xdead;
00464 //        spi_data.flags[1] = 0xdead;
00465 //        led = 1;
00466 //        }
00467 //    
00468 //    else{
00469         led = 0;
00470         
00471         memset(&l1_control, 0, sizeof(l1_control));
00472         memset(&l2_control, 0, sizeof(l2_control));
00473         
00474         l1_control.a.p_des = spi_command.q_des_abad[0];
00475         l1_control.a.v_des  = spi_command.qd_des_abad[0];
00476         l1_control.a.kp = spi_command.kp_abad[0];
00477         l1_control.a.kd = spi_command.kd_abad[0];
00478         l1_control.a.t_ff = spi_command.tau_abad_ff[0];
00479         
00480         l1_control.h.p_des = spi_command.q_des_hip[0];
00481         l1_control.h.v_des  = spi_command.qd_des_hip[0];
00482         l1_control.h.kp = spi_command.kp_hip[0];
00483         l1_control.h.kd = spi_command.kd_hip[0];
00484         l1_control.h.t_ff = spi_command.tau_hip_ff[0];
00485         
00486         l1_control.k.p_des = spi_command.q_des_knee[0];
00487         l1_control.k.v_des  = spi_command.qd_des_knee[0];
00488         l1_control.k.kp = spi_command.kp_knee[0];
00489         l1_control.k.kd = spi_command.kd_knee[0];
00490         l1_control.k.t_ff = spi_command.tau_knee_ff[0];
00491         
00492         l2_control.a.p_des = spi_command.q_des_abad[1];
00493         l2_control.a.v_des  = spi_command.qd_des_abad[1];
00494         l2_control.a.kp = spi_command.kp_abad[1];
00495         l2_control.a.kd = spi_command.kd_abad[1];
00496         l2_control.a.t_ff = spi_command.tau_abad_ff[1];
00497         
00498         l2_control.h.p_des = spi_command.q_des_hip[1];
00499         l2_control.h.v_des  = spi_command.qd_des_hip[1];
00500         l2_control.h.kp = spi_command.kp_hip[1];
00501         l2_control.h.kd = spi_command.kd_hip[1];
00502         l2_control.h.t_ff = spi_command.tau_hip_ff[1];
00503         
00504         l2_control.k.p_des = spi_command.q_des_knee[1];
00505         l2_control.k.v_des  = spi_command.qd_des_knee[1];
00506         l2_control.k.kp = spi_command.kp_knee[1];
00507         l2_control.k.kd = spi_command.kd_knee[1];
00508         l2_control.k.t_ff = spi_command.tau_knee_ff[1];
00509         
00510         
00511         spi_data.flags[0] = 0;
00512         spi_data.flags[1] = 0;
00513         spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
00514         spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
00515         //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
00516         spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
00517         spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
00518         //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
00519         
00520         //spi_data.flags[0] = 0xbeef;
00521         //spi_data.flags[1] = 0xbeef;
00522         //PackAll();
00523         //WriteAll();
00524 //    }
00525     spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
00526     for(int i = 0; i < DATA_LEN; i++){
00527         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
00528     
00529 }
00530     
00531 
00532 void test_control()
00533 {
00534     for(int i = 0; i < 2; i++)
00535     {
00536         spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
00537         spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
00538         spi_data.q_hip[i]  = spi_command.q_des_hip[i]  + 1.f;
00539         
00540         spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
00541         spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
00542         spi_data.qd_hip[i]  = spi_command.qd_des_hip[i]  + 1.f;
00543     }
00544     
00545     spi_data.flags[0] = 0xdead;
00546     //spi_data.flags[1] = 0xbeef;
00547     
00548     // only do first 56 bytes of message.
00549     spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
00550     
00551     for(int i = 0; i < DATA_LEN; i++)
00552         tx_buff[i] = ((uint16_t*)(&spi_data))[i];
00553 }
00554 
00555 void init_spi(void){
00556     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
00557     spi->format(16, 0);
00558     spi->frequency(12000000);
00559     spi->reply(0x0);
00560     cs.fall(&spi_isr);
00561     printf("done\n\r");
00562 }
00563 
00564     
00565 int main() {
00566     wait(1);
00567     //led = 1;
00568     pc.baud(921600);
00569     pc.attach(&serial_isr);
00570 //    estop.mode(PullUp);
00571     //spi.format(16, 0);
00572     //spi.frequency(1000000);
00573     //spi.reply(0x0);
00574     //cs.fall(&spi_isr);
00575 
00576     //can1.frequency(1000000);                     // set bit rate to 1Mbps
00577     //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
00578     can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00579     //can2.frequency(1000000);                     // set bit rate to 1Mbps
00580     //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
00581     can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00582     
00583     memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
00584     memset(&spi_data, 0, sizeof(spi_data_t));
00585     memset(&spi_command,0,sizeof(spi_command_t));
00586     
00587     
00588     NVIC_SetPriority(TIM5_IRQn, 1);
00589     //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
00590     //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
00591     
00592     printf("\n\r SPIne\n\r");
00593     //printf("%d\n\r", RX_ID << 18);
00594     
00595     a1_can.len = 8;                         //transmit 8 bytes
00596     a2_can.len = 8;                         //transmit 8 bytes
00597     h1_can.len = 8;
00598     h2_can.len = 8;
00599     k1_can.len = 8;
00600     k2_can.len = 8;
00601     rxMsg1.len = 6;                          //receive 6 bytes
00602     rxMsg2.len = 6;                          //receive 6 bytes
00603 
00604     a1_can.id = 0x1;                        
00605     a2_can.id = 0x1;                 
00606     h1_can.id = 0x2;
00607     h2_can.id = 0x2;
00608     k1_can.id = 0x3;
00609     k2_can.id = 0x3;     
00610 
00611     pack_cmd(&a1_can, l1_control.a); 
00612     pack_cmd(&a2_can, l2_control.a); 
00613     pack_cmd(&h1_can, l1_control.h); 
00614     pack_cmd(&h2_can, l2_control.h); 
00615     pack_cmd(&k1_can, l1_control.k); 
00616     pack_cmd(&k2_can, l2_control.k); 
00617     WriteAll();
00618 
00619 
00620     // SPI doesn't work if enabled while the CS pin is pulled low
00621     // Wait for CS to not be low, then enable SPI
00622     if(!spi_enabled){   
00623         while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
00624         init_spi();
00625         spi_enabled = 1;
00626         }
00627             
00628     while(1) {
00629         counter++;
00630         can2.read(rxMsg2);
00631         unpack_reply(rxMsg2, &l2_state);
00632         can1.read(rxMsg1);                    // read message into Rx message storage
00633         unpack_reply(rxMsg1, &l1_state);
00634         wait_us(10);
00635 
00636 //        counter++;
00637 //        can1.read(rxMsg1);
00638 //        unpack_reply(rxMsg1, &l1_state);
00639 //        can2.read(rxMsg2);                    // read message into Rx message storage
00640 //        unpack_reply(rxMsg2, &l2_state);
00641 //        wait_us(10);
00642 
00643         }
00644         
00645 
00646         
00647         
00648     }
00649     
00650 
00651 
00652 
00653