7 #include <linux/spi/spidev.h> 10 #include <lcm/lcm-cpp.hpp> 44 const float hip_offset[4] = {M_PI / 2.f, -M_PI / 2.f, -M_PI / 2.f, M_PI / 2.f};
50 for (
size_t i = 0; i < len; i++) t = t ^ data[i];
63 spi_torque_t *torque_out,
int board_num) {
64 torque_out->tau_abad[board_num] =
65 cmd->kp_abad[board_num] *
66 (cmd->q_des_abad[board_num] - data->q_abad[board_num]) +
67 cmd->kd_abad[board_num] *
68 (cmd->qd_des_abad[board_num] - data->qd_abad[board_num]) +
69 cmd->tau_abad_ff[board_num];
71 torque_out->tau_hip[board_num] =
72 cmd->kp_hip[board_num] *
73 (cmd->q_des_hip[board_num] - data->q_hip[board_num]) +
74 cmd->kd_hip[board_num] *
75 (cmd->qd_des_hip[board_num] - data->qd_hip[board_num]) +
76 cmd->tau_hip_ff[board_num];
78 torque_out->tau_knee[board_num] =
79 cmd->kp_knee[board_num] *
80 (cmd->q_des_knee[board_num] - data->q_knee[board_num]) +
81 cmd->kd_knee[board_num] *
82 (cmd->qd_des_knee[board_num] - data->qd_knee[board_num]) +
83 cmd->tau_knee_ff[board_num];
87 if (cmd->flags[board_num] & 0b1) {
88 if (cmd->flags[board_num] & 0b10)
94 if (torque_out->tau_abad[board_num] > torque_limits[0])
95 torque_out->tau_abad[board_num] = torque_limits[0];
96 if (torque_out->tau_abad[board_num] < -torque_limits[0])
97 torque_out->tau_abad[board_num] = -torque_limits[0];
99 if (torque_out->tau_hip[board_num] > torque_limits[1])
100 torque_out->tau_hip[board_num] = torque_limits[1];
101 if (torque_out->tau_hip[board_num] < -torque_limits[1])
102 torque_out->tau_hip[board_num] = -torque_limits[1];
104 if (torque_out->tau_knee[board_num] > torque_limits[2])
105 torque_out->tau_knee[board_num] = torque_limits[2];
106 if (torque_out->tau_knee[board_num] < -torque_limits[2])
107 torque_out->tau_knee[board_num] = -torque_limits[2];
112 size_t command_size =
sizeof(spi_command_t);
113 size_t data_size =
sizeof(spi_data_t);
118 if (pthread_mutex_init(&
spi_mutex, NULL) != 0)
119 printf(
"[ERROR: RT SPI] Failed to create spi data mutex\n");
122 printf(
"[RT SPI] Error command size is %ld, expected %d\n", command_size,
125 printf(
"[RT SPI] command size good\n");
128 printf(
"[RT SPI] Error data size is %ld, expected %d\n", data_size,
131 printf(
"[RT SPI] data size good\n");
133 printf(
"[RT SPI] Open\n");
139 spi_1_fd = open(
"/dev/spidev2.0", O_RDWR);
140 if (
spi_1_fd < 0) perror(
"[ERROR] Couldn't open spidev 2.0");
141 spi_2_fd = open(
"/dev/spidev2.1", O_RDWR);
142 if (
spi_2_fd < 0) perror(
"[ERROR] Couldn't open spidev 2.1");
145 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_mode (1)");
148 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_mode (2)");
151 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_mode (1)");
154 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_mode (2)");
157 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_bits_per_word (1)");
160 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_bits_per_word (2)");
163 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_bits_per_word (1)");
166 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_bits_per_word (2)");
169 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_max_speed_hz (1)");
171 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_wr_max_speed_hz (2)");
174 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_max_speed_hz (1)");
176 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_max_speed_hz (2)");
179 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_lsb_first (1)");
182 if (rv < 0) perror(
"[ERROR] ioctl spi_ioc_rd_lsb_first (2)");
190 for (
int i = 0; i < 2; i++) {
214 spine_cmd->
kp_abad[i] = cmd->kp_abad[i + leg_0];
215 spine_cmd->
kp_hip[i] = cmd->kp_hip[i + leg_0];
216 spine_cmd->
kp_knee[i] = cmd->kp_knee[i + leg_0];
218 spine_cmd->
kd_abad[i] = cmd->kd_abad[i + leg_0];
219 spine_cmd->
kd_hip[i] = cmd->kd_hip[i + leg_0];
220 spine_cmd->
kd_knee[i] = cmd->kd_knee[i + leg_0];
229 spine_cmd->
flags[i] = cmd->flags[i + leg_0];
236 for (
int i = 0; i < 2; i++) {
239 data->q_hip[i + leg_0] = (spine_data->
q_hip[i] -
hip_offset[i + leg_0]) *
244 data->qd_abad[i + leg_0] =
247 data->qd_knee[i + leg_0] =
250 data->flags[i + leg_0] = spine_data->
flags[i];
253 uint32_t calc_checksum =
xor_checksum((uint32_t *)spine_data, 14);
254 if (calc_checksum != (uint32_t)spine_data->
checksum)
255 printf(
"SPI ERROR BAD CHECKSUM GOT 0x%hx EXPECTED 0x%hx\n", calc_checksum,
269 for (
int spi_board = 0; spi_board < 2; spi_board++) {
274 uint16_t *cmd_d = (uint16_t *)&g_spine_cmd;
275 uint16_t *data_d = (uint16_t *)&g_spine_data;
282 tx_buf[i] = (cmd_d[i] >> 8) + ((cmd_d[i] & 0xff) << 8);
289 struct spi_ioc_transfer spi_message[1];
292 memset(spi_message, 0, 1 *
sizeof(
struct spi_ioc_transfer));
295 for (
int i = 0; i < 1; i++) {
297 spi_message[i].cs_change = 1;
298 spi_message[i].delay_usecs = 0;
299 spi_message[i].len = word_len * 66;
300 spi_message[i].rx_buf = (uint64_t)rx_buf;
301 spi_message[i].tx_buf = (uint64_t)tx_buf;
310 for (
int i = 0; i < 30; i++)
311 data_d[i] = (rx_buf[i] >> 8) + ((rx_buf[i] & 0xff) << 8);
321 for (
int i = 0; i < 4; i++) {
const float hip_offset[4]
static spine_data_t g_spine_data
void spi_send_receive(spi_command_t *command, spi_data_t *data)
#define K_EXPECTED_COMMAND_SIZE
#define K_EXPECTED_DATA_SIZE
const float abad_offset[4]
const float max_torque[3]
unsigned char spi_bits_per_word
const float knee_side_sign[4]
const float disabled_torque[3]
#define K_KNEE_OFFSET_POS
void publish_spi_torque(spi_torque_t *data)
pthread_mutex_t spi_mutex
const float hip_side_sign[4]
void spine_to_spi(spi_data_t *data, spine_data_t *spine_data, int leg_0)
#define K_WORDS_PER_MESSAGE
static spine_cmd_t g_spine_cmd
void fake_spine_control(spi_command_t *cmd, spi_data_t *data, spi_torque_t *torque_out, int board_num)
int spi_driver_iterations
spi_command_t * get_spi_command()
const float knee_offset[4]
const float wimp_torque[3]
void spi_to_spine(spi_command_t *cmd, spine_cmd_t *spine_cmd, int leg_0)
spi_data_t * get_spi_data()
uint32_t xor_checksum(uint32_t *data, size_t len)
const float abad_side_sign[4]
spi_command_t spi_command_drv