MCP_SPIne
Diff: main.cpp
- Revision:
- 6:52b418e1d6d0
- Parent:
- 3:9ef9b4c66648
--- a/main.cpp Wed May 02 18:19:22 2018 +0000 +++ b/main.cpp Sun Jul 15 19:00:15 2018 +0000 @@ -53,8 +53,8 @@ Serial pc(PA_2, PA_3); -CAN can1(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name -CAN can2(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name +CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name +CAN can2(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name CANMessage rxMsg1, rxMsg2; CANMessage txMsg1, txMsg2; @@ -340,8 +340,11 @@ } // run control, which fills in tx_buff for the next iteration - if(calc_checksum != spi_command.checksum){ - spi_data.flags[1] = 0xdead;} + if(calc_checksum != spi_command.checksum) + { + spi_data.flags[1] = 0xdead; + //printf("Bad Checksum!\n\r"); + } //test_control(); //spi_data.q_abad[0] = 12.0f; @@ -449,51 +452,52 @@ memset(&l1_control, 0, sizeof(l1_control)); memset(&l2_control, 0, sizeof(l2_control)); + float tmax = 0.5f; - l1_control.a.p_des = spi_command.q_des_abad[0]; - l1_control.a.v_des = spi_command.qd_des_abad[0]; - l1_control.a.kp = spi_command.kp_abad[0]; - l1_control.a.kd = spi_command.kd_abad[0]; - l1_control.a.t_ff = spi_command.tau_abad_ff[0]; + l1_control.a.p_des = 0;//spi_command.q_des_abad[0]; + l1_control.a.v_des = 0;//spi_command.qd_des_abad[0]; + l1_control.a.kp = 0;//spi_command.kp_abad[0]; + l1_control.a.kd = 0;//spi_command.kd_abad[0]; + l1_control.a.t_ff = fmaxf(fminf(spi_command.tau_abad_ff[0], tmax),-tmax); - l1_control.h.p_des = spi_command.q_des_hip[0]; - l1_control.h.v_des = spi_command.qd_des_hip[0]; - l1_control.h.kp = spi_command.kp_hip[0]; - l1_control.h.kd = spi_command.kd_hip[0]; - l1_control.h.t_ff = spi_command.tau_hip_ff[0]; + l1_control.h.p_des = 0;//spi_command.q_des_hip[0]; + l1_control.h.v_des = 0;//spi_command.qd_des_hip[0]; + l1_control.h.kp = 0;//spi_command.kp_hip[0]; + l1_control.h.kd = 0;//spi_command.kd_hip[0]; + l1_control.h.t_ff = fmaxf(fminf(spi_command.tau_hip_ff[0], tmax), -tmax); - l1_control.k.p_des = spi_command.q_des_knee[0]; - l1_control.k.v_des = spi_command.qd_des_knee[0]; - l1_control.k.kp = spi_command.kp_knee[0]; - l1_control.k.kd = spi_command.kd_knee[0]; - l1_control.k.t_ff = spi_command.tau_knee_ff[0]; + l1_control.k.p_des = 0;//spi_command.q_des_knee[0]; + l1_control.k.v_des = 0;//spi_command.qd_des_knee[0]; + l1_control.k.kp = 0;//spi_command.kp_knee[0]; + l1_control.k.kd = 0;//spi_command.kd_knee[0]; + l1_control.k.t_ff = fmaxf(fminf(spi_command.tau_knee_ff[0], tmax), -tmax); - l2_control.a.p_des = spi_command.q_des_abad[1]; - l2_control.a.v_des = spi_command.qd_des_abad[1]; - l2_control.a.kp = spi_command.kp_abad[1]; - l2_control.a.kd = spi_command.kd_abad[1]; - l2_control.a.t_ff = spi_command.tau_abad_ff[1]; + l2_control.a.p_des = 0;//spi_command.q_des_abad[1]; + l2_control.a.v_des = 0;//spi_command.qd_des_abad[1]; + l2_control.a.kp = 0;//spi_command.kp_abad[1]; + l2_control.a.kd = 0;//spi_command.kd_abad[1]; + l2_control.a.t_ff = fmaxf(fminf(spi_command.tau_abad_ff[1], tmax), -tmax); - l2_control.h.p_des = spi_command.q_des_hip[1]; - l2_control.h.v_des = spi_command.qd_des_hip[1]; - l2_control.h.kp = spi_command.kp_hip[1]; - l2_control.h.kd = spi_command.kd_hip[1]; - l2_control.h.t_ff = spi_command.tau_hip_ff[1]; + l2_control.h.p_des = 0;//spi_command.q_des_hip[1]; + l2_control.h.v_des = 0;//spi_command.qd_des_hip[1]; + l2_control.h.kp = 0;//spi_command.kp_hip[1]; + l2_control.h.kd = 0;//spi_command.kd_hip[1]; + l2_control.h.t_ff = fmaxf(fminf(spi_command.tau_hip_ff[1], tmax), -tmax); - l2_control.k.p_des = spi_command.q_des_knee[1]; - l2_control.k.v_des = spi_command.qd_des_knee[1]; - l2_control.k.kp = spi_command.kp_knee[1]; - l2_control.k.kd = spi_command.kd_knee[1]; - l2_control.k.t_ff = spi_command.tau_knee_ff[1]; + l2_control.k.p_des = 0;//spi_command.q_des_knee[1]; + l2_control.k.v_des = 0;//spi_command.qd_des_knee[1]; + l2_control.k.kp = 0;//spi_command.kp_knee[1]; + l2_control.k.kd = 0;//spi_command.kd_knee[1]; + l2_control.k.t_ff = fmaxf(fminf(spi_command.tau_knee_ff[1], tmax), -tmax); spi_data.flags[0] = 0; spi_data.flags[1] = 0; - spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); - spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; + //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); + //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2; - spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); - spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; + //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); + //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2; //spi_data.flags[0] = 0xbeef; @@ -552,10 +556,10 @@ //spi.reply(0x0); //cs.fall(&spi_isr); - can1.frequency(1000000); // set bit rate to 1Mbps + //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.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