Updated for checksum
Diff: main.cpp
- Revision:
- 0:76c761d3caf1
- Child:
- 1:aa253b5f5b65
diff -r 000000000000 -r 76c761d3caf1 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Mar 20 18:38:51 2021 +0000 @@ -0,0 +1,708 @@ + + //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!! + //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING + +#include "mbed.h" +#include "math_ops.h" +#include <cstring> +#include "leg_message.h" + +// length of receive/transmit buffers +#define RX_LEN 98 //CHECK THESE BUFFER LENGHTS +#define TX_LEN 98 //CHECK THESE BUFFER LENGHTS + +// length of outgoing/incoming messages +#define DATA_LEN 44 //CHECK THESE BUFFER LENGHTS +#define CMD_LEN 98 //CHECK THESE BUFFER LENGHTS + +// Master CAN ID /// +#define CAN_ID 0x0 + + +/// Value Limits /// + #define P_MIN -12.5f + #define P_MAX 12.5f + #define V_MIN -65.0f + #define V_MAX 65.0f + #define KP_MIN 0.0f + #define KP_MAX 500.0f + #define KD_MIN 0.0f + #define KD_MAX 5.0f + #define T_MIN -18.0f + #define T_MAX 18.0f + + /// Joint Soft Stops /// + #define A_LIM_P 1.5f + #define A_LIM_N -1.5f + #define H_LIM_P 5.0f + #define H_LIM_N -5.0f + #define K_LIM_P 0.2f + #define K_LIM_N 7.7f + #define KP_SOFTSTOP 100.0f + #define KD_SOFTSTOP 0.4f; + +#define ENABLE_CMD 0xFFFF +#define DISABLE_CMD 0x1F1F + +spi_data_t spi_data; // data from spine to up +spi_command_t spi_command; // data from up to spine + +// spi buffers +uint16_t rx_buff[RX_LEN]; +uint16_t tx_buff[TX_LEN]; + +DigitalOut led(PC_5); + + +Serial pc(PA_2, PA_3); +CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name +CAN can2(PA_11, PA_12, 1000000); // CAN Rx pin name, CAN Tx pin name +CAN can3(PA_8, PA_15, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN4 on board + +CANMessage rxMsg1, rxMsg2, rxMsg3; +CANMessage txMsg1, txMsg2, txMsg3; +CANMessage q11_can, q21_can, q31_can, q12_can, q22_can, q32_can, q13_can, q23_can, q33_can; //TX Messages +int ledState; +Ticker sendCAN; +int counter = 0; +volatile bool msgAvailable = false; +Ticker loop; + +int spi_enabled = 0; +InterruptIn cs(PA_4); +DigitalIn estop(PB_15); +//SPISlave spi(PA_7, PA_6, PA_5, PA_4); + + +grouped_act_state g1_state, g2_state, g3_state; +grouped_act_control q1_control, q2_control, q3_control; + +uint16_t x = 0; +uint16_t x2 = 0; +uint16_t count = 0; +uint16_t counter2 = 0; //SEE IF WE NEED TO UPDATE THESE TO ADD COUNTER3 AND X3 + +int control_mode = 1; +int is_standing = 0; //SEE IF WE STILL NEED THE STANDING THING +int enabled = 0; + +// generates fake spi data from spi command +void test_control(); //MAY NEED TO GET RID OF THIS? +void control(); + + +/// CAN Command Packet Structure /// +/// 16 bit position command, between -4*pi and 4*pi +/// 12 bit velocity command, between -30 and + 30 rad/s +/// 12 bit kp, between 0 and 500 N-m/rad +/// 12 bit kd, between 0 and 100 N-m*s/rad +/// 12 bit feed forward torque, between -18 and 18 N-m +/// CAN Packet is 8 8-bit words +/// Formatted as follows. For each quantity, bit 0 is LSB +/// 0: [position[15-8]] +/// 1: [position[7-0]] +/// 2: [velocity[11-4]] +/// 3: [velocity[3-0], kp[11-8]] +/// 4: [kp[7-0]] +/// 5: [kd[11-4]] +/// 6: [kd[3-0], torque[11-8]] +/// 7: [torque[7-0]] + +void pack_cmd(CANMessage * msg, joint_control joint){ + + /// limit data to be within bounds /// + float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX); + float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX); + float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX); + float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX); + float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX); + /// convert floats to unsigned ints /// + uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); + uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); + uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); + uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); + uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12); + /// pack ints into the can buffer /// + msg->data[0] = p_int>>8; + msg->data[1] = p_int&0xFF; + msg->data[2] = v_int>>4; + msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); + msg->data[4] = kp_int&0xFF; + msg->data[5] = kd_int>>4; + msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8); + msg->data[7] = t_int&0xff; + } + +/// CAN Reply Packet Structure /// +/// 16 bit position, between -4*pi and 4*pi +/// 12 bit velocity, between -30 and + 30 rad/s +/// 12 bit current, between -40 and 40; +/// CAN Packet is 5 8-bit words +/// Formatted as follows. For each quantity, bit 0 is LSB +/// 0: [position[15-8]] +/// 1: [position[7-0]] +/// 2: [velocity[11-4]] +/// 3: [velocity[3-0], current[11-8]] +/// 4: [current[7-0]] + +void unpack_reply(CANMessage msg, grouped_act_state * group){ + /// unpack ints from can buffer /// + uint16_t id = msg.data[0]; + uint16_t p_int = (msg.data[1]<<8)|msg.data[2]; + uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4); + uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; + /// convert uints to floats /// + float p = uint_to_float(p_int, P_MIN, P_MAX, 16); + float v = uint_to_float(v_int, V_MIN, V_MAX, 12); + float t = uint_to_float(i_int, -T_MAX, T_MAX, 12); + + if(id==1){ + group->a.p = p; + group->a.v = v; + group->a.t = t; + } + else if(id==2){ + group->h.p = p; + group->h.v = v; + group->h.t = t; + } + else if(id==3){ + group->k.p = p; + group->k.v = v; + group->k.t = t; + } + } + + void rxISR1() { + can1.read(rxMsg1); // read message into Rx message storage + unpack_reply(rxMsg1, &g1_state); +} +void rxISR2(){ + can2.read(rxMsg2); + unpack_reply(rxMsg2, &g2_state); + } +void rxISR3(){ + can3.read(rxMsg3); + unpack_reply(rxMsg3, &g3_state); + } + +void PackAll(){ + pack_cmd(&q11_can, q1_control.a); + pack_cmd(&q21_can, q1_control.h); + pack_cmd(&q31_can, q1_control.k); + pack_cmd(&q12_can, q2_control.a); + pack_cmd(&k22_can, q2_control.h); + pack_cmd(&k32_can, q2_control.k); + pack_cmd(&q13_can, q3_control.a); + pack_cmd(&k23_can, q3_control.h); + pack_cmd(&k33_can, q3_control.k); + + } +void WriteAll(){ + //toggle = 1; + can1.write(q11_can); + wait(.00002); + can2.write(q12_can); + wait(.00002); + can3.write(q13_can); + wait(.00002); + can1.write(q21_can); + wait(.00002); + can2.write(q22_can); + wait(.00002); + can3.write(q23_can); + wait(.00002); + can1.write(q31_can); + wait(.00002); + can2.write(q32_can); + wait(.00002); + can3.write(q33_can); + wait(.00002); + //toggle = 0; + } + +void sendCMD(){ + counter ++; + + PackAll(); + + if(counter>100){ + printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", q1_state.a.p, q1_state.h.p, q1_state.k.p, q2_state.a.p, q2_state.h.p, q_state.k.p, q3_state.a.p, q3_state.h.p, q3_state.k.p); + counter = 0 ; + } + + WriteAll(); + } + + + + +void Zero(CANMessage * msg){ + msg->data[0] = 0xFF; + msg->data[1] = 0xFF; + msg->data[2] = 0xFF; + msg->data[3] = 0xFF; + msg->data[4] = 0xFF; + msg->data[5] = 0xFF; + msg->data[6] = 0xFF; + msg->data[7] = 0xFE; + WriteAll(); + } + +void EnterMotorMode(CANMessage * msg){ + msg->data[0] = 0xFF; + msg->data[1] = 0xFF; + msg->data[2] = 0xFF; + msg->data[3] = 0xFF; + msg->data[4] = 0xFF; + msg->data[5] = 0xFF; + msg->data[6] = 0xFF; + msg->data[7] = 0xFC; + //WriteAll(); + } + +void ExitMotorMode(CANMessage * msg){ + msg->data[0] = 0xFF; + msg->data[1] = 0xFF; + msg->data[2] = 0xFF; + msg->data[3] = 0xFF; + msg->data[4] = 0xFF; + msg->data[5] = 0xFF; + msg->data[6] = 0xFF; + msg->data[7] = 0xFD; + //WriteAll(); + } +void serial_isr(){ + /// handle keyboard commands from the serial terminal /// + while(pc.readable()){ + char c = pc.getc(); + //led = !led; + switch(c){ + case(27): + //loop.detach(); + printf("\n\r exiting motor mode \n\r"); + ExitMotorMode(&q11_can); + ExitMotorMode(&q21_can); + ExitMotorMode(&q31_can); + ExitMotorMode(&q12_can); + ExitMotorMode(&q22_can); + ExitMotorMode(&q32_can); + ExitMotorMode(&q13_can); + ExitMotorMode(&q23_can); + ExitMotorMode(&q33_can); + enabled = 0; + break; + case('m'): + printf("\n\r entering motor mode \n\r"); + EnterMotorMode(&q11_can); + EnterMotorMode(&q21_can); + EnterMotorMode(&q31_can); + EnterMotorMode(&q12_can); + EnterMotorMode(&q22_can); + EnterMotorMode(&q32_can); + EnterMotorMode(&q13_can); + EnterMotorMode(&q23_can); + EnterMotorMode(&q33_can); + wait(.5); + enabled = 1; + //loop.attach(&sendCMD, .001); + break; + case('s'): + printf("\n\r standing \n\r"); + counter2 = 0; + is_standing = 1; + //stand(); + break; + case('z'): + printf("\n\r zeroing \n\r"); + Zero(&q11_can); + Zero(&q21_can); + Zero(&q31_can); + Zero(&q12_can); + Zero(&q22_can); + Zero(&q32_can); + Zero(&q13_can); + Zero(&q23_can); + Zero(&q33_can); + break; + } + } + WriteAll(); + + } + +uint32_t xor_checksum(uint32_t* data, size_t len) +{ + uint32_t t = 0; + for(int i = 0; i < len; i++) + t = t ^ data[i]; + return t; +} + +void spi_isr(void) +{ + GPIOC->ODR |= (1 << 8); + GPIOC->ODR &= ~(1 << 8); + int bytecount = 0; + SPI1->DR = tx_buff[0]; + while(cs == 0) { + if(SPI1->SR&0x1) { + [bytecount] = SPI1->DR; + bytecount++; + if(bytecount<TX_LEN) { + SPI1->DR = tx_buff[bytecount]; + } + } + + } + + // after reading, save into spi_command + // should probably check checksum first! + uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32); + for(int i = 0; i < CMD_LEN; i++) + { + ((uint16_t*)(&spi_command))[i] = rx_buff[i]; + } + + // run control, which fills in tx_buff for the next iteration + if(calc_checksum != spi_command.checksum){ + spi_data.flags[1] = 0xdead;} + + //test_control(); + //spi_data.q_abad[0] = 12.0f; + control(); + PackAll(); + WriteAll(); + + + //for (int i = 0; i<TX_LEN; i++) { + // tx_buff[i] = 2*rx_buff[i]; + //} +// for (int i=0; i<TX_LEN; i++) { +// //printf("%d ", rx_buff[i]); +// } + //printf("\n\r"); +} + +int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ + if((state.p)>=limit_p){ + //control->p_des = limit_p; + control->v_des = 0.0f; + control->kp = 0; + control->kd = KD_SOFTSTOP; + control->t_ff += KP_SOFTSTOP*(limit_p - state.p); + return 1; + } + else if((state.p)<=limit_n){ + //control->p_des = limit_n; + control->v_des = 0.0f; + control->kp = 0; + control->kd = KD_SOFTSTOP; + control->t_ff += KP_SOFTSTOP*(limit_n - state.p); + return 1; + } + return 0; + + } + + +void control() +{ + + if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){ + enabled = 1; + //BUS ONE + EnterMotorMode(&q11_can); + can1.write(q11_can); + EnterMotorMode(&q21_can); + can1.write(q21_can); + EnterMotorMode(&q31_can); + can1.write(q31_can); + //BUS TWO + EnterMotorMode(&q12_can); + can2.write(q12_can); + EnterMotorMode(&q22_can); + can2.write(q22_can); + EnterMotorMode(&q31_can); + can2.write(q32_can); + //BUS THREE + EnterMotorMode(&q13_can); + can3.write(q13_can); + EnterMotorMode(&q23_can); + can3.write(q23_can); + EnterMotorMode(&q33_can); + can3.write(q33_can); + printf("e\n\r"); + return; + } + else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ + enabled = 0; + //BUS ONE + ExitMotorMode(&q11_can); + can1.write(q11_can); + ExitMotorMode(&q21_can); + can1.write(q21_can); + ExitMotorMode(&q31_can); + can1.write(q31_can); + //BUS TWO + ExitMotorMode(&q12_can); + can2.write(q12_can); + ExitMotorMode(&q22_can); + can2.write(q22_can); + ExitMotorMode(&q31_can); + can2.write(q32_can); + //BUS THREE + ExitMotorMode(&q13_can); + can3.write(q13_can); + ExitMotorMode(&q23_can); + can3.write(q23_can); + ExitMotorMode(&q33_can); + can3.write(q33_can); + printf("x\n\r"); + return; + } + + spi_data.q_1s[0] = g1_state.a.p; + spi_data.q_2s[0] = g1_state.h.p; + spi_data.q_3s[0] = g1_state.k.p; + spi_data.qd_1s[0] = g1_state.a.v; + spi_data.qd_2s[0] = g1_state.h.v; + spi_data.qd_3s[0] = g1_state.k.v; + + spi_data.q_1s[1] = g1_state.a.p; + spi_data.q_2s[1] = g1_state.h.p; + spi_data.q_3s[1] = g1_state.k.p; + spi_data.qd_1s[1] = g1_state.a.v; + spi_data.qd_2s[1] = g1_state.h.v; + spi_data.qd_3s[1] = g1_state.k.v; + + spi_data.q_1s[2] = g1_state.a.p; + spi_data.q_2s[2] = g1_state.h.p; + spi_data.q_3s[2] = g1_state.k.p; + spi_data.qd_1s[2] = g1_state.a.v; + spi_data.qd_2s[2] = g1_state.h.v; + spi_data.qd_3s[2] = g1_state.k.v; + + + + if(estop==0){ + //printf("estopped!!!!\n\r"); + memset(&q1_control, 0, sizeof(q1_control)); + memset(&q2_control, 0, sizeof(q2_control)); + memset(&q3_control, 0, sizeof(q3_control)); + spi_data.flags[0] = 0xdead; + spi_data.flags[1] = 0xdead; + spi_data.flags[2] = 0xdead; + led = 1; + } + + else{ + led = 0; + + memset(&q1_control, 0, sizeof(q1_control)); + memset(&q2_control, 0, sizeof(q2_control)); + memset(&q3_control, 0, sizeof(q3_control)); + + + q1_control.a.p_des = spi_command.q_des_1s[0]; + q1_control.a.v_des = spi_command.qd_des_1s[0]; + q1_control.a.kp = spi_command.kp_1s[0]; + q1_control.a.kd = spi_command.kd_1s[0]; + q1_control.a.t_ff = spi_command.tau_1s_ff[0]; + + q1_control.h.p_des = spi_command.q_des_2s[0]; + q1_control.h.v_des = spi_command.qd_des_2s[0]; + q1_control.h.kp = spi_command.kp_2s[0]; + q1_control.h.kd = spi_command.kd_2s[0]; + q1_control.h.t_ff = spi_command.tau_2s_ff[0]; + + q1_control.k.p_des = spi_command.q_des_3s[0]; + q1_control.k.v_des = spi_command.qd_des_3s[0]; + q1_control.k.kp = spi_command.kp_3s[0]; + q1_control.k.kd = spi_command.kd_3s[0]; + q1_control.k.t_ff = spi_command.tau_3s_ff[0]; + + q2_control.a.p_des = spi_command.q_des_1s[1]; + q2_control.a.v_des = spi_command.qd_des_1s[1]; + q2_control.a.kp = spi_command.kp_1s[1]; + q2_control.a.kd = spi_command.kd_1s[1]; + q2_control.a.t_ff = spi_command.tau_1s_ff[1]; + + q2_control.h.p_des = spi_command.q_des_2s[1]; + q2_control.h.v_des = spi_command.qd_des_2s[1]; + q2_control.h.kp = spi_command.kp_2s[1]; + q2_control.h.kd = spi_command.kd_2s[1]; + q2_control.h.t_ff = spi_command.tau_2s_ff[1]; + + q2_control.k.p_des = spi_command.q_des_3s[1]; + q2_control.k.v_des = spi_command.qd_des_3s[1]; + q2_control.k.kp = spi_command.kp_3s[1]; + q2_control.k.kd = spi_command.kd_3s[1]; + q2_control.k.t_ff = spi_command.tau_3s_ff[1]; + + q3_control.a.p_des = spi_command.q_des_1s[2]; + q3_control.a.v_des = spi_command.qd_des_1s[2]; + q3_control.a.kp = spi_command.kp_1s[2]; + q3_control.a.kd = spi_command.kd_1s[2]; + q3_control.a.t_ff = spi_command.tau_1s_ff[2]; + + q3_control.h.p_des = spi_command.q_des_2s[2]; + q3_control.h.v_des = spi_command.qd_des_2s[2]; + q3_control.h.kp = spi_command.kp_2s[2]; + q3_control.h.kd = spi_command.kd_2s[2]; + q3_control.h.t_ff = spi_command.tau_2s_ff[2]; + + q3_control.k.p_des = spi_command.q_des_3s[2]; + q3_control.k.v_des = spi_command.qd_des_3s[2]; + q3_control.k.kp = spi_command.kp_3s[2]; + q3_control.k.kd = spi_command.kd_3s[2]; + q3_control.k.t_ff = spi_command.tau_3s_ff[2]; + + + spi_data.flags[0] = 0; + spi_data.flags[1] = 0; + spi_data.flags[2] = 0; + spi_data.flags[0] |= softstop_joint(g1_state.a, &q1_control.a, A_LIM_P, A_LIM_N); + spi_data.flags[0] |= (softstop_joint(g1_state.h, &q1_control.h, H_LIM_P, H_LIM_N))<<1; + //spi_data.flags[0] |= (softstop_joint(g1_state.k, &q1_control.k, K_LIM_P, K_LIM_N))<<2; + spi_data.flags[1] |= softstop_joint(g2_state.a, &q2_control.a, A_LIM_P, A_LIM_N); + spi_data.flags[1] |= (softstop_joint(g2_state.h, &q2_control.h, H_LIM_P, H_LIM_N))<<1; + //spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2; + spi_data.flags[2] |= softstop_joint(g3_state.a, &q3_control.a, A_LIM_P, A_LIM_N); + spi_data.flags[2] |= (softstop_joint(g3_state.h, &q3_control.h, H_LIM_P, H_LIM_N))<<1; + //spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2; + + //spi_data.flags[0] = 0xbeef; + //spi_data.flags[1] = 0xbeef; + //PackAll(); + //WriteAll(); + } + spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14); + for(int i = 0; i < DATA_LEN; i++){ + tx_buff[i] = ((uint16_t*)(&spi_data))[i];} + +} + + +void test_control() +{ + for(int i = 0; i < 3; i++) + { + spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f; + spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f; + spi_data.q_3s[i] = spi_command.q_des_3s[i] + 1.f; + + spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f; + spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f; + spi_data.qd_3s[i] = spi_command.qd_des_3s[i] + 1.f; + } + + spi_data.flags[0] = 0xdead; + //spi_data.flags[1] = 0xbeef; + + // only do first 56 bytes of message. + spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14); + + for(int i = 0; i < DATA_LEN; i++) + tx_buff[i] = ((uint16_t*)(&spi_data))[i]; +} + +void init_spi(void){ + SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4); + spi->format(16, 0); + spi->frequency(12000000); + spi->reply(0x0); + cs.fall(&spi_isr); + printf("done\n\r"); +} + + +int main() { + wait(1); + //led = 1; + pc.baud(921600); + pc.attach(&serial_isr); + estop.mode(PullUp); + //spi.format(16, 0); + //spi.frequency(1000000); + //spi.reply(0x0); + //cs.fall(&spi_isr); + + //can1.frequency(1000000); // set bit rate to 1Mbps + //can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler + can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + //can2.frequency(1000000); // set bit rate to 1Mbps + //can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler + can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + + memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t)); + memset(&spi_data, 0, sizeof(spi_data_t)); + memset(&spi_command,0,sizeof(spi_command_t)); + + + NVIC_SetPriority(TIM5_IRQn, 1); + //NVIC_SetPriority(CAN1_RX0_IRQn, 3); + //NVIC_SetPriority(CAN2_RX0_IRQn, 3); + + pc.printf("\n\r SPIne\n\r"); + //printf("%d\n\r", RX_ID << 18); + + q11_can.len = 8; //transmit 8 bytes + q21_can.len = 8; //transmit 8 bytes + q31_can.len = 8; + q12_can.len = 8; //transmit 8 bytes + q22_can.len = 8; //transmit 8 bytes + q32_can.len = 8; + q13_can.len = 8; //transmit 8 bytes + q23_can.len = 8; //transmit 8 bytes + q33_can.len = 8; + rxMsg1.len = 6; //receive 6 bytes + rxMsg2.len = 6; + rxMsg3.len = 6; + + q11_can.id = 0x1; + q21_can.id = 0x2; + q31_can.id = 0x3; + q12_can.id = 0x1; + q22_can.id = 0x2; + q32_can.id = 0x3; + q13_can.id = 0x1; + q23_can.id = 0x2; + q33_can.id = 0x3; + + pack_cmd(&q11_can, q1_control.a); + pack_cmd(&q12_can, q2_control.a); + pack_cmd(&q13_can, q3_control.a); + pack_cmd(&q21_can, q1_control.h); + pack_cmd(&q22_can, q2_control.h); + pack_cmd(&q23_can, q3_control.h); + pack_cmd(&q31_can, q1_control.k); + pack_cmd(&q32_can, q2_control.k); + pack_cmd(&q33_can, q3_control.k); + WriteAll(); + + + // SPI doesn't work if enabled while the CS pin is pulled low + // Wait for CS to not be low, then enable SPI + if(!spi_enabled){ + while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);} + init_spi(); + spi_enabled = 1; + } + + //spi_command=set the thing here... + + while(1) { + pc.printf("test, of SPINE\r\n"); + counter++; + can2.read(rxMsg2); + unpack_reply(rxMsg2, &g2_state); + can1.read(rxMsg1); // read message into Rx message storage + unpack_reply(rxMsg1, &g1_state); + can3.read(rxMsg3); // read message into Rx message storage + unpack_reply(rxMsg3, &g3_state); + wait_us(10); + + } +} \ No newline at end of file