Updated for checksum
Diff: main.cpp
- Revision:
- 9:16a550d36425
- Parent:
- 8:450fbccaf4f6
- Child:
- 10:42438194e82b
--- a/main.cpp Tue Apr 06 00:58:00 2021 +0000 +++ b/main.cpp Fri Apr 09 20:41:24 2021 +0000 @@ -425,6 +425,41 @@ +void print_SPI_data() { + pc.printf("SPI MESSAGE SENT:\n"); + //CAN ONE + 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]); + +} + + + + void spi_isr(void) { @@ -451,6 +486,7 @@ for(int i = 0; i < CMD_LEN; i++) { ((uint16_t*)(&spi_command))[i] = rx_buff[i]; + //pc.printf("BYTE %d SENT: %d\n", i, tx_buff[i]); //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]); } //print_SPI_command(); @@ -464,6 +500,7 @@ pc.printf("CURRENT: %d\n", spi_command.checksum);} */ + //print_SPI_data(); control(); PackAll(); WriteAll(); @@ -479,6 +516,7 @@ } 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; @@ -495,6 +533,7 @@ control->t_ff += KP_SOFTSTOP*(limit_n - state.p); return 1; } + */ return 0; } @@ -570,19 +609,19 @@ 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; + 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; //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; + 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; if(estop==0){ printf("estopped!!!!\n\r"); @@ -664,15 +703,15 @@ 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] |= 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;