Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F91703153
command.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Wed, Nov 13, 16:08
Size
24 KB
Mime Type
text/x-c
Expires
Fri, Nov 15, 16:08 (2 d)
Engine
blob
Format
Raw Data
Handle
22310622
Attached To
R6619 Oncilla Motordriver Firmware
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
;
}
Event Timeline
Log In to Comment