main.cpp

00001 
00002 //counter for misc purposes
00003 int counter3 = 0;
00004                                                                                     //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
00005                                                                                     //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING
00006 
00007 #include "mbed.h"
00008 #include "math_ops.h"
00009 #include <cstring>
00010 #include "leg_message.h"
00011 
00012 // length of receive/transmit buffers
00013 #define RX_LEN 52                                                                   //CHECK THESE BUFFER LENGHTS
00014 #define TX_LEN 52                                                                   //CHECK THESE BUFFER LENGHTS
00015 
00016 // length of outgoing/incoming messages
00017 #define DATA_LEN 36                                                                 //CHECK THESE BUFFER LENGHTS
00018 #define CMD_LEN  52                                                                 //CHECK THESE BUFFER LENGHTS
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 -65.0f
00028  #define V_MAX 65.0f
00029  #define KP_MIN 0.0f
00030  #define KP_MAX 500.0f
00031  #define KD_MIN 0.0f
00032  #define KD_MAX 10.0f
00033  #define T_MIN -72.0f
00034  #define T_MAX 72.0f
00035  
00036  /// Joint Soft Stops ///
00037  #define A1_LIM_P 1.5f
00038  #define A1_LIM_N -1.5f
00039  #define A2_LIM_P 1.5f
00040  #define A2_LIM_N -1.5f
00041  #define A3_LIM_P 1.5f
00042  #define A3_LIM_N -1.5f
00043  #define KP_SOFTSTOP 100.0f
00044  #define KD_SOFTSTOP 0.4f;
00045 
00046 #define ENABLE_CMD 0xFFFF
00047 #define DISABLE_CMD 0x1F1F
00048 
00049 spi_data_t spi_data; // data from spine to up
00050 spi_command_t spi_command; // data from up to spine
00051 spi_command_t tmp_crc_chk;
00052 
00053 // spi buffers
00054 uint16_t rx_buff[RX_LEN];
00055 uint16_t tx_buff[TX_LEN];
00056 
00057 DigitalOut led(PC_5);
00058 
00059 
00060 Serial       pc(PA_2, PA_3);
00061 CAN          can1(PA_11, PA_12, 1000000);
00062 CAN          can2(PA_8, PA_15, 1000000);
00063 CAN          can3(PB_12, PB_13, 1000000); //corresponds to bus 1-3-6 (controller 1) or 2-4-5 (controller 2) IN THAT ORDER
00064 
00065 CANMessage   rxMsg1, rxMsg2, rxMsg3;
00066 CANMessage   txMsg1, txMsg2, txMsg3;
00067 CANMessage   q11_can, q21_can; //q12_can, q13_can, q22_can, q23_can, q31_can, q32_can, q33_can;    //TX Messages
00068 int                     ledState;
00069 Ticker                  sendCAN;
00070 int                     counter = 0;
00071 volatile bool           msgAvailable = false;
00072 Ticker loop;
00073 
00074 int spi_enabled = 0;
00075 InterruptIn cs(PA_4);
00076 DigitalIn estop(PB_15);
00077 //SPISlave spi(PA_7, PA_6, PA_5, PA_4);
00078 
00079 
00080 grouped_act_state g1_state, g2_state; // g3_state;
00081 grouped_act_control g1_control, g2_control; // g3_control;
00082 
00083 uint16_t x = 0;
00084 uint16_t x2 = 0;
00085 uint16_t count = 0;
00086 uint16_t counter2 = 0;                                                              //SEE IF WE NEED TO UPDATE THESE TO ADD COUNTER3 AND X3
00087 
00088 int control_mode = 1;
00089 int is_standing = 0;                                                                //SEE IF WE STILL NEED THE STANDING THING
00090 int enabled = 0;
00091 
00092 // generates fake spi data from spi command
00093 void test_control();                                                                //MAY NEED TO GET RID OF THIS?
00094 void control();
00095 
00096 
00097 /// CAN Command Packet Structure ///
00098 /// 16 bit position command, between -4*pi and 4*pi
00099 /// 12 bit velocity command, between -30 and + 30 rad/s
00100 /// 12 bit kp, between 0 and 500 N-m/rad
00101 /// 12 bit kd, between 0 and 100 N-m*s/rad
00102 /// 12 bit feed forward torque, between -18 and 18 N-m
00103 /// CAN Packet is 8 8-bit words
00104 /// Formatted as follows.  For each quantity, bit 0 is LSB
00105 /// 0: [position[15-8]]
00106 /// 1: [position[7-0]] 
00107 /// 2: [velocity[11-4]]
00108 /// 3: [velocity[3-0], kp[11-8]]
00109 /// 4: [kp[7-0]]
00110 /// 5: [kd[11-4]]
00111 /// 6: [kd[3-0], torque[11-8]]
00112 /// 7: [torque[7-0]]
00113 
00114 void pack_cmd(CANMessage * msg, joint_control joint){
00115      
00116      /// limit data to be within bounds ///
00117      float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
00118      float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
00119      float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
00120      float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
00121      float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
00122      /// convert floats to unsigned ints ///
00123      uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
00124      uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
00125      uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
00126      uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
00127      uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
00128      /// pack ints into the can buffer ///
00129      msg->data[0] = p_int>>8;                                       
00130      msg->data[1] = p_int&0xFF;
00131      msg->data[2] = v_int>>4;
00132      msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
00133      msg->data[4] = kp_int&0xFF;
00134      msg->data[5] = kd_int>>4;
00135      msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
00136      msg->data[7] = t_int&0xff;
00137      }
00138      
00139 /// CAN Reply Packet Structure ///
00140 /// 16 bit position, between -4*pi and 4*pi
00141 /// 12 bit velocity, between -30 and + 30 rad/s
00142 /// 12 bit current, between -40 and 40;
00143 /// CAN Packet is 5 8-bit words
00144 /// Formatted as follows.  For each quantity, bit 0 is LSB
00145 /// 0: [position[15-8]]
00146 /// 1: [position[7-0]] 
00147 /// 2: [velocity[11-4]]
00148 /// 3: [velocity[3-0], current[11-8]]
00149 /// 4: [current[7-0]]
00150 
00151 void unpack_reply(CANMessage msg, grouped_act_state * group){
00152     /// unpack ints from can buffer ///
00153     uint16_t id = msg.data[0];
00154     uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
00155     uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
00156     uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
00157     /// convert uints to floats ///
00158     float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
00159     float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
00160     float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
00161     
00162     if(id==1){
00163         group->a1.p = p;
00164         group->a1.v = v;
00165         group->a1.t = t;
00166         }
00167     } 
00168 
00169  void rxISR1() {
00170     can1.read(rxMsg1);                    // read message into Rx message storage
00171     unpack_reply(rxMsg1, &g1_state);
00172 }
00173 void rxISR2(){
00174     can2.read(rxMsg2);
00175     unpack_reply(rxMsg2, &g2_state);
00176     }
00177     
00178 void PackAll(){
00179     //actuators on the CAN1 bus
00180     pack_cmd(&q11_can, g1_control.a1);
00181     //actuators on the CAN2 bus
00182     pack_cmd(&q21_can, g2_control.a1);
00183     }
00184 void WriteAll(){
00185     //toggle = 1;
00186     //ID = 1 actuators
00187     can1.write(q11_can);
00188     wait(.00002);
00189     can2.write(q21_can);
00190     wait(.00002);
00191     }
00192 
00193 void sendCMD(){
00194     counter ++;
00195 
00196     PackAll();
00197 
00198     if(counter>100){
00199         pc.printf("%.3f %.3f\n\r", g1_state.a1.p, g2_state.a1.p);
00200         counter = 0 ;
00201         }
00202     
00203     WriteAll();
00204     }
00205 
00206 
00207 
00208     
00209 void Zero(CANMessage * msg){
00210     msg->data[0] = 0xFF;
00211     msg->data[1] = 0xFF;
00212     msg->data[2] = 0xFF;
00213     msg->data[3] = 0xFF;
00214     msg->data[4] = 0xFF;
00215     msg->data[5] = 0xFF;
00216     msg->data[6] = 0xFF;
00217     msg->data[7] = 0xFE;
00218     //WriteAll();
00219     }
00220 
00221 void EnterMotorMode(CANMessage * msg){
00222     msg->data[0] = 0xFF;
00223     msg->data[1] = 0xFF;
00224     msg->data[2] = 0xFF;
00225     msg->data[3] = 0xFF;
00226     msg->data[4] = 0xFF;
00227     msg->data[5] = 0xFF;
00228     msg->data[6] = 0xFF;
00229     msg->data[7] = 0xFC;
00230     //WriteAll();
00231     }
00232     
00233 void ExitMotorMode(CANMessage * msg){
00234     msg->data[0] = 0xFF;
00235     msg->data[1] = 0xFF;
00236     msg->data[2] = 0xFF;
00237     msg->data[3] = 0xFF;
00238     msg->data[4] = 0xFF;
00239     msg->data[5] = 0xFF;
00240     msg->data[6] = 0xFF;
00241     msg->data[7] = 0xFD;
00242     //WriteAll();
00243     }
00244 
00245 
00246 void serial_isr(){
00247      /// handle keyboard commands from the serial terminal ///
00248      while(pc.readable()){
00249         char c = pc.getc();
00250         //led = !led;
00251         switch(c){
00252             case(27):
00253                 //loop.detach();
00254                 pc.printf("\n\r exiting motor mode \n\r");
00255                 //CAN BUS 1
00256                 ExitMotorMode(&q11_can);
00257                 //CAN BUS 2
00258                 ExitMotorMode(&q21_can);
00259                 //DISABLE FLAG
00260                 enabled = 0;
00261                 break;
00262             case('m'):
00263                 pc.printf("\n\r entering motor mode \n\r");
00264                 //CAN BUS 1
00265                 EnterMotorMode(&q11_can);
00266                 //CAN BUS 2
00267                 EnterMotorMode(&q21_can);
00268                 //WAIT FOR ENABLE
00269                 wait(.5);
00270                 //ENABLE FLAG
00271                 enabled = 1;
00272                 //loop.attach(&sendCMD, .001);
00273                 break;
00274             case('s'):
00275                 pc.printf("\n\r standing \n\r");
00276                 counter2 = 0;
00277                 is_standing = 1;
00278                 //stand();
00279                 break;
00280             case('z'):
00281                 pc.printf("\n\r zeroing \n\r");
00282                 //CAN BUS 1
00283                 Zero(&q11_can);
00284                 //Zero(&q12_can);
00285                 //Zero(&q13_can);
00286                 //CAN BUS 2
00287                 Zero(&q21_can);
00288                 //Zero(&q22_can);
00289                 //Zero(&q23_can);
00290                 //CAN BUS 3
00291                 //Zero(&q31_can);
00292                 //Zero(&q32_can);
00293                 //Zero(&q33_can);
00294                 break;
00295             }
00296         }
00297         WriteAll();
00298         
00299     }
00300 
00301     
00302 uint32_t xor_checksum(uint32_t* data, size_t len)
00303 {
00304     uint32_t t = 0;
00305     for(int i = 0; i < len; i++)   
00306         t = t ^ data[i];
00307     return t;
00308 }
00309 
00310 
00311 
00312 void print_SPI_command() {
00313     pc.printf("SPI MESSAGE RECIEVED:\n");
00314     //CAN ID ONE
00315     pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]);
00316     pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]);
00317     pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]);
00318     pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]);
00319     pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
00320     
00321     pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]);
00322     pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]);
00323     pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]);
00324     pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]);
00325     pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]);
00326     
00327 }
00328 
00329 
00330 
00331 
00332 void print_SPI_data() {
00333     pc.printf("SPI MESSAGE SENT:\n");
00334     //CAN ID ONES
00335     pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]);
00336     pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]);
00337     
00338     pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]);
00339     pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]);
00340     
00341 }
00342 
00343 
00344 
00345 void process()
00346 {
00347     //pc.printf("%f\n", spi_command.q_des_2s[0]);
00348     // update qs
00349     spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0;
00350     spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0;
00351     // update qds
00352     spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0;
00353     spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0;
00354     // update taus
00355     spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0;
00356     spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0;
00357     // UDPATE FLAGS
00358     spi_data.flags[0] = 0;
00359     spi_data.flags[1] = 0;
00360     // UPDATE CHECKSUM
00361     spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);  //DONT CRC THE CRC!
00362     for(int i = 0; i < DATA_LEN; i++){
00363         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
00364 }
00365 
00366 
00367 
00368 void spi_isr(void)
00369 {
00370     //pc.printf("CS ACTIVE\n");
00371     GPIOC->ODR |= (1 << 8);
00372     GPIOC->ODR &= ~(1 << 8);
00373     int bytecount = 0;
00374     SPI1->DR = tx_buff[0];
00375     while(cs == 0) {
00376         if(SPI1->SR&0x1) {
00377             rx_buff[bytecount] = SPI1->DR;
00378             bytecount++;
00379             if(bytecount<TX_LEN) {
00380                 SPI1->DR = tx_buff[bytecount];
00381             }
00382         }
00383     }
00384     //pc.printf("RECIEVED: %d BYTES\n", bytecount);
00385     
00386     //update crc_chk from buffer
00387    for(int i = 0; i < RX_LEN; i++) {((uint16_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
00388    // CHECK THE CHECKSUM 
00389    uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 12); //DONT CRC THE CRC!!!
00390    // READ CHECKSUM
00391    uint32_t _rx_crc = tmp_crc_chk.checksum;
00392     
00393     if(_crc == _rx_crc) {
00394        //pc.printf("CHECKSUM PASSED...");
00395        //update crc_chk from buffer
00396        spi_command = tmp_crc_chk;
00397        //do math on the input
00398        //process();
00399        control();
00400        PackAll();
00401        WriteAll();
00402        //new_command = 1;
00403        //print_SPI_command();
00404     }
00405     else{pc.printf("bigger F in the chat...\n");}
00406 }
00407 
00408 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
00409     /*
00410     if((state.p)>=limit_p){
00411         //control->p_des = limit_p;
00412         control->v_des = 0.0f;
00413         control->kp = 0;
00414         control->kd = KD_SOFTSTOP;
00415         control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
00416         return 1;
00417     }
00418     else if((state.p)<=limit_n){
00419         //control->p_des = limit_n;
00420         control->v_des = 0.0f;
00421         control->kp = 0;
00422         control->kd = KD_SOFTSTOP;
00423         control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
00424         return 1;
00425     }
00426     */
00427     return 0;
00428     
00429     }
00430     
00431     
00432 void control()
00433 {
00434     
00435     if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
00436         enabled = 1;
00437         //BUS ONE
00438         EnterMotorMode(&q11_can);
00439         can1.write(q11_can);
00440         //BUS TWO
00441         EnterMotorMode(&q21_can);
00442         can2.write(q21_can);
00443         //pc.printf("e\n\r");
00444         return;
00445     }
00446     else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
00447         enabled = 0;
00448         //BUS ONE
00449         ExitMotorMode(&q11_can);
00450         can1.write(q11_can);
00451         //BUS TWO
00452         ExitMotorMode(&q21_can);
00453         can2.write(q21_can);
00454         return;
00455         }
00456     
00457     //BUS 1 DATA
00458     spi_data.q_1s[0] = g1_state.a1.p;
00459     spi_data.qd_1s[0] = g1_state.a1.v;
00460     spi_data.tau_1s[0] = g1_state.a1.t;
00461     //BUS 2 DATA
00462     spi_data.q_1s[1] = g2_state.a1.p;
00463     spi_data.qd_1s[1] = g2_state.a1.v;
00464     spi_data.tau_1s[1] = g2_state.a1.t;
00465     
00466     if(estop==0){
00467         printf("estopped!!!!\n\r");
00468         memset(&g1_control, 0, sizeof(g1_control));
00469         memset(&g2_control, 0, sizeof(g2_control));
00470         //memset(&g3_control, 0, sizeof(g3_control));
00471         spi_data.flags[0] = 0xdead;
00472         spi_data.flags[1] = 0xdead;
00473         //spi_data.flags[2] = 0xdead;
00474         led = 1;
00475         }
00476     
00477     else{
00478         led = 0;
00479         memset(&g1_control, 0, sizeof(g1_control));
00480         memset(&g2_control, 0, sizeof(g2_control));
00481         //memset(&g3_control, 0, sizeof(g3_control));
00482         
00483         //TRANSLATE SPI TO ACTUATOR COMMANNDS
00484         //CAN1
00485         //CAN1 MOTOR1
00486         g1_control.a1.p_des = spi_command.q_des_1s[0];
00487         g1_control.a1.v_des  = spi_command.qd_des_1s[0];
00488         g1_control.a1.kp = spi_command.kp_1s[0];
00489         g1_control.a1.kd = spi_command.kd_1s[0];
00490         g1_control.a1.t_ff = spi_command.tau_1s_ff[0];
00491         //CAN2 MOTOR1
00492         g2_control.a1.p_des = spi_command.q_des_1s[1];
00493         g2_control.a1.v_des  = spi_command.qd_des_1s[1];
00494         g2_control.a1.kp = spi_command.kp_1s[1];
00495         g2_control.a1.kd = spi_command.kd_1s[1];
00496         g2_control.a1.t_ff = spi_command.tau_1s_ff[1];
00497         //SPI FLAGS RETURN                                                                      //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE
00498         spi_data.flags[0] = 0;
00499         spi_data.flags[1] = 0;
00500     }
00501     spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);
00502     for(int i = 0; i < DATA_LEN; i++){
00503         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
00504     
00505 }
00506     
00507 
00508 void test_control()
00509 {
00510     for(int i = 0; i < 3; i++)
00511     {
00512         spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
00513         
00514         spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
00515     }
00516     
00517     spi_data.flags[0] = 0xdead;
00518     //spi_data.flags[1] = 0xbeef;
00519     
00520     // only do first 56 bytes of message.
00521     spi_data.checksum = xor_checksum((uint32_t*)&spi_data,12);
00522     
00523     for(int i = 0; i < DATA_LEN; i++)
00524         tx_buff[i] = ((uint16_t*)(&spi_data))[i];
00525 }
00526 
00527 void init_spi(void){
00528     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
00529     spi->format(16, 0);
00530     spi->frequency(500000);
00531     spi->reply(0x0);
00532     cs.fall(&spi_isr);
00533     pc.printf("done\n\r");
00534 }
00535     
00536 int main() {
00537     wait(1);
00538     //led = 1;
00539     pc.baud(115200);                                                                //MAYBE CHANGE THIS IF NEEDED
00540     pc.attach(&serial_isr);
00541     estop.mode(PullUp);
00542 
00543     can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00544     can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00545 
00546     memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
00547     memset(&spi_data, 0, sizeof(spi_data_t));
00548     memset(&spi_command,0,sizeof(spi_command_t));
00549     
00550 
00551     NVIC_SetPriority(TIM5_IRQn, 1);
00552     //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
00553     //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
00554     
00555     pc.printf("\n\r SPIne\n\r");
00556     //printf("%d\n\r", RX_ID << 18);
00557     
00558     //CAN 1 BUS
00559     q11_can.len = 8;                         //transmit 8 bytes
00560     //CAN 2 BUS
00561     q21_can.len = 8;                         //transmit 8 bytes
00562     //RECIEVE
00563     rxMsg1.len = 6;                          //receive 6 bytes
00564     rxMsg2.len = 6;
00565     
00566     //CAN 1 BUS
00567     q11_can.id = 0x1;
00568     //CAN 2 BUS
00569     q21_can.id = 0x1;
00570 
00571     //actuators on the CAN1 bus
00572     pack_cmd(&q11_can, g1_control.a1);
00573     //actuators on the CAN2 bus
00574     pack_cmd(&q21_can, g2_control.a1);
00575     //WRITE THE INITIAL COMMAND 
00576     WriteAll();
00577     
00578     //just debugging things
00579     pc.printf("SETUP VARS ALL DONE\n");
00580 
00581 
00582     // SPI doesn't work if enabled while the CS pin is pulled low
00583     // Wait for CS to not be low, then enable SPI
00584     if(!spi_enabled){ 
00585         while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
00586         init_spi();
00587         spi_enabled = 1;
00588         pc.printf("SPI ENABLED AND READY\n");
00589         }
00590         
00591     //spi_command=set the thing here... 
00592             
00593     while(1) {
00594         //pc.printf("heartbeat...\n");
00595         counter++;
00596         can2.read(rxMsg2);
00597         unpack_reply(rxMsg2, &g2_state);
00598         can1.read(rxMsg1);                    // read message into Rx message storage
00599         unpack_reply(rxMsg1, &g1_state);
00600         ///wait(0.01);
00601     }
00602 
00603 }