Homec4science
Diffusion Oncilla Motordriver Firmware (load-cell-good) Browse

oncilla-motordriver-fw/src/command.c
df700679f13cload-cell-good

Last Change
Raw File

command.c
View Options

/*
command.c
Copyright (C) 2011 Rico Moeckel <rico.moeckel at epfl dot ch>,
Michiel D'Haene <michiel.dhaene at gmail dot com>,
EPFL Biorobotics Laboratory (http://biorob.epfl.ch)
EPFL Ecole Polytechnique Federale de Lausanne (http://www.epfl.ch)
ADD LAB from Michiel
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/**
\defgroup command COMMAND
\brief Command handling.
This module is responsible for handling and execution of commands.
All commands that should be automatically executed when found in an SBCP packet
need to be registered to the array sbcp_cmd_tbl[].
\author Rico Moeckel, Michiel D'Haene
\version 1.0
\date 2011-04-14
*/
/*@{*/
/** \file command.c
\brief Command handling.
*/
#include "command.h"
#include <p33Fxxxx.h>
#include "config.h"
#include "sbcp.h"
#include "pindefs.h"
#include "motorcontrol.h"
#include "misc.h"
#include "control_table.h"
#include "error.h"
extern table_field control_table[A_MDRV_TABLE_SIZE];
extern uint8 control_table_mode[A_MDRV_TABLE_SIZE];
extern uint8 protected;
extern motor M1, M2;
/*************************************************************************
*
* VARIABLES AND STRUCTS
*
*************************************************************************/
/// \brief command table
///
/// Register your command functions according to their instruction number
/// in this table.
/// IMPORTAINT: commands have to be placed as strings in " ".
const tSBCP_CmdTbl sbcp_cmd_tbl[]= {
{command_firmware, "firmware version"}, ///< inquire name and version number of current firmware
{command_set_parameters, "set parameter"}, ///< set parameters in the control table
{command_get_parameters, "get parameter"}, ///< get parameters from the control table
{command_set_protection, "set protection"}, ///< set or clear protection of protected parameter fields. Default is protected
{command_calibrate, "calibrate"} ///< calibrate command, matches the current internal position count to the magnetic encoders position
};
const int SBCP_COMMAND_TABLE_LENGTH = (sizeof(sbcp_cmd_tbl)/sizeof(tSBCP_CmdTbl)); ///< stores number of entries in command table
#define MESSAGE_BUFFER DMA_PACKET_QUEUE_BUS_TX_BUFFER_CPU
/*
void init_message_frame(void) {
// first fields are the same for each message, and thus can be fixed in a general message frame
// each message is first copied to the ringbuffer, and thus the same message frame memory can be reused for each message
MESSAGE_BUFFER[SBCP_POS_HEADER] = SBCP_HEADER;
MESSAGE_BUFFER[SBCP_POS_CLASS] = SBCP_MY_CLASS;
MESSAGE_BUFFER[SBCP_POS_ID] = SBCP_MY_ID;
}
*/
/*
void sbcp_bus_send_empty_response_message(unsigned char error_code) {
empty_message_frame[SBCP_POS_INSTRUCTION] = error_code;
// position of checksum is the same as the start of the payload position
empty_message_frame[SBCP_POS_PAYLOAD_START] = ((unsigned char)SBCP_MY_CLASS+(unsigned char)SBCP_MY_ID)+error_code;
// sbcp_bus_send_message(empty_message_frame, 6);
}
unsigned int sbcp_bus_update_frame_for_standard_response_message(unsigned char payload_length) {
const unsigned int SBCP_POS_CHECKSUM = SBCP_POS_PAYLOAD_START + payload_length;
message_frame[SBCP_POS_PAYLOAD_LENGTH] = payload_length;
message_frame[SBCP_POS_INSTRUCTION] = NO_ERROR;
message_frame[SBCP_POS_CHECKSUM] = ((unsigned char)SBCP_MY_CLASS+(unsigned char)SBCP_MY_ID)+payload_length;
return SBCP_POS_CHECKSUM;
}*/
#define SBCP_PREPARE_RESPONSE_MESSAGE(payload_length,instruction) do{\
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_LENGTH] = payload_length;\
MESSAGE_BUFFER[SBCP_POS_INSTRUCTION] = instruction;\
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + payload_length] = \
SBCP_MY_CLASS + SBCP_MY_ID + payload_length + instruction;\
sbcp.bus_tx_flags = SBCP_TX_BUFFER_OCCUPIED;\
}while(0)
/*************************************************************************
*
* FUNCTIONS
*
*************************************************************************/
/** \brief Command firmware function
Function for reading the firmware name and version number as being stored
in the define SBCP_FIRMWARE_VERSION in file config.h via the SBCP protocol.
\param buffer:
pointer to buffer containing the received communication packet
\param cmd_string:
command string
\param packet_start:
index of the buffer where the packet starts (first byte of packet)
\param packet_end:
index of the buffer where the packet ends (last byte of packet)
\param parameter_start:
index of the buffer where the first parameter starts (first byte of parameter)
\return no returns
*/
void command_firmware(const unsigned int *buffer ) {
unsigned int SBCP_POS_CHECKSUM = 7;
//will mark that a new packet is ready
SBCP_PREPARE_RESPONSE_MESSAGE(2,NO_ERROR);
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START] = control_table[A_MDRV_FIRMWARE_VERSION].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_CHECKSUM] += control_table[A_MDRV_FIRMWARE_VERSION].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 1] = control_table[A_MDRV_FIRMWARE_VERSION].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_CHECKSUM] += control_table[A_MDRV_FIRMWARE_VERSION].c[LOW_BYTE];
//sbcp_bus_send_message(message_frame, 6 + 2);
}
/** \brief Command help function
Function dedicated to debugging.
\param buffer:
pointer to buffer containing the received communication packet
\param instruction:
instruction field
\param packet_start:
index of the buffer where the packet starts (first byte of packet)
\param packet_end:
index of the buffer where the packet ends (last byte of packet)
\param parameter_start:
index of the buffer where the first parameter starts (first byte of parameter)
\return no returns
*/
void command_help(const unsigned int *buffer)
{
SBCP_PREPARE_RESPONSE_MESSAGE(0,NO_ERROR);
}
void command_set_protection(const unsigned int *buffer)
{
if (buffer[SBCP_POS_PAYLOAD_LENGTH] != 1) {
if (control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_ERROR) {
SBCP_PREPARE_RESPONSE_MESSAGE(0,MESSAGE_UNEXPECTED_LENGTH);
}
return;
}
protected = buffer[SBCP_POS_PAYLOAD_START];
if (control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_WRITE) {
SBCP_PREPARE_RESPONSE_MESSAGE(0,NO_ERROR);
}
}
void command_calibrate(const unsigned int *buffer) {
int M1CCW_INT, M1CW_INT, M2CCW_INT, M2CW_INT;
int intBuff = 0; //100; // intBuff defines an extra value to prevent overshoot on the CCW limit
//get the correct value. If the motordriver board is calibrated, we take
//magnetic encoder data, otherwise, we use relative one
unsigned char error_code = NO_ERROR;
int absolute_Q1_encoder_value, absolute_Q2_encoder_value,absolute_Q3_encoder_value;
if(control_table[A_MDRV_CALIBRATION_STATUS].i == STATUS_CALIBRATION_INITIAL_PERFORMED) {
//don't forget to mask the encoder status bits in the control table
absolute_Q1_encoder_value = control_table[A_MDRV_ME_Q1_POSITION].u & 0x0fff;
absolute_Q2_encoder_value = control_table[A_MDRV_ME_Q2_POSITION].u & 0x0fff;
absolute_Q3_encoder_value = control_table[A_MDRV_ME_Q3_POSITION].u & 0x0fff;
} else {
// absolute encoder limit values are not yet set,
// so it doesn't make sence to use these values
// we just assume the current position to be in the half of the default
// allowed range
absolute_Q1_encoder_value = (control_table[A_MDRV_HIP_CW_ANGLE_LIMIT].i + control_table[A_MDRV_HIP_CCW_ANGLE_LIMIT].i)/2;
absolute_Q2_encoder_value = (control_table[A_MDRV_KNEE_CW_ANGLE_LIMIT].i + control_table[A_MDRV_KNEE_CCW_ANGLE_LIMIT].i)/2;
absolute_Q3_encoder_value = 0;
error_code = MOTOR_NOT_MECHANICALLY_CALIBRATED;
}
M1CCW_INT = absolute_Q1_encoder_value - control_table[A_MDRV_HIP_CCW_ANGLE_LIMIT].i;// + intBuff ;
M1CW_INT = control_table[A_MDRV_HIP_CW_ANGLE_LIMIT].i - absolute_Q1_encoder_value;
M2CCW_INT = absolute_Q2_encoder_value - control_table[A_MDRV_KNEE_CCW_ANGLE_LIMIT].i;// + intBuff ;
M2CW_INT = control_table[A_MDRV_KNEE_CW_ANGLE_LIMIT].i - absolute_Q2_encoder_value;
// if absolute encoder value rolls over (4095->0), add a full turn to the interval
if( M1CCW_INT < 0 )
M1CCW_INT +=4096;
if( M1CW_INT < 0 )
M1CW_INT +=4096;
if( M2CCW_INT < 0 )
M2CCW_INT +=4096;
if( M2CW_INT < 0 )
M2CW_INT +=4096;
// set relative encoder limits and current counter value
M1.pos_limit_CCW = 0;
M2.pos_limit_CCW = 0;
//get values for feedback
table_field hip_CCW_value;
hip_CCW_value.i = M1_INT_POS_2_EXT_POS(M1.pos_limit_CCW);
table_field knee_CCW_value;
knee_CCW_value.i = M2_INT_POS_2_EXT_POS(M2.pos_limit_CCW);
// critical section, disable timer 1 interrrupt
IEC0bits.T1IE = 0;
M1.pos_cnt = M1_EXT_POS_2_INT_POS(M1CCW_INT);//-intBuff);
M1.previous_cnt = M1.pos_cnt;
M1.goal_pos = M1.pos_cnt;
// reset moving parameters
M1.moving_en = 0;
M1.moving_pos = M1.pos_cnt;
POS1CNT = M1.previous_cnt;
table_field hip_CW_value;
hip_CW_value.i = M1CCW_INT + M1.pos_cnt;
M1.pos_limit_CW = M1_EXT_POS_2_INT_POS(M1CW_INT+M1.pos_cnt);
M2.pos_cnt = M2_EXT_POS_2_INT_POS(M2CCW_INT);//-intBuff);
M2.previous_cnt = M2.pos_cnt;
M2.goal_pos = M2.pos_cnt;
// reset moving parameters
M2.moving_en = 0;
M2.moving_pos = M2.pos_cnt;
POS2CNT = M2.previous_cnt;
table_field knee_CW_value;
knee_CW_value.i = M2CCW_INT + M2.pos_cnt;
M2.pos_limit_CW = M2_EXT_POS_2_INT_POS(M2CW_INT+M2.pos_cnt);
// end critical section, restore timer 1 interrupt
IEC0bits.T1IE = 1;
control_table[A_MDRV_ME_Q1_OFFSET].i = M1CCW_INT - intBuff - absolute_Q1_encoder_value;
control_table[A_MDRV_ME_Q2_OFFSET].i = M2CCW_INT - intBuff - absolute_Q2_encoder_value;
control_table[A_MDRV_ME_Q3_OFFSET].i = absolute_Q2_encoder_value - absolute_Q3_encoder_value;
//mark the motor as run-time calibrated and ready
control_table_AND_mask(A_MDRV_HIP_MOTOR_CONTROL_MODE ,
~(STATUS_MOTOR_RUNTIME_UNCALIBRATED));
control_table_AND_mask(A_MDRV_KNEE_MOTOR_CONTROL_MODE,
~(STATUS_MOTOR_RUNTIME_UNCALIBRATED));
//send the feedback
SBCP_PREPARE_RESPONSE_MESSAGE(14, error_code);
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START ] = hip_CW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += hip_CW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 1 ] = hip_CW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += hip_CW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 2 ] = hip_CCW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += hip_CCW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 3 ] = hip_CCW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += hip_CCW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 4 ] = control_table[A_MDRV_ME_Q1_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q1_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 5 ] = control_table[A_MDRV_ME_Q1_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q1_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 6 ] = knee_CW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += knee_CW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 7 ] = knee_CW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += knee_CW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 8 ] = knee_CCW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += knee_CCW_value.c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 9 ] = knee_CCW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += knee_CCW_value.c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 10 ] = control_table[A_MDRV_ME_Q2_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q2_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 11 ] = control_table[A_MDRV_ME_Q3_OFFSET].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q3_OFFSET].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 12 ] = control_table[A_MDRV_ME_Q3_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q3_OFFSET].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 13 ] = control_table[A_MDRV_ME_Q3_OFFSET].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] += control_table[A_MDRV_ME_Q3_OFFSET].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 14 ] &= 0x00ff; //just to clean overflows
}
void command_set_parameters( const unsigned int *buffer)
{
unsigned int i = 0;
unsigned char address;
uint8 error_code = NO_ERROR;
unsigned int payload_length = buffer[SBCP_POS_PAYLOAD_LENGTH];
// payload length should be multiple of 3: 1 address byte, and 2 bytes for the content of the 16-bit field
if ((error_code % 3) != 0) {
error_code = MESSAGE_UNEXPECTED_LENGTH;
}
// retract all parameters
while ((i < payload_length) && (error_code == NO_ERROR)) {
address = buffer[SBCP_POS_PAYLOAD_START + i];
if (address > A_MDRV_TABLE_SIZE)
error_code = PARAMETER_NOT_IN_RANGE;
else if ((control_table_mode[address] & SBCP_P_PROTECTED) && (protected == TRUE))
error_code = PARAMETER_PROTECTED;
else if (!(control_table_mode[address] & SBCP_P_WRITABLE))
error_code = PARAMETER_NOT_WRITABLE;
else {
control_table[address].c[HIGH_BYTE] = buffer[SBCP_POS_PAYLOAD_START + i + 1];
control_table[address].c[LOW_BYTE] = buffer[SBCP_POS_PAYLOAD_START + i + 2];
set_control_table(address, control_table[address].i);
error_code = check_for_action(address);
i += 3;
}
}
// check if error has occured
if (error_code != NO_ERROR) {
if (control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_ERROR) {
SBCP_PREPARE_RESPONSE_MESSAGE(0,error_code);
}
return;
}
// no error has occured
if (control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_WRITE) {
SBCP_PREPARE_RESPONSE_MESSAGE(0,NO_ERROR);
}
}
void command_get_parameters(const unsigned int *buffer)
{
unsigned int i = 0;
unsigned char address;
uint8 error_code = NO_ERROR;
// each field in the payload denotes an address location for which the content, i.e. 2 bytes, has to be sent to the master
const unsigned int payload_length = buffer[SBCP_POS_PAYLOAD_LENGTH];
SBCP_PREPARE_RESPONSE_MESSAGE(2 * payload_length,NO_ERROR);
const unsigned int SBCP_POS_CHECKSUM = 2 * payload_length + SBCP_POS_PAYLOAD_START;
while ((i<payload_length) && (error_code == NO_ERROR)) {
address = buffer[SBCP_POS_PAYLOAD_START + i];
if (address > A_MDRV_TABLE_SIZE)
error_code = PARAMETER_NOT_IN_RANGE;
else if (!(control_table_mode[address] & SBCP_P_READABLE))
error_code = PARAMETER_NOT_AVAILABLE;
else {
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 2*i] = control_table[address].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_CHECKSUM] += control_table[address].c[HIGH_BYTE];
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 2*i + 1] = control_table[address].c[LOW_BYTE];
MESSAGE_BUFFER[SBCP_POS_CHECKSUM] += control_table[address].c[LOW_BYTE];
i++;
}
}
// check if error has occured
if (error_code != NO_ERROR) {
if (control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_ERROR) {
// shorten message to the number of parameters that have been parsed before the error occured
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_LENGTH] = 2*i;
MESSAGE_BUFFER[SBCP_POS_INSTRUCTION] = error_code;
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_START + 2 * i] = MESSAGE_BUFFER[SBCP_POS_CHECKSUM] + error_code;
MESSAGE_BUFFER[SBCP_POS_PAYLOAD_LENGTH + 2 * i] -= 2 * (payload_length - i);
}
return;
}
// no error has occured
if (!(control_table[A_MDRV_STATUS_RETURN_LEVEL].i & STATUS_COM_RESPOND_TO_READ)) {
sbcp.bus_tx_flags = SBCP_TX_BUFFER_FREE; //cpu will never send the data !
}
}
// check if additional actions are required upon changing the (writable) parameter in the control table
uint8 check_for_action(unsigned char address) {
uint8 error_code = NO_ERROR;
switch (address) {
case A_MDRV_ID:
// re-init the message frames
// init_message_frame();
break;
// case A_MDRV_TIME_OUT_DELAY: break;
case A_MDRV_HIP_CW_ANGLE_LIMIT:
M1.pos_limit_CW = M1_EXT_POS_2_INT_POS((long)control_table[address].i); break;
case A_MDRV_HIP_CCW_ANGLE_LIMIT:
M1.pos_limit_CCW = M1_EXT_POS_2_INT_POS((long)control_table[address].i); break;
case A_MDRV_KNEE_CW_ANGLE_LIMIT:
M2.pos_limit_CW = M1_EXT_POS_2_INT_POS((long)control_table[address].i); break;
case A_MDRV_KNEE_CCW_ANGLE_LIMIT:
M2.pos_limit_CCW = M1_EXT_POS_2_INT_POS((long)control_table[address].i); break;
// case A_MDRV_SERVO1_CW_ANGLE_LIMIT: break;
// case A_MDRV_SERVO1_CCW_ANGLE_LIMIT: break;
// case A_MDRV_SERVO2_CW_ANGLE_LIMIT: break;
// case A_MDRV_SERVO2_CCW_ANGLE_LIMIT: break;
case A_MDRV_HIP_TORQUE_LIMIT:
M1.max_trq = (unsigned int)control_table[address].i; break;
case A_MDRV_KNEE_TORQUE_LIMIT: break;
M2.max_trq = (unsigned int)control_table[address].i; break;
// case A_MDRV_SERVO1_TORQUE_LIMIT: break;
// case A_MDRV_SERVO2_TORQUE_LIMIT: break;
// case A_MDRV_STATUS_RETURN_LEVEL: break;
// case A_MDRV_ALARM_SHUTDOWN: break;
case A_MDRV_HIP_MOTOR_CONTROL_MODE:
switch (control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i) {
case STATUS_MOTOR_RESET: MRST_ENABLE; break;
case STATUS_MOTOR_COAST: MRST_DISABLE; BRAKE1 = 1; COAST1 = 0; break;
case STATUS_MOTOR_BRAKE: MRST_DISABLE; BRAKE1 = 0; COAST1 = 1; break;
default: MRST_DISABLE; BRAKE1 = 1; COAST1 = 1; break;
}
M1.moving_en = FALSE;
break;
case A_MDRV_KNEE_MOTOR_CONTROL_MODE:
switch (control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i) {
case STATUS_MOTOR_RESET: MRST_ENABLE; break;
case STATUS_MOTOR_COAST: MRST_DISABLE; BRAKE2 = 1; COAST2 = 0; break;
case STATUS_MOTOR_BRAKE: MRST_DISABLE; BRAKE2 = 0; COAST2 = 1; break;
default: MRST_DISABLE; BRAKE2 = 1; COAST2 = 1; break;
}
M2.moving_en = FALSE;
break;
// case A_MDRV_SERVO1_MOTOR_CONTROL_MODE: break;
// case A_MDRV_SERVO2_MOTOR_CONTROL_MODE: break;
case A_MDRV_HIP_GOAL_POSITION:
switch (control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i & STATUS_MOTOR_NON_CRITICAL_STATE_MASK) {
case STATUS_MOTOR_POSITION_CONTROL: case STATUS_MOTOR_DIRECT_CONTROL:
error_code = motor_position_control(&M1, M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_GOAL_POSITION].i), M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_HIP_GOAL_SPEED].i) );
control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
case STATUS_MOTOR_SMOOTH_POSITION_CONTROL:
error_code = motor_smooth_position_control(&M1, M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_GOAL_POSITION].i), control_table[A_MDRV_POSITION_UPDATE_INTERVAL].i );
control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
}
break;
case A_MDRV_KNEE_GOAL_POSITION:
switch (control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i & STATUS_MOTOR_NON_CRITICAL_STATE_MASK) {
case STATUS_MOTOR_POSITION_CONTROL: case STATUS_MOTOR_DIRECT_CONTROL:
error_code = motor_position_control(&M2, M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_GOAL_POSITION].i), M2_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_KNEE_GOAL_SPEED].i) );
control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
case STATUS_MOTOR_SMOOTH_POSITION_CONTROL:
error_code = motor_smooth_position_control(&M2, M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_GOAL_POSITION].i), control_table[A_MDRV_POSITION_UPDATE_INTERVAL].i );
control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
}
break;
// case A_MDRV_SERVO1_GOAL_POSITION: break;
// case A_MDRV_SERVO2_GOAL_POSITION: break;
case A_MDRV_HIP_GOAL_SPEED:
switch (control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i & STATUS_MOTOR_NON_CRITICAL_STATE_MASK) {
case STATUS_MOTOR_DIRECT_CONTROL:
error_code = motor_position_control(&M1, M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_GOAL_POSITION].i), M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_HIP_GOAL_SPEED].i) );
control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
case STATUS_MOTOR_SPEED_CONTROL:
error_code = motor_speed_control(&M1, M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_HIP_GOAL_SPEED].i) );
control_table[A_MDRV_HIP_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
}
break;
case A_MDRV_KNEE_GOAL_SPEED:
switch (control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i & STATUS_MOTOR_NON_CRITICAL_STATE_MASK ) {
case STATUS_MOTOR_DIRECT_CONTROL:
error_code = motor_position_control(&M1, M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_GOAL_POSITION].i), M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_KNEE_GOAL_SPEED].i) );
control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
case STATUS_MOTOR_SPEED_CONTROL:
error_code = motor_speed_control(&M1, M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_KNEE_GOAL_SPEED].i) );
control_table[A_MDRV_KNEE_MOTOR_CONTROL_MODE].i |= STATUS_MOTOR_MOVING;
break;
}
break;
// case A_MDRV_SERVO1_GOAL_SPEED: break;
// case A_MDRV_SERVO2_GOAL_SPEED: break;
case A_MDRV_HIP_MAX_ACCELERATION:
M1.max_accel = M1_EXT_ACCEL_2_INT_ACCEL(control_table[A_MDRV_HIP_MAX_ACCELERATION].i);
break;
case A_MDRV_KNEE_MAX_ACCELERATION:
M2.max_accel = M2_EXT_ACCEL_2_INT_ACCEL(control_table[A_MDRV_KNEE_MAX_ACCELERATION].i);
break;
// case A_MDRV_SERVO1_MAX_ACCEL: break;
// case A_MDRV_SERVO2_MAX_ACCEL: break;
case A_MDRV_HIP_MAX_TORQUE:
if (control_table[A_MDRV_HIP_MAX_TORQUE].i < control_table[A_MDRV_HIP_TORQUE_LIMIT].i)
M1.max_trq = control_table[A_MDRV_HIP_MAX_TORQUE].i;
else
M1.max_trq = control_table[A_MDRV_HIP_TORQUE_LIMIT].i;
break;
case A_MDRV_KNEE_MAX_TORQUE:
if (control_table[A_MDRV_KNEE_MAX_TORQUE].i < control_table[A_MDRV_KNEE_TORQUE_LIMIT].i)
M2.max_trq = control_table[A_MDRV_KNEE_MAX_TORQUE].i;
else
M2.max_trq = control_table[A_MDRV_KNEE_TORQUE_LIMIT].i;
break;
// case A_MDRV_SERVO1_MAX_TORQUE: break;
// case A_MDRV_SERVO2_MAX_TORQUE: break;
case A_MDRV_HIP_SPRING_PRELOAD:
M1.compliance_preload = control_table[A_MDRV_HIP_SPRING_PRELOAD].i;
break;
case A_MDRV_HIP_SPRING_FORCE:
M1.compliance_gain = M1_EXT_TRQ_2_INT_TRQ(control_table[A_MDRV_HIP_SPRING_FORCE].i);
break;
case A_MDRV_HIP_SPRING_DAMPING:
M1.compliance_damping = M1_EXT_DAMP_2_INT_DAMP(control_table[A_MDRV_HIP_SPRING_DAMPING].i);
break;
case A_MDRV_KNEE_SPRING_PRELOAD:
M2.compliance_preload = control_table[A_MDRV_KNEE_SPRING_PRELOAD].i;
break;
case A_MDRV_KNEE_SPRING_FORCE:
M2.compliance_gain = M2_EXT_TRQ_2_INT_TRQ(control_table[A_MDRV_KNEE_SPRING_FORCE].i);
break;
case A_MDRV_KNEE_SPRING_DAMPING:
M2.compliance_damping = M2_EXT_DAMP_2_INT_DAMP(control_table[A_MDRV_KNEE_SPRING_DAMPING].i);
break;
case A_MDRV_HIP_PID_PGAIN:
M1.speed_pgain = control_table[A_MDRV_HIP_PID_PGAIN].i;
break;
case A_MDRV_HIP_PID_IGAIN:
M1.speed_igain = control_table[A_MDRV_HIP_PID_IGAIN].i;
break;
case A_MDRV_HIP_PID_DGAIN:
M1.speed_dgain = control_table[A_MDRV_HIP_PID_DGAIN].i;
break;
case A_MDRV_KNEE_PID_PGAIN:
M1.speed_pgain = control_table[A_MDRV_KNEE_PID_PGAIN].i;
break;
case A_MDRV_KNEE_PID_IGAIN:
M1.speed_igain = control_table[A_MDRV_KNEE_PID_IGAIN].i;
break;
case A_MDRV_KNEE_PID_DGAIN:
M1.speed_dgain = control_table[A_MDRV_KNEE_PID_DGAIN].i;
break;
// case A_MDRV_POSITION_UPDATE_INTERVAL: break;
case A_MDRV_LED:
if (control_table[A_MDRV_LED].i == 0) LED_DISABLE;
else LED_ENABLE;
break;
}
return error_code;
}
c4science · Help