Updated for checksum
main.cpp
- Committer:
- adimmit
- Date:
- 2021-04-01
- Revision:
- 4:e8c1b3f8fc6c
- Parent:
- 3:d5d603c7e5b6
- Child:
- 5:f1703165ca7e
File content as of revision 4:e8c1b3f8fc6c:
//counter for misc purposes int counter3 = 0; //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(&q22_can, q2_control.h); pack_cmd(&q32_can, q2_control.k); pack_cmd(&q13_can, q3_control.a); pack_cmd(&q23_can, q3_control.h); pack_cmd(&q33_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){ pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", g1_state.a.p, g1_state.h.p, g1_state.k.p, g2_state.a.p, g2_state.h.p, g2_state.k.p, g3_state.a.p, g3_state.h.p, g3_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(); pc.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'): pc.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'): pc.printf("\n\r standing \n\r"); counter2 = 0; is_standing = 1; //stand(); break; case('z'): pc.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 print_SPI_command() { pc.printf("SPI MESSAGE RECIEVED:\n"); pc.printf("MOTOR 1 Q: %f\n", spi_command.q_des_1s[0]); pc.printf("MOTOR 1 Qd: %f\n", spi_command.qd_des_1s[0]); pc.printf("MOTOR 1 Kp: %f\n", spi_command.kp_1s[0]); pc.printf("MOTOR 1 Kd: %f\n", spi_command.kd_1s[0]); pc.printf("MOTOR 1 T_FF: %f\n", spi_command.tau_1s_ff[0]); pc.printf("MOTOR 2 Q: %f\n", spi_command.q_des_2s[0]); pc.printf("MOTOR 2 Qd: %f\n", spi_command.qd_des_2s[0]); pc.printf("MOTOR 2 Kp: %f\n", spi_command.kp_2s[0]); pc.printf("MOTOR 2 Kd: %f\n", spi_command.kd_2s[0]); pc.printf("MOTOR 2 T_FF: %f\n", spi_command.tau_2s_ff[0]); pc.printf("MOTOR 3 Q: %f\n", spi_command.q_des_3s[0]); pc.printf("MOTOR 3 Qd: %f\n", spi_command.qd_des_3s[0]); pc.printf("MOTOR 3 Kp: %f\n", spi_command.kp_3s[0]); pc.printf("MOTOR 3 Kd: %f\n", spi_command.kd_3s[0]); pc.printf("MOTOR 3 T_FF: %f\n", spi_command.tau_3s_ff[0]); pc.printf("MOTOR 4 Q: %f\n", spi_command.q_des_1s[1]); pc.printf("MOTOR 4 Qd: %f\n", spi_command.qd_des_1s[1]); pc.printf("MOTOR 4 Kp: %f\n", spi_command.kp_1s[1]); pc.printf("MOTOR 4 Kd: %f\n", spi_command.kd_1s[1]); pc.printf("MOTOR 4 T_FF: %f\n", spi_command.tau_1s_ff[1]); pc.printf("MOTOR 5 Q: %f\n", spi_command.q_des_2s[1]); pc.printf("MOTOR 5 Qd: %f\n", spi_command.qd_des_2s[1]); pc.printf("MOTOR 5 Kp: %f\n", spi_command.kp_2s[1]); pc.printf("MOTOR 5 Kd: %f\n", spi_command.kd_2s[1]); pc.printf("MOTOR 5 T_FF: %f\n", spi_command.tau_2s_ff[1]); pc.printf("MOTOR 6 Q: %f\n", spi_command.q_des_3s[1]); pc.printf("MOTOR 6 Qd: %f\n", spi_command.qd_des_3s[1]); pc.printf("MOTOR 6 Kp: %f\n", spi_command.kp_3s[1]); pc.printf("MOTOR 6 Kd: %f\n", spi_command.kd_3s[1]); pc.printf("MOTOR 6 T_FF: %f\n", spi_command.tau_3s_ff[1]); pc.printf("MOTOR 7 Q: %f\n", spi_command.q_des_1s[2]); pc.printf("MOTOR 7 Qd: %f\n", spi_command.qd_des_1s[2]); pc.printf("MOTOR 7 Kp: %f\n", spi_command.kp_1s[2]); pc.printf("MOTOR 7 Kd: %f\n", spi_command.kd_1s[2]); pc.printf("MOTOR 7 T_FF: %f\n", spi_command.tau_1s_ff[2]); pc.printf("MOTOR 8 Q: %f\n", spi_command.q_des_2s[2]); pc.printf("MOTOR 8 Qd: %f\n", spi_command.qd_des_2s[2]); pc.printf("MOTOR 8 Kp: %f\n", spi_command.kp_2s[2]); pc.printf("MOTOR 8 Kd: %f\n", spi_command.kd_2s[2]); pc.printf("MOTOR 8 T_FF: %f\n", spi_command.tau_2s_ff[2]); pc.printf("MOTOR 9 Q: %f\n", spi_command.q_des_3s[2]); pc.printf("MOTOR 9 Qd: %f\n", spi_command.qd_des_3s[2]); pc.printf("MOTOR 9 Kp: %f\n", spi_command.kp_3s[2]); pc.printf("MOTOR 9 Kd: %f\n", spi_command.kd_3s[2]); pc.printf("MOTOR 9 T_FF: %f\n", spi_command.tau_3s_ff[2]); } void spi_isr(void) { //pc.printf("CS ACTIVE\n"); GPIOC->ODR |= (1 << 8); GPIOC->ODR &= ~(1 << 8); int bytecount = 0; SPI1->DR = tx_buff[0]; while(cs == 0) { if(SPI1->SR&0x1) { rx_buff[bytecount] = SPI1->DR; bytecount++; if(bytecount<TX_LEN) { SPI1->DR = tx_buff[bytecount]; } } } //pc.printf("RECIEVED: %d BYTES\n", bytecount); //pc.printf("HERE'S A SPI COMMAND:\n"); // 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]; //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]); } //print_SPI_command(); // run control, which fills in tx_buff for the next iteration /* if(calc_checksum != spi_command.checksum){ spi_data.flags[1] = 0xdead; pc.printf("FAILED CHECKSUM\n"); pc.printf("ACTUAL: %d\n", calc_checksum); pc.printf("CURRENT: %d\n", spi_command.checksum);} */ //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(&q32_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); pc.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(&q32_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); pc.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); pc.printf("done\n\r"); } int main() { wait(1); //led = 1; pc.baud(115200); //MAYBE CHANGE THIS IF NEEDED 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(); //just debugging things pc.printf("SETUP VARS ALL DONE\n"); // 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)){pc.printf("waiting for CS Pin\n"); wait_us(10);} init_spi(); spi_enabled = 1; pc.printf("SPI ENABLED AND READY\n"); } //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); //print heatbeat (always will print message 0) /* if (counter3 == 100000) { //for debugging purposes pc.printf("HEARTBEAT\n"); counter3 = 0; } counter3++; */ } }