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