static void flexsea_stream_exp_4(void)
{
int numb = 0;
//Special1 command to test the Dual ShuoBot Exo
numb = tx_cmd_ctrl_special_1(FLEXSEA_EXECUTE_2, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
KEEP, 0, KEEP, 0, 0, 0);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
//flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_spi_rx();
usleep(STREAM_DELAY_US);
numb = tx_cmd_ctrl_special_1(FLEXSEA_EXECUTE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
KEEP, 0, KEEP, 0, 0, 0);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
//flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_spi_rx();
usleep(STREAM_DELAY_US);
}
static void flexsea_stream_exp_6(void)
{
int numb = 0;
//Special2 command to test the CSEA Knee.
numb = tx_cmd_ctrl_special_2(FLEXSEA_EXECUTE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
0, 0, 0, 0, 0,\
KEEP, 0, 0, 0, 0);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
#ifdef USE_SPI
flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_spi_rx();
#endif
#ifdef USE_USB
flexsea_serial_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_usb_rx();
#endif
flexsea_stream_print_5();
}
//Everytime we receive an SPI string we transmit data
//This function prepares the data to be sent.
//ToDo delete?
void flexsea_prepare_spi_tx_buffer(uint8_t base_addr)
{
flexsea_update_slave_read_buffer(base_addr);
build_slave_payload(base_addr);
comm_gen_str(payload_str, comm_str_spi, PAYLOAD_BUF_LEN);
comm_str_to_txbuffer();
}
void braking_sequence(int cycles, int delay)
{
int i = 0, numb = 0;
for(i = 0; i < cycles; i++)
{
numb = tx_cmd_ctrl_o(FLEXSEA_EXECUTE_1, CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, -400);
numb = comm_gen_str(tmp_payload_xmit, comm_str_usb, numb);
flexsea_send_serial_slave(PORT_SPI, comm_str_usb, numb);
usleep(delay);
numb = tx_cmd_ctrl_o(FLEXSEA_EXECUTE_1, CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, 0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_usb, numb);
flexsea_send_serial_slave(PORT_SPI, comm_str_usb, numb);
usleep(delay);
}
}
static void flexsea_stream_exp_3(void)
{
int numb = 0;
//For now we only read the switch
numb = tx_cmd_switch(FLEXSEA_MANAGE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
//flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_spi_rx();
flexsea_stream_print_3();
}
static void flexsea_stream_exp_5(void)
{
int numb = 0;
//Special4 command to test the Dual ShuoBot Exo.
//Manage needs to be autosampling Special1 for this to work.
numb = tx_cmd_ctrl_special_4(FLEXSEA_MANAGE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
KEEP, 0, KEEP, 0, 0, 0, \
KEEP, 0, KEEP, 0, 0, 0);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
//flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
decode_spi_rx();
usleep(STREAM_DELAY_US);
}
void MainWindow::timerStreamEvent(void)
{
if(stream_status)
{
//==========
//Test: call Stream 1
int numb = 0;
//Special1 command to test the ShuoBot Exo
numb = tx_cmd_ctrl_special_1(FLEXSEA_EXECUTE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
KEEP, 0, KEEP, 0, 0, exp_pwm);
numb = comm_gen_str(payload_str, comm_str_spi, PAYLOAD_BUF_LEN);
numb = COMM_STR_BUF_LEN;
//flexsea_serial_transmit(numb, comm_str_spi, 0); //DIY driver
USBSerialPort_Write(numb, comm_str_spi); //QSerialPort
//Can we decode what we received?
USBSerialPort_Read(usb_rx);
decode_usb_rx(usb_rx);
//Display return status:
//qDebug() << "decode_usb_rx(): " << val;
ui->disp_enc->setText(QString::number(exec1.encoder));
ui->disp_gyrox->setText(QString::number(exec1.imu.x));
ui->disp_gyroy->setText(QString::number(exec1.imu.y));
ui->disp_gyroz->setText(QString::number(exec1.imu.z));
ui->disp_strain->setText(QString::number(exec1.strain));
ui->disp_ana->setText(QString::number(exec1.analog[0]));
ui->disp_current->setText(QString::number(exec1.current));
ui->disp_stat1->setText(QString::number(exec1.status1));
ui->disp_stat2->setText(QString::number(exec1.status2));
ui->tabWidget->repaint();
//==========
}
}
static void flexsea_log_exp_1(FILE *logfile, char min, char sec, uint32_t *tmp, uint32_t *lines, uint32_t *good)
{
//Used to test the ShuoBot Exo
unsigned int numb = 0;
numb = tx_cmd_ctrl_special_1(FLEXSEA_EXECUTE_1, CMD_READ, payload_str, PAYLOAD_BUF_LEN, \
KEEP, 0, KEEP, 0, 77, 0);
numb = comm_gen_str(payload_str, comm_str_usb, PAYLOAD_BUF_LEN); //Was comm_str_spi. ToDo is it ok?
numb = COMM_STR_BUF_LEN;
//flexsea_spi_transmit(numb, comm_str_spi, 0);
//Can we decode what we received?
*tmp = decode_spi_rx();
(*lines)++;
(*good) += (*tmp);
//Log to file:
fprintf(logfile, "[%d:%d],%i,%i,%i,%i,%i,%i,%i\n", min, sec, \
exec1.enc_display, exec1.current, exec1.gyro.x, exec1.gyro.y, exec1.gyro.z, \
exec1.strain, exec1.analog[0]);
return;
}
static void parser_flexsea(int slave, int cmd, char rw, char *argv[])
{
int numb = 0;
int tmp0 = 0, tmp1 = 0, tmp2 = 0, tmp3 = 0, tmp4 = 0, tmp5 = 0;
switch(cmd) //ToDo support r/w
{
case 0: //'ping'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 1: //'status'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 2: //'reset'
_USE_PRINTF("Reset: ");
console_reset(slave_id[slave]);
break;
case 3: //'ack'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 4: //'mem'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
_USE_PRINTF("[Memory]: Base addr. = %i, bytes = %i.\n", tmp0, tmp1);
//Prepare and send data:
//numb = tx_cmd_mem_read(slave_id[slave], 0, tmp0, tmp1); //ToDo
numb = comm_gen_str(payload_str, comm_str_spi, numb);
break;
case 5: //'acqui'
tmp0 = atoi(argv[4]);
_USE_PRINTF("[Acquisition mode]: %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_data_acqui(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 6: //'rs485_config'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 7: //'usb_config'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 8: //'usb_write'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 9: //'temp'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 10: //'switch'
_USE_PRINTF("[Switch]");
//Prepare and send data:
numb = tx_cmd_switch(slave_id[slave], CMD_READ, tmp_payload_xmit, PAYLOAD_BUF_LEN);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 11: //'imu'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
_USE_PRINTF("[IMU]: %i & %i.\n", tmp0, tmp1);
//Prepare and send data:
//numb = tx_cmd_imu_read(slave_id[slave], tmp0, tmp1); //ToDo
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 12: //'encoder'
tmp0 = atoi(argv[4]);
if(rw == 'r')
{
_USE_PRINTF("[Read Encoder]\n");
//Prepare and send data:
numb = tx_cmd_encoder(slave_id[slave], CMD_READ, tmp_payload_xmit, PAYLOAD_BUF_LEN, 0);
}
else
{
_USE_PRINTF("[Write Encoder]: %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_encoder(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0);
}
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 13: //'strain'
_USE_PRINTF("[Strain]\n");
//Prepare and send data:
//numb = tx_cmd_strain_read(slave_id[slave]); //ToDo
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 14: //'cmd_strain_config'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
_USE_PRINTF("[Config Strain]: Offset = %i, Gain = %i, Output ref. = %i.\n", tmp0, tmp1, tmp2);
//Prepare and send data:
//numb = tx_cmd_strain_config(slave_id[slave], tmp0, tmp1, tmp2); //ToDo
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 15: //'volt'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 16: //'batt'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 17: //'power_out'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 18: //'clutch'
tmp0 = atoi(argv[4]);
_USE_PRINTF("[Clutch]: %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_exp_clutch(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 19: //'adv_ana_config'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 20: //'analog'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
_USE_PRINTF("[Analog]: Base addr. = %i, Values = %i.\n", tmp0, tmp1);
//Prepare and send data:
//numb = tx_cmd_analog_read(slave_id[slave], tmp0, tmp1); //ToDo
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 21: //'digital'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 22: //'digital_config'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 23: //'exp_periph_config'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 24: //'ctrl_mode'
tmp0 = atoi(argv[4]);
_USE_PRINTF("[Control Mode]: %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_ctrl_mode(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 25: //'ctrl_i_g'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
_USE_PRINTF("[Current Controller Gains]: kp = %i, ki = %i, kd = %i.\n", tmp0, tmp1, tmp2);
//Prepare and send data:
numb = tx_cmd_ctrl_i_g(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0, tmp1, tmp2);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 26: //'ctrl_p_g'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
_USE_PRINTF("[Position Controller Gains]: kp = %i, ki = %i, kd = %i.\n", tmp0, tmp1, tmp2);
//Prepare and send data:
numb = tx_cmd_ctrl_p_g(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0, tmp1, tmp2);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 27: //'ctrl_z_g'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
_USE_PRINTF("[Impedance Controller Gains]: kp = %i, ki = %i, kd = %i.\n", tmp0, tmp1, tmp2);
//Prepare and send data:
numb = tx_cmd_ctrl_z_g(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0, tmp1, tmp2);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 28: //'ctrl_o'
tmp0 = atoi(argv[4]);
_USE_PRINTF("[Open Loop Controller]: PWMDC = %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_ctrl_o(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 29: //'ctrl_i'
tmp0 = atoi(argv[4]);
_USE_PRINTF("[Current Controller]: %i.\n", tmp0);
//Prepare and send data:
numb = tx_cmd_ctrl_i(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, tmp0, 0);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 30: //'ctrl_p'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
tmp3 = atoi(argv[7]);
_USE_PRINTF("[Position Controller]: %i, %i, %i, %i.\n", tmp0, tmp1, tmp2, tmp3);
//Prepare and send data:
numb = tx_cmd_ctrl_p(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, 0, tmp0, tmp1, tmp2, tmp3);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 31: //'shorted_leads'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
case 32: //'spc1'
tmp0 = atoi(argv[4]);
tmp1 = atoi(argv[5]);
tmp2 = atoi(argv[6]);
tmp3 = atoi(argv[7]);
tmp4 = atoi(argv[8]);
tmp5 = atoi(argv[9]);
_USE_PRINTF("[Special1]: %i, %i, %i, %i, %i, %i.\n", tmp0, tmp1, tmp2, tmp3, tmp4, tmp5);
//Prepare and send data:
numb = tx_cmd_ctrl_special_1(slave_id[slave], CMD_WRITE, tmp_payload_xmit, PAYLOAD_BUF_LEN, \
tmp0, tmp1, tmp2, tmp3, tmp4, tmp5);
numb = comm_gen_str(tmp_payload_xmit, comm_str_spi, numb);
break;
case 33: //'spc2'
_USE_PRINTF("Command isn't programmed yet.\n");
break;
default:
_USE_PRINTF("Invalid command.\n");
break;
}
//If a command was generated, send it
if(numb)
{
#ifdef USE_SPI
flexsea_send_serial_slave(PORT_SPI, comm_str_spi, numb);
#endif
#ifdef USE_USB
flexsea_send_serial_slave(PORT_USB, comm_str_spi, numb+1);
#endif
}
}