Updated for checksum
Revision 15:56a06363a698, committed 22 months ago
- Comitter:
- adimmit
- Date:
- Wed Sep 28 16:32:46 2022 +0000
- Parent:
- 14:6fec697f62ef
- Commit message:
- updated to work with checksum and with the 2-motor dyno;
Changed in this revision
leg_message.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/leg_message.h Fri Feb 04 22:38:23 2022 +0000 +++ b/leg_message.h Wed Sep 28 16:32:46 2022 +0000 @@ -3,43 +3,27 @@ #include <stdint.h> -// 31*4 bytes = 124 8-bit words -// 31 32-bit numbers +// 9*2 bytes = 18 16-bit words 36 8-bit words +// 9 32-bit numbers struct spi_data_t { - float q_1s[3]; - float q_2s[3]; - float q_3s[3]; - float qd_1s[3]; - float qd_2s[3]; - float qd_3s[3]; - float tau_1s[3]; - float tau_2s[3]; - float tau_3s[3]; - uint32_t flags[3]; + float q_1s[2]; + float qd_1s[2]; + float tau_1s[2]; + int32_t flags[2]; uint32_t checksum; }; -// 49*4 bytes = 196 8-bit words -// 49 32-bit numbers +// 13*2 bytes = 26 16-bit words 52 8-bit words +// 13 32-bit numbers struct spi_command_t { - float q_des_1s[3]; //first motor on each can bus 0-can1, 1-can2, 2-can3 - float q_des_2s[3]; - float q_des_3s[3]; - float qd_des_1s[3]; - float qd_des_2s[3]; - float qd_des_3s[3]; - float kp_1s[3]; - float kp_2s[3]; - float kp_3s[3]; - float kd_1s[3]; - float kd_2s[3]; - float kd_3s[3]; - float tau_1s_ff[3]; - float tau_2s_ff[3]; - float tau_3s_ff[3]; - uint32_t flags[3]; + float q_des_1s[2]; //first motor on each can bus 0-can1, 1-can2, 2-can3 + float qd_des_1s[2]; + float kp_1s[2]; + float kd_1s[2]; + float tau_1s_ff[2]; + int32_t flags[2]; uint32_t checksum; }; @@ -54,10 +38,10 @@ }; struct grouped_act_state{ - joint_state a1, a2, a3; //removed a2 and a3 + joint_state a1; //removed a2 and a3 }; struct grouped_act_control{ - joint_control a1, a2, a3; + joint_control a1; } ; #endif \ No newline at end of file
--- a/main.cpp Fri Feb 04 22:38:23 2022 +0000 +++ b/main.cpp Wed Sep 28 16:32:46 2022 +0000 @@ -10,12 +10,12 @@ #include "leg_message.h" // length of receive/transmit buffers -#define RX_LEN 98 //CHECK THESE BUFFER LENGHTS -#define TX_LEN 62 //CHECK THESE BUFFER LENGHTS +#define RX_LEN 52 //CHECK THESE BUFFER LENGHTS +#define TX_LEN 52 //CHECK THESE BUFFER LENGHTS // length of outgoing/incoming messages -#define DATA_LEN 62 //CHECK THESE BUFFER LENGHTS -#define CMD_LEN 98 //CHECK THESE BUFFER LENGHTS +#define DATA_LEN 36 //CHECK THESE BUFFER LENGHTS +#define CMD_LEN 52 //CHECK THESE BUFFER LENGHTS // Master CAN ID /// #define CAN_ID 0x0 @@ -64,7 +64,7 @@ CANMessage rxMsg1, rxMsg2, rxMsg3; CANMessage txMsg1, txMsg2, txMsg3; -CANMessage q11_can, q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages +CANMessage q11_can, q21_can; //q12_can, q13_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages int ledState; Ticker sendCAN; int counter = 0; @@ -77,8 +77,8 @@ //SPISlave spi(PA_7, PA_6, PA_5, PA_4); -grouped_act_state g1_state, g2_state, g3_state; -grouped_act_control g1_control, g2_control, g3_control; +grouped_act_state g1_state, g2_state; // g3_state; +grouped_act_control g1_control, g2_control; // g3_control; uint16_t x = 0; uint16_t x2 = 0; @@ -164,16 +164,6 @@ group->a1.v = v; group->a1.t = t; } - else if(id==2){ - group->a2.p = p; - group->a2.v = v; - group->a2.t = t; - } - else if(id==3){ - group->a3.p = p; - group->a3.v = v; - group->a3.t = t; - } } void rxISR1() { @@ -184,24 +174,12 @@ can2.read(rxMsg2); unpack_reply(rxMsg2, &g2_state); } -void rxISR3(){ - can3.read(rxMsg3); - unpack_reply(rxMsg3, &g3_state); - } void PackAll(){ //actuators on the CAN1 bus pack_cmd(&q11_can, g1_control.a1); - pack_cmd(&q12_can, g1_control.a2); - pack_cmd(&q13_can, g1_control.a3); //actuators on the CAN2 bus pack_cmd(&q21_can, g2_control.a1); - pack_cmd(&q22_can, g2_control.a2); - pack_cmd(&q23_can, g2_control.a3); - //actuators on the CAN3 bus - pack_cmd(&q31_can, g3_control.a1); - pack_cmd(&q32_can, g3_control.a2); - pack_cmd(&q33_can, g3_control.a3); } void WriteAll(){ //toggle = 1; @@ -210,23 +188,6 @@ wait(.00002); can2.write(q21_can); wait(.00002); - can3.write(q31_can); - wait(.00002); - //ID = 2 actuators - can1.write(q12_can); - wait(.00002); - can2.write(q22_can); - wait(.00002); - can3.write(q32_can); - wait(.00002); - //ID = 3 actuators - can1.write(q13_can); - wait(.00002); - can2.write(q23_can); - wait(.00002); - can3.write(q33_can); - wait(.00002); - //toggle = 0; } void sendCMD(){ @@ -235,7 +196,7 @@ PackAll(); if(counter>100){ - pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", g1_state.a1.p, g1_state.a2.p, g1_state.a3.p, g2_state.a1.p, g2_state.a2.p, g2_state.a3.p, g3_state.a1.p, g3_state.a2.p, g3_state.a3.p); + pc.printf("%.3f %.3f\n\r", g1_state.a1.p, g2_state.a1.p); counter = 0 ; } @@ -293,16 +254,8 @@ pc.printf("\n\r exiting motor mode \n\r"); //CAN BUS 1 ExitMotorMode(&q11_can); - ExitMotorMode(&q12_can); - ExitMotorMode(&q13_can); //CAN BUS 2 ExitMotorMode(&q21_can); - ExitMotorMode(&q22_can); - ExitMotorMode(&q23_can); - //CAN BUS 3 - ExitMotorMode(&q31_can); - ExitMotorMode(&q32_can); - ExitMotorMode(&q33_can); //DISABLE FLAG enabled = 0; break; @@ -310,16 +263,8 @@ pc.printf("\n\r entering motor mode \n\r"); //CAN BUS 1 EnterMotorMode(&q11_can); - EnterMotorMode(&q12_can); - EnterMotorMode(&q13_can); //CAN BUS 2 EnterMotorMode(&q21_can); - EnterMotorMode(&q22_can); - EnterMotorMode(&q23_can); - //CAN BUS 3 - EnterMotorMode(&q31_can); - EnterMotorMode(&q32_can); - EnterMotorMode(&q33_can); //WAIT FOR ENABLE wait(.5); //ENABLE FLAG @@ -336,16 +281,16 @@ pc.printf("\n\r zeroing \n\r"); //CAN BUS 1 Zero(&q11_can); - Zero(&q12_can); - Zero(&q13_can); + //Zero(&q12_can); + //Zero(&q13_can); //CAN BUS 2 Zero(&q21_can); - Zero(&q22_can); - Zero(&q23_can); + //Zero(&q22_can); + //Zero(&q23_can); //CAN BUS 3 - Zero(&q31_can); - Zero(&q32_can); - Zero(&q33_can); + //Zero(&q31_can); + //Zero(&q32_can); + //Zero(&q33_can); break; } } @@ -366,61 +311,19 @@ void print_SPI_command() { pc.printf("SPI MESSAGE RECIEVED:\n"); - //CAN ONE + //CAN ID ONE pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]); pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]); pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]); pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]); pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]); - pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]); - pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]); - pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]); - pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]); - pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]); - - pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]); - pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]); - pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]); - pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]); - pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]); - pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]); pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]); pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]); pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]); pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]); - pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]); - pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]); - pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]); - pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]); - pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]); - - pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]); - pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]); - pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]); - pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]); - pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]); - - pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]); - pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]); - pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]); - pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]); - pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]); - - pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]); - pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]); - pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]); - pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]); - pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]); - - pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]); - pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]); - pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]); - pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]); - pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]); - } @@ -428,34 +331,13 @@ void print_SPI_data() { pc.printf("SPI MESSAGE SENT:\n"); - //CAN ONE + //CAN ID ONES pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]); pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]); - pc.printf("MOTOR 1-2 Q: %f\n", spi_data.q_2s[0]); - pc.printf("MOTOR 1-2 Qd: %f\n", spi_data.qd_2s[0]); - - pc.printf("MOTOR 1-3 Q: %f\n", spi_data.q_3s[0]); - pc.printf("MOTOR 1-3 Qd: %f\n", spi_data.qd_3s[0]); - pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]); pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]); - pc.printf("MOTOR 2-2 Q: %f\n", spi_data.q_2s[1]); - pc.printf("MOTOR 2-2 Qd: %f\n", spi_data.qd_2s[1]); - - pc.printf("MOTOR 2-3 Q: %f\n", spi_data.q_3s[1]); - pc.printf("MOTOR 2-3 Qd: %f\n", spi_data.qd_3s[1]); - - pc.printf("MOTOR 3-1 Q: %f\n", spi_data.q_1s[2]); - pc.printf("MOTOR 3-1 Qd: %f\n", spi_data.qd_1s[2]); - - pc.printf("MOTOR 3-2 Q: %f\n", spi_data.q_2s[2]); - pc.printf("MOTOR 3-2 Qd: %f\n", spi_data.qd_2s[2]); - - pc.printf("MOTOR 3-3 Q: %f\n", spi_data.q_3s[2]); - pc.printf("MOTOR 3-3 Qd: %f\n", spi_data.qd_3s[2]); - } @@ -465,40 +347,18 @@ //pc.printf("%f\n", spi_command.q_des_2s[0]); // update qs spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0; - spi_data.q_2s[0] = spi_command.q_des_2s[0]+1.0; - spi_data.q_3s[0] = spi_command.q_des_3s[0]+1.0; spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0; - spi_data.q_2s[1] = spi_command.q_des_2s[1]+1.0; - spi_data.q_3s[1] = spi_command.q_des_3s[1]+1.0; - spi_data.q_1s[2] = spi_command.q_des_1s[2]+1.0; - spi_data.q_2s[2] = spi_command.q_des_2s[2]+1.0; - spi_data.q_3s[2] = spi_command.q_des_3s[2]+1.0; // update qds spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0; - spi_data.qd_2s[0] = spi_command.qd_des_2s[0]+1.0; - spi_data.qd_3s[0] = spi_command.qd_des_3s[0]+1.0; spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0; - spi_data.qd_2s[1] = spi_command.qd_des_2s[1]+1.0; - spi_data.qd_3s[1] = spi_command.qd_des_3s[1]+1.0; - spi_data.qd_1s[2] = spi_command.qd_des_1s[2]+1.0; - spi_data.qd_2s[2] = spi_command.qd_des_2s[2]+1.0; - spi_data.qd_3s[2] = spi_command.qd_des_3s[2]+1.0; // update taus spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0; - spi_data.tau_2s[0] = spi_command.tau_2s_ff[0]+1.0; - spi_data.tau_3s[0] = spi_command.tau_3s_ff[0]+1.0; spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0; - spi_data.tau_2s[1] = spi_command.tau_2s_ff[1]+1.0; - spi_data.tau_3s[1] = spi_command.tau_3s_ff[1]+1.0; - spi_data.tau_1s[2] = spi_command.tau_1s_ff[2]+1.0; - spi_data.tau_2s[2] = spi_command.tau_2s_ff[2]+1.0; - spi_data.tau_3s[2] = spi_command.tau_3s_ff[2]+1.0; // UDPATE FLAGS spi_data.flags[0] = 0; spi_data.flags[1] = 0; - spi_data.flags[2] = 0; // UPDATE CHECKSUM - spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); + spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8); //DONT CRC THE CRC! for(int i = 0; i < DATA_LEN; i++){ tx_buff[i] = ((uint16_t*)(&spi_data))[i];} } @@ -526,7 +386,7 @@ //update crc_chk from buffer for(int i = 0; i < RX_LEN; i++) {((uint16_t*)(&tmp_crc_chk))[i] = rx_buff[i];} // CHECK THE CHECKSUM - uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 48); + uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 12); //DONT CRC THE CRC!!! // READ CHECKSUM uint32_t _rx_crc = tmp_crc_chk.checksum; @@ -538,13 +398,11 @@ //process(); control(); PackAll(); - WriteAll(); + WriteAll(); //new_command = 1; - //print_SPI_command(); + //print_SPI_command(); } else{pc.printf("bigger F in the chat...\n");} - - } int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ @@ -579,27 +437,9 @@ //BUS ONE EnterMotorMode(&q11_can); can1.write(q11_can); - EnterMotorMode(&q12_can); - can1.write(q12_can); - EnterMotorMode(&q13_can); - can1.write(q13_can); //BUS TWO EnterMotorMode(&q21_can); can2.write(q21_can); - EnterMotorMode(&q22_can); - can2.write(q22_can); - EnterMotorMode(&q23_can); - can2.write(q23_can); - //BUS THREE - EnterMotorMode(&q31_can); - can3.write(q31_can); - EnterMotorMode(&q32_can); - can3.write(q32_can); - EnterMotorMode(&q33_can); - can3.write(q33_can); - //WRITE THE COMMANDS - //WriteAll(); - //SERIAL TO USER //pc.printf("e\n\r"); return; } @@ -608,70 +448,29 @@ //BUS ONE ExitMotorMode(&q11_can); can1.write(q11_can); - ExitMotorMode(&q12_can); - can1.write(q12_can); - ExitMotorMode(&q13_can); - can1.write(q13_can); //BUS TWO ExitMotorMode(&q21_can); can2.write(q21_can); - ExitMotorMode(&q22_can); - can2.write(q22_can); - ExitMotorMode(&q23_can); - can2.write(q23_can); - //BUS THREE - ExitMotorMode(&q31_can); - can3.write(q31_can); - ExitMotorMode(&q32_can); - can3.write(q32_can); - ExitMotorMode(&q33_can); - can3.write(q33_can); - //WRITE THE COMMANDS - //WriteAll(); - //SERIAL TO USER - //pc.printf("x\n\r"); return; } //BUS 1 DATA spi_data.q_1s[0] = g1_state.a1.p; - spi_data.q_2s[0] = g1_state.a2.p; - spi_data.q_3s[0] = g1_state.a3.p; spi_data.qd_1s[0] = g1_state.a1.v; - spi_data.qd_2s[0] = g1_state.a2.v; - spi_data.qd_3s[0] = g1_state.a3.v; spi_data.tau_1s[0] = g1_state.a1.t; - spi_data.tau_2s[0] = g1_state.a2.t; - spi_data.tau_3s[0] = g1_state.a3.t; //BUS 2 DATA spi_data.q_1s[1] = g2_state.a1.p; - spi_data.q_2s[1] = g2_state.a2.p; - spi_data.q_3s[1] = g2_state.a3.p; spi_data.qd_1s[1] = g2_state.a1.v; - spi_data.qd_2s[1] = g2_state.a2.v; - spi_data.qd_3s[1] = g2_state.a3.v; spi_data.tau_1s[1] = g2_state.a1.t; - spi_data.tau_2s[1] = g2_state.a2.t; - spi_data.tau_3s[1] = g2_state.a3.t; - //BUS 3 DATA - spi_data.q_1s[2] = g3_state.a1.p; - spi_data.q_2s[2] = g3_state.a2.p; - spi_data.q_3s[2] = g3_state.a3.p; - spi_data.qd_1s[2] = g3_state.a1.v; - spi_data.qd_2s[2] = g3_state.a2.v; - spi_data.qd_3s[2] = g3_state.a3.v; - spi_data.tau_1s[2] = g3_state.a1.t; - spi_data.tau_2s[2] = g3_state.a2.t; - spi_data.tau_3s[2] = g3_state.a3.t; if(estop==0){ printf("estopped!!!!\n\r"); memset(&g1_control, 0, sizeof(g1_control)); memset(&g2_control, 0, sizeof(g2_control)); - memset(&g3_control, 0, sizeof(g3_control)); + //memset(&g3_control, 0, sizeof(g3_control)); spi_data.flags[0] = 0xdead; spi_data.flags[1] = 0xdead; - spi_data.flags[2] = 0xdead; + //spi_data.flags[2] = 0xdead; led = 1; } @@ -679,7 +478,7 @@ led = 0; memset(&g1_control, 0, sizeof(g1_control)); memset(&g2_control, 0, sizeof(g2_control)); - memset(&g3_control, 0, sizeof(g3_control)); + //memset(&g3_control, 0, sizeof(g3_control)); //TRANSLATE SPI TO ACTUATOR COMMANNDS //CAN1 @@ -689,78 +488,17 @@ g1_control.a1.kp = spi_command.kp_1s[0]; g1_control.a1.kd = spi_command.kd_1s[0]; g1_control.a1.t_ff = spi_command.tau_1s_ff[0]; - //CAN1 MOTOR 2 - g1_control.a2.p_des = spi_command.q_des_2s[0]; - g1_control.a2.v_des = spi_command.qd_des_2s[0]; - g1_control.a2.kp = spi_command.kp_2s[0]; - g1_control.a2.kd = spi_command.kd_2s[0]; - g1_control.a2.t_ff = spi_command.tau_2s_ff[0]; - //CAN1 MOTOR 3 - g1_control.a3.p_des = spi_command.q_des_3s[0]; - g1_control.a3.v_des = spi_command.qd_des_3s[0]; - g1_control.a3.kp = spi_command.kp_3s[0]; - g1_control.a3.kd = spi_command.kd_3s[0]; - g1_control.a3.t_ff = spi_command.tau_3s_ff[0]; - //CAN2 //CAN2 MOTOR1 g2_control.a1.p_des = spi_command.q_des_1s[1]; g2_control.a1.v_des = spi_command.qd_des_1s[1]; g2_control.a1.kp = spi_command.kp_1s[1]; g2_control.a1.kd = spi_command.kd_1s[1]; g2_control.a1.t_ff = spi_command.tau_1s_ff[1]; - //CAN2 MOTOR 2 - g2_control.a2.p_des = spi_command.q_des_2s[1]; - g2_control.a2.v_des = spi_command.qd_des_2s[1]; - g2_control.a2.kp = spi_command.kp_2s[1]; - g2_control.a2.kd = spi_command.kd_2s[1]; - g2_control.a2.t_ff = spi_command.tau_2s_ff[1]; - //CAN2 MOTOR 3 - g2_control.a3.p_des = spi_command.q_des_3s[1]; - g2_control.a3.v_des = spi_command.qd_des_3s[1]; - g2_control.a3.kp = spi_command.kp_3s[1]; - g2_control.a3.kd = spi_command.kd_3s[1]; - g2_control.a3.t_ff = spi_command.tau_3s_ff[1]; - //CAN3 - //CAN3 MOTOR1 - g3_control.a1.p_des = spi_command.q_des_1s[2]; - g3_control.a1.v_des = spi_command.qd_des_1s[2]; - g3_control.a1.kp = spi_command.kp_1s[2]; - g3_control.a1.kd = spi_command.kd_1s[2]; - g3_control.a1.t_ff = spi_command.tau_1s_ff[2]; - //CAN3 MOTOR 2 - g3_control.a2.p_des = spi_command.q_des_2s[2]; - g3_control.a2.v_des = spi_command.qd_des_2s[2]; - g3_control.a2.kp = spi_command.kp_2s[2]; - g3_control.a2.kd = spi_command.kd_2s[2]; - g3_control.a2.t_ff = spi_command.tau_2s_ff[2]; - //CAN3 MOTOR 3 - g3_control.a3.p_des = spi_command.q_des_3s[2]; - g3_control.a3.v_des = spi_command.qd_des_3s[2]; - g3_control.a3.kp = spi_command.kp_3s[2]; - g3_control.a3.kd = spi_command.kd_3s[2]; - g3_control.a3.t_ff = spi_command.tau_3s_ff[2]; - //SPI FLAGS RETURN //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE spi_data.flags[0] = 0; spi_data.flags[1] = 0; - spi_data.flags[2] = 0; - //spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A1_LIM_P, A1_LIM_N); - //spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, A2_LIM_P, A2_LIM_N))<<1; - //spi_data.flags[0] |= (softstop_joint(g1_state.a3, &g1_control.a3, A3_LIM_P, A3_LIM_N))<<2; - //spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A1_LIM_P, A1_LIM_N); - //spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, A2_LIM_P, A2_LIM_N))<<1; - //spi_data.flags[1] |= (softstop_joint(g2_state.a3, &g2_control.a3, A3_LIM_P, A3_LIM_N))<<2; - //spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A1_LIM_P, A1_LIM_N); - //spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, A2_LIM_P, A2_LIM_N))<<1; - //spi_data.flags[2] |= (softstop_joint(g3_state.a3, &g3_control.a3, A3_LIM_P, A3_LIM_N))<<2; - - //spi_data.flags[0] = 0xbeef; - //spi_data.flags[1] = 0xbeef; - //PackAll(); - //WriteAll(); } - // UPDATE CHECKSUM - spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); + spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8); for(int i = 0; i < DATA_LEN; i++){ tx_buff[i] = ((uint16_t*)(&spi_data))[i];} @@ -772,19 +510,15 @@ 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); + spi_data.checksum = xor_checksum((uint32_t*)&spi_data,12); for(int i = 0; i < DATA_LEN; i++) tx_buff[i] = ((uint16_t*)(&spi_data))[i]; @@ -793,7 +527,7 @@ void init_spi(void){ SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4); spi->format(16, 0); - spi->frequency(6000000); + spi->frequency(500000); spi->reply(0x0); cs.fall(&spi_isr); pc.printf("done\n\r"); @@ -805,18 +539,9 @@ 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)); @@ -832,46 +557,21 @@ //CAN 1 BUS q11_can.len = 8; //transmit 8 bytes - q12_can.len = 8; //transmit 8 bytes - q13_can.len = 8; //CAN 2 BUS q21_can.len = 8; //transmit 8 bytes - q22_can.len = 8; //transmit 8 bytes - q23_can.len = 8; - //CAN 3 BUS - q31_can.len = 8; //transmit 8 bytes - q32_can.len = 8; //transmit 8 bytes - q33_can.len = 8; //RECIEVE rxMsg1.len = 6; //receive 6 bytes rxMsg2.len = 6; - rxMsg3.len = 6; //CAN 1 BUS q11_can.id = 0x1; - q12_can.id = 0x2; - q13_can.id = 0x3; //CAN 2 BUS q21_can.id = 0x1; - q22_can.id = 0x2; - q23_can.id = 0x3; - //CAN 3 BUS - q31_can.id = 0x1; - q32_can.id = 0x2; - q33_can.id = 0x3; //actuators on the CAN1 bus pack_cmd(&q11_can, g1_control.a1); - pack_cmd(&q12_can, g1_control.a2); - pack_cmd(&q13_can, g1_control.a3); //actuators on the CAN2 bus pack_cmd(&q21_can, g2_control.a1); - pack_cmd(&q22_can, g2_control.a2); - pack_cmd(&q23_can, g2_control.a3); - //actuators on the CAN3 bus - pack_cmd(&q31_can, g3_control.a1); - pack_cmd(&q32_can, g3_control.a2); - pack_cmd(&q33_can, g3_control.a3); //WRITE THE INITIAL COMMAND WriteAll(); @@ -891,24 +591,13 @@ //spi_command=set the thing here... while(1) { - //pc.printf("test, of SPINE\r\n"); + //pc.printf("heartbeat...\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++; - */ - } + ///wait(0.01); + } } \ No newline at end of file