Updated for checksum
Diff: main.cpp
- Revision:
- 5:f1703165ca7e
- Parent:
- 4:e8c1b3f8fc6c
- Child:
- 6:077e37d5c2da
--- a/main.cpp Thu Apr 01 17:53:19 2021 +0000 +++ b/main.cpp Thu Apr 01 21:28:37 2021 +0000 @@ -29,9 +29,9 @@ #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 + #define KD_MAX 10.0f + #define T_MIN -72.0f + #define T_MAX 72.0f /// Joint Soft Stops /// #define A_LIM_P 1.5f @@ -57,13 +57,13 @@ 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 +CAN can1(PA_11, PA_12, 1000000); +CAN can2(PA_8, PA_15, 1000000); +CAN can3(PB_12, PB_13, 1000000); //corresponds to bus 1-3-6 (controller 1) or 2-4-5 (controller 2) IN THAT ORDER 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 +CANMessage q11_can, q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can; //TX Messages int ledState; Ticker sendCAN; int counter = 0; @@ -77,7 +77,7 @@ grouped_act_state g1_state, g2_state, g3_state; -grouped_act_control q1_control, q2_control, q3_control; +grouped_act_control g1_control, g2_control, g3_control; uint16_t x = 0; uint16_t x2 = 0; @@ -159,19 +159,19 @@ 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; + group->a1.p = p; + group->a1.v = v; + group->a1.t = t; } else if(id==2){ - group->h.p = p; - group->h.v = v; - group->h.t = t; + group->a2.p = p; + group->a2.v = v; + group->a2.t = t; } else if(id==3){ - group->k.p = p; - group->k.v = v; - group->k.t = t; + group->a3.p = p; + group->a3.v = v; + group->a3.t = t; } } @@ -189,34 +189,39 @@ } 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); - + //actuators on the CAN1 bus + pack_cmd(&q11_can, q1_control.a1); + pack_cmd(&q12_can, q1_control.a2); + pack_cmd(&q13_can, q1_control.a3); + //actuators on the CAN2 bus + pack_cmd(&q21_can, q2_control.a1); + pack_cmd(&q22_can, q2_control.a2); + pack_cmd(&q23_can, q2_control.a3); + //actuators on the CAN3 bus + pack_cmd(&q31_can, q3_control.a1); + pack_cmd(&q32_can, q3_control.a2); + pack_cmd(&q33_can, q3_control.a3); } void WriteAll(){ //toggle = 1; + //ID = 1 actuators can1.write(q11_can); wait(.00002); - can2.write(q12_can); + can2.write(q21_can); wait(.00002); - can3.write(q13_can); + can3.write(q31_can); wait(.00002); - can1.write(q21_can); + //ID = 2 actuators + can1.write(q12_can); wait(.00002); can2.write(q22_can); wait(.00002); - can3.write(q23_can); + can3.write(q32_can); wait(.00002); - can1.write(q31_can); + //ID = 3 actuators + can1.write(q13_can); wait(.00002); - can2.write(q32_can); + can2.write(q23_can); wait(.00002); can3.write(q33_can); wait(.00002); @@ -274,6 +279,7 @@ msg->data[7] = 0xFD; WriteAll(); } + void serial_isr(){ /// handle keyboard commands from the serial terminal /// while(pc.readable()){ @@ -283,29 +289,38 @@ case(27): //loop.detach(); 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(&q31_can); - ExitMotorMode(&q12_can); ExitMotorMode(&q22_can); + ExitMotorMode(&q23_can); + //CAN BUS 3 + ExitMotorMode(&q31_can); ExitMotorMode(&q32_can); - ExitMotorMode(&q13_can); - ExitMotorMode(&q23_can); ExitMotorMode(&q33_can); + //DISABLE FLAG enabled = 0; break; case('m'): 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(&q31_can); - EnterMotorMode(&q12_can); EnterMotorMode(&q22_can); + EnterMotorMode(&q23_can); + //CAN BUS 3 + EnterMotorMode(&q31_can); EnterMotorMode(&q32_can); - EnterMotorMode(&q13_can); - EnterMotorMode(&q23_can); EnterMotorMode(&q33_can); + //WAIT FOR ENABLE wait(.5); + //ENABLE FLAG enabled = 1; //loop.attach(&sendCMD, .001); break; @@ -317,14 +332,17 @@ break; case('z'): pc.printf("\n\r zeroing \n\r"); + //CAN BUS 1 Zero(&q11_can); - Zero(&q21_can); - Zero(&q31_can); Zero(&q12_can); + Zero(&q13_can); + //CAN BUS 2 + Zero(&q21_can); Zero(&q22_can); + Zero(&q23_can); + //CAN BUS 3 + Zero(&q31_can); Zero(&q32_can); - Zero(&q13_can); - Zero(&q23_can); Zero(&q33_can); break; } @@ -345,60 +363,60 @@ 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]); + //CAN 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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]); + 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]); } @@ -433,7 +451,7 @@ ((uint16_t*)(&spi_command))[i] = rx_buff[i]; //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]); } - //print_SPI_command(); + print_SPI_command(); // run control, which fills in tx_buff for the next iteration /* @@ -443,9 +461,7 @@ 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(); @@ -490,24 +506,25 @@ //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); - can1.write(q21_can); - EnterMotorMode(&q31_can); - can1.write(q31_can); - //BUS TWO - EnterMotorMode(&q12_can); - can2.write(q12_can); + can2.write(q21_can); EnterMotorMode(&q22_can); can2.write(q22_can); - EnterMotorMode(&q32_can); - can2.write(q32_can); + EnterMotorMode(&q23_can); + can2.write(q23_can); //BUS THREE - EnterMotorMode(&q13_can); - can3.write(q13_can); - EnterMotorMode(&q23_can); - can3.write(q23_can); + EnterMotorMode(&q31_can); + can3.write(q31_can); + EnterMotorMode(&q32_can); + can3.write(q32_can); EnterMotorMode(&q33_can); can3.write(q33_can); + //SERIAL TO USER pc.printf("e\n\r"); return; } @@ -516,53 +533,53 @@ //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); - can1.write(q21_can); - ExitMotorMode(&q31_can); - can1.write(q31_can); - //BUS TWO - ExitMotorMode(&q12_can); - can2.write(q12_can); + can2.write(q21_can); ExitMotorMode(&q22_can); can2.write(q22_can); - ExitMotorMode(&q32_can); - can2.write(q32_can); + ExitMotorMode(&q23_can); + can2.write(q23_can); //BUS THREE - ExitMotorMode(&q13_can); - can3.write(q13_can); - ExitMotorMode(&q23_can); - can3.write(q23_can); + ExitMotorMode(&q31_can); + can3.write(q31_can); + ExitMotorMode(&q32_can); + can3.write(q32_can); ExitMotorMode(&q33_can); can3.write(q33_can); + //SERIAL TO USER 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; - - + //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; + //BUS 2 DATA + spi_data.q_1s[1] = g1_state.a1.p; + spi_data.q_2s[1] = g1_state.a2.p; + spi_data.q_3s[1] = g1_state.a3.p; + spi_data.qd_1s[1] = g1_state.a1.v; + spi_data.qd_2s[1] = g1_state.a2.v; + spi_data.qd_3s[1] = g1_state.a3.v; + //BUS 3 DATA + spi_data.q_1s[2] = g1_state.a1.p; + spi_data.q_2s[2] = g1_state.a2.p; + spi_data.q_3s[2] = g1_state.a3.p; + spi_data.qd_1s[2] = g1_state.a1.v; + spi_data.qd_2s[2] = g1_state.a2.v; + spi_data.qd_3s[2] = g1_state.a3.v; if(estop==0){ - //printf("estopped!!!!\n\r"); + 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)); @@ -574,78 +591,81 @@ 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]; + //TRANSLATE SPI TO ACTUATOR COMMANNDS + //CAN1 + //CAN1 MOTOR1 + g1_control.a1.p_des = spi_command.q_des_1s[0]; + g1_control.a1.v_des = spi_command.qd_des_1s[0]; + 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]; - 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 FLAGS RETURN //MIGHT NEED FIXING LATER WHEN WE DO RETURN DATA 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.a1, &g1_control.a1, A_LIM_P, A_LIM_N); + spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, 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.a1, &g2_control.a1, A_LIM_P, A_LIM_N); + spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, 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.a1, &g3_control.a1, A_LIM_P, A_LIM_N); + spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, 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; @@ -686,7 +706,7 @@ void init_spi(void){ SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4); spi->format(16, 0); - spi->frequency(12000000); + spi->frequency(6000000); spi->reply(0x0); cs.fall(&spi_isr); pc.printf("done\n\r"); @@ -723,38 +743,49 @@ pc.printf("\n\r SPIne\n\r"); //printf("%d\n\r", RX_ID << 18); + //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 - 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 + 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; - q21_can.id = 0x2; - q31_can.id = 0x3; - q12_can.id = 0x1; + q12_can.id = 0x2; + q13_can.id = 0x3; + //CAN 2 BUS + q21_can.id = 0x1; q22_can.id = 0x2; - q32_can.id = 0x3; - q13_can.id = 0x1; - q23_can.id = 0x2; - q33_can.id = 0x3; + q23_can.id = 0x3; + //CAN 3 BUS + q31_can.id = 0x1; + q32_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); + //actuators on the CAN1 bus + pack_cmd(&q11_can, q1_control.a1); + pack_cmd(&q12_can, q1_control.a2); + pack_cmd(&q13_can, q1_control.a3); + //actuators on the CAN2 bus + pack_cmd(&q21_can, q2_control.a1); + pack_cmd(&q22_can, q2_control.a2); + pack_cmd(&q23_can, q2_control.a3); + //actuators on the CAN3 bus + pack_cmd(&q31_can, q3_control.a1); + pack_cmd(&q32_can, q3_control.a2); + pack_cmd(&q33_can, q3_control.a3); + //WRITE THE INITIAL COMMAND WriteAll(); //just debugging things