SpineBoard Class Reference

#include <SpineBoard.h>

Collaboration diagram for SpineBoard:

Public Member Functions
	SpineBoard ()
void	init (float side_sign, s32 board)
void	run ()
void	resetData ()
void	resetCommand ()

Public Attributes
SpiCommand *	cmd = nullptr
SpiData *	data = nullptr
float	torque_out [3]

Private Attributes
float	side_sign
s32	board_num
const float	max_torque [3] = {17.f, 17.f, 26.f}
const float	wimp_torque [3] = {6.f, 6.f, 6.f}
const float	disabled_torque [3] = {0.f, 0.f, 0.f}
const float	q_limit_p [3] = {1.5f, 5.0f, 0.f}
const float	q_limit_n [3] = {-1.5f, -5.0f, 0.f}
const float	kp_softstop = 100.f
const float	kd_softstop = 0.4f
s32	iter_counter = 0

Detailed Description

Definition at line 45 of file SpineBoard.h.

Constructor & Destructor Documentation

SpineBoard::SpineBoard ( )

inline

Definition at line 47 of file SpineBoard.h.

{}

Member Function Documentation

void SpineBoard::init	(	float	side_sign,
		s32	board
	)

Definition at line 8 of file SpineBoard.cpp.

References board_num, and side_sign.

                                               {
  printf(
      "[Cheetah Control] Hello! I am the SPINE board for leg %d (0-indexed) "
      "with side sign %.3f\n",
      board, sideSign);
  this->board_num = board;
  this->side_sign = sideSign;
}

Here is the caller graph for this function:

void SpineBoard::resetCommand

(

)

Definition at line 35 of file SpineBoard.cpp.

References board_num, cmd, SpiCommand::flags, SpiCommand::kd_abad, SpiCommand::kd_hip, SpiCommand::kd_knee, SpiCommand::kp_abad, SpiCommand::kp_hip, SpiCommand::kp_knee, SpiCommand::q_des_abad, SpiCommand::q_des_hip, SpiCommand::q_des_knee, SpiCommand::qd_des_abad, SpiCommand::qd_des_hip, SpiCommand::qd_des_knee, SpiCommand::tau_abad_ff, SpiCommand::tau_hip_ff, and SpiCommand::tau_knee_ff.

                              {
  if (cmd == nullptr) {
    printf(
        "[ERROR: SPINE board] reset_spine_board_command called when "
        "cheetahlcm_spi_command_t* was null\n");
    return;
  }

  cmd->flags[board_num] = 0;
  cmd->kd_abad[board_num] = 0.f;
  cmd->kd_hip[board_num] = 0.f;
  cmd->kd_knee[board_num] = 0.f;
  cmd->kp_abad[board_num] = 0.f;
  cmd->kp_hip[board_num] = 0.f;
  cmd->kp_knee[board_num] = 0.f;
  cmd->qd_des_abad[board_num] = 0.f;
  cmd->qd_des_hip[board_num] = 0.f;
  cmd->qd_des_knee[board_num] = 0.f;
  cmd->q_des_abad[board_num] = 0.f;
  cmd->q_des_hip[board_num] = 0.f;
  cmd->q_des_knee[board_num] = 0.f;
  cmd->tau_abad_ff[board_num] = 0.f;
  cmd->tau_hip_ff[board_num] = 0.f;
  cmd->tau_hip_ff[board_num] = 0.f;
  cmd->tau_knee_ff[board_num] = 0.f;
}

Here is the caller graph for this function:

void SpineBoard::resetData

(

)

Definition at line 17 of file SpineBoard.cpp.

References board_num, data, SpiData::flags, SpiData::q_abad, SpiData::q_hip, SpiData::q_knee, SpiData::qd_abad, SpiData::qd_hip, SpiData::qd_knee, and SpiData::spi_driver_status.

                           {
  if (data == nullptr) {
    printf(
        "[ERROR: SPINE board] reset_spine_board_data called when "
        "cheetahlcm_spi_data_t* was null\n");
    return;
  }

  data->flags[board_num] = 0;
  data->qd_abad[board_num] = 0.f;
  data->qd_hip[board_num] = 0.f;
  data->qd_knee[board_num] = 0.f;
  data->q_abad[board_num] = 0.f;
  data->q_hip[board_num] = 0.f;
  data->q_knee[board_num] = 0.f;
  data->spi_driver_status = 0;
}

Here is the caller graph for this function:

void SpineBoard::run

(

)

Check abad softstop ///

Check hip softstop ///

No knee softstop right now ///

Definition at line 62 of file SpineBoard.cpp.

References board_num, cmd, data, disabled_torque, SpiCommand::flags, iter_counter, SpiCommand::kd_abad, SpiCommand::kd_hip, SpiCommand::kd_knee, kd_softstop, SpiCommand::kp_abad, SpiCommand::kp_hip, SpiCommand::kp_knee, kp_softstop, max_torque, SpiData::q_abad, SpiCommand::q_des_abad, SpiCommand::q_des_hip, SpiCommand::q_des_knee, SpiData::q_hip, SpiData::q_knee, q_limit_n, q_limit_p, SpiData::qd_abad, SpiCommand::qd_des_abad, SpiCommand::qd_des_hip, SpiCommand::qd_des_knee, SpiData::qd_hip, SpiData::qd_knee, SpiCommand::tau_abad_ff, SpiCommand::tau_hip_ff, SpiCommand::tau_knee_ff, torque_out, and wimp_torque.

                     {
  iter_counter++;
  if (cmd == nullptr || data == nullptr) {
    printf(
        "[ERROR: SPINE board] run_spine_board_iteration called with null "
        "command or data!\n");
    torque_out[0] = 0.f;
    torque_out[1] = 0.f;
    torque_out[2] = 0.f;
    return;
  }

  if (data->q_abad[board_num] > q_limit_p[0]) {
    torque_out[0] = kp_softstop * (q_limit_p[0] - data->q_abad[board_num]) -
                    kd_softstop * (data->qd_abad[board_num]) +
                    cmd->tau_abad_ff[board_num];
  } else if (data->q_abad[board_num] < q_limit_n[0]) {
    torque_out[0] = kp_softstop * (q_limit_n[0] - data->q_abad[board_num]) -
                    kd_softstop * (data->qd_abad[board_num]) +
                    cmd->tau_abad_ff[board_num];
  } else {
    torque_out[0] = cmd->kp_abad[board_num] *
                        (cmd->q_des_abad[board_num] - data->q_abad[board_num]) +
                    cmd->kd_abad[board_num] * (cmd->qd_des_abad[board_num] -
                                               data->qd_abad[board_num]) +
                    cmd->tau_abad_ff[board_num];
  }

  if (data->q_hip[board_num] > q_limit_p[1]) {
    torque_out[1] = kp_softstop * (q_limit_p[1] - data->q_hip[board_num]) -
                    kd_softstop * (data->qd_hip[board_num]) +
                    cmd->tau_hip_ff[board_num];
  } else if (data->q_hip[board_num] < q_limit_n[1]) {
    torque_out[1] = kp_softstop * (q_limit_n[1] - data->q_hip[board_num]) -
                    kd_softstop * (data->qd_hip[board_num]) +
                    cmd->tau_hip_ff[board_num];
  } else {
    torque_out[1] = cmd->kp_hip[board_num] *
                        (cmd->q_des_hip[board_num] - data->q_hip[board_num]) +
                    cmd->kd_hip[board_num] *
                        (cmd->qd_des_hip[board_num] - data->qd_hip[board_num]) +
                    cmd->tau_hip_ff[board_num];
  }

  torque_out[2] = cmd->kp_knee[board_num] *
                      (cmd->q_des_knee[board_num] - data->q_knee[board_num]) +
                  cmd->kd_knee[board_num] *
                      (cmd->qd_des_knee[board_num] - data->qd_knee[board_num]) +
                  cmd->tau_knee_ff[board_num];

  const float* torque_limits = disabled_torque;

  if (cmd->flags[board_num] & 0b1) {
    if (cmd->flags[board_num] & 0b10)
      torque_limits = wimp_torque;
    else
      torque_limits = max_torque;
  }

  for (int i = 0; i < 3; i++) {
    if (torque_out[i] > torque_limits[i]) torque_out[i] = torque_limits[i];
    if (torque_out[i] < -torque_limits[i]) torque_out[i] = -torque_limits[i];
  }

  //    if(! (iter_counter % 100))
  //    {
  //        printf("SPINE BOARD %d: flags: 0x%hx, torque out: %.3f %.3f %.3f\n",
  //        board_num, cmd->flags[board_num],
  //               torque_out[0], torque_out[1], torque_out[2]);
  //    }
}

Member Data Documentation

s32 SpineBoard::board_num

private

Definition at line 58 of file SpineBoard.h.

SpiCommand* SpineBoard::cmd = nullptr

Definition at line 52 of file SpineBoard.h.

SpiData* SpineBoard::data = nullptr

Definition at line 53 of file SpineBoard.h.

const float SpineBoard::disabled_torque[3] = {0.f, 0.f, 0.f}

private

Definition at line 61 of file SpineBoard.h.

s32 SpineBoard::iter_counter = 0

private

Definition at line 66 of file SpineBoard.h.

const float SpineBoard::kd_softstop = 0.4f

private

Definition at line 65 of file SpineBoard.h.

const float SpineBoard::kp_softstop = 100.f

private

Definition at line 64 of file SpineBoard.h.

const float SpineBoard::max_torque[3] = {17.f, 17.f, 26.f}

private

Definition at line 59 of file SpineBoard.h.

const float SpineBoard::q_limit_n[3] = {-1.5f, -5.0f, 0.f}

private

Definition at line 63 of file SpineBoard.h.

const float SpineBoard::q_limit_p[3] = {1.5f, 5.0f, 0.f}

private

Definition at line 62 of file SpineBoard.h.

float SpineBoard::side_sign

private

Definition at line 57 of file SpineBoard.h.

float SpineBoard::torque_out[3]

Definition at line 54 of file SpineBoard.h.

const float SpineBoard::wimp_torque[3] = {6.f, 6.f, 6.f}

private

Definition at line 60 of file SpineBoard.h.

---

The documentation for this class was generated from the following files:

• SpineBoard.h
• SpineBoard.cpp