Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F73540124
motorcontrol.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
Mon, Jul 22, 12:25
Size
25 KB
Mime Type
text/x-c
Expires
Wed, Jul 24, 12:25 (2 d)
Engine
blob
Format
Raw Data
Handle
19218547
Attached To
R6619 Oncilla Motordriver Firmware
motorcontrol.c
View Options
#include <p33FJ128MC804.h>
#include "motorcontrol.h"
#include "misc.h"
#include "pindefs.h"
#include "config.h"
#include "timer.h"
#include "error.h"
#include "control_table.h"
// smooth position control mode: allowed jitter between two speed update commands
// expressed in times the expected update period
// jitter of 0 means now jitter allowed, jitter of 2 means 3 times the normal update rate is allowed
#define MAX_JITTER 3
// create 2 motor structures
motor
M1
,
M2
;
///////////////////////////////////////////////////////////////////////////////
// timing critical part
// estimate current speed and acceleration of motor and update the position counter
/*void update_position_and_speed(motor *M, int pos_cnt) {
int tmp_speed;
//calculate speed without resetting POSxCNT registers -> eliminates incremental errors
tmp_speed = pos_cnt - M->previous_cnt;
M->current_accel = tmp_speed - M->current_speed;
M->current_speed = tmp_speed;
M->previous_cnt = pos_cnt;
M->pos_cnt += M->current_speed;
}*/
///////////////////////////////////////////////////////////////////////////////
void
init_motor
(
motor
*
M
)
{
// qei position parameters
M
->
previous_cnt
=
0
;
M
->
pos_cnt
=
0
;
// move controlling parameters
M
->
goal_pos
=
0
;
M
->
set_speed
=
0
;
// internally controlled variables
M
->
moving_pos
=
0
;
M
->
moving_speed
=
0
;
M
->
current_speed
=
0
;
M
->
current_accel
=
0
;
M
->
resolution_error
=
0
;
M
->
moving_en
=
0
;
// pid control in the "stiff" region
M
->
speed_pgain
=
0
;
M
->
speed_igain
=
0
;
M
->
speed_dgain
=
0
;
M
->
i_error
=
0
;
M
->
prev_p_error
=
0
;
M
->
update_i_error
=
FALSE
;
// compliant mode, "damped spring" dynamics
M
->
compliance_preload
=
0
;
M
->
compliance_gain
=
0
;
M
->
compliance_damping
=
0
;
M
->
max_trq
=
0
;
// control output parameters
M
->
output_trq
=
0
;
// motor limits
M
->
pos_limit_CW
=
0
;
M
->
pos_limit_CCW
=
0
;
M
->
max_speed
=
0
;
M
->
max_accel
=
0
;
}
void
M1_load_control_table_settings
(
void
)
{
// move controlling parameters
M1
.
goal_pos
=
M1_EXT_POS_2_INT_POS
((
long
)
control_table
[
A_MDRV_HIP_GOAL_POSITION
].
i
);
M1
.
set_speed
=
M1_EXT_SPEED_2_INT_SPEED
((
long
)
control_table
[
A_MDRV_HIP_GOAL_SPEED
].
i
);
M1
.
moving_en
=
0
;
// disable moving
// pid control in the "stiff" region
M1
.
speed_pgain
=
control_table
[
A_MDRV_HIP_PID_PGAIN
].
i
;
M1
.
speed_igain
=
control_table
[
A_MDRV_HIP_PID_IGAIN
].
i
;
M1
.
speed_dgain
=
control_table
[
A_MDRV_HIP_PID_DGAIN
].
i
;
// reset internal PID variables
M1
.
i_error
=
0
;
M1
.
prev_p_error
=
0
;
M1
.
update_i_error
=
FALSE
;
// compliant mode, "damped spring" dynamics
M1
.
compliance_preload
=
control_table
[
A_MDRV_HIP_SPRING_PRELOAD
].
i
;
M1
.
compliance_gain
=
M1_EXT_TRQ_2_INT_TRQ
(
control_table
[
A_MDRV_HIP_SPRING_FORCE
].
i
);
M1
.
compliance_damping
=
control_table
[
A_MDRV_HIP_SPRING_DAMPING
].
i
;
M1
.
max_trq
=
(
unsigned
int
)
control_table
[
A_MDRV_HIP_MAX_TORQUE
].
i
;
// motor limits
// M1.pos_limit_CW = M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_CW_ANGLE_LIMIT].i);
// M1.pos_limit_CCW = M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_CCW_ANGLE_LIMIT].i);
M1
.
max_speed
=
(
unsigned
int
)
M1_MAX_SPEED_CNT
;
M1
.
max_accel
=
M1_EXT_ACCEL_2_INT_ACCEL
(
control_table
[
A_MDRV_HIP_MAX_ACCELERATION
].
i
);
}
void
M2_load_control_table_settings
(
void
)
{
// move controlling parameters
M2
.
goal_pos
=
M2_EXT_POS_2_INT_POS
((
long
)
control_table
[
A_MDRV_KNEE_GOAL_POSITION
].
i
);
M2
.
set_speed
=
M2_EXT_SPEED_2_INT_SPEED
((
long
)
control_table
[
A_MDRV_KNEE_GOAL_SPEED
].
i
);
M2
.
moving_en
=
0
;
// disable moving
// pid control in the "stiff" region
M2
.
speed_pgain
=
control_table
[
A_MDRV_KNEE_PID_PGAIN
].
i
;
M2
.
speed_igain
=
control_table
[
A_MDRV_KNEE_PID_IGAIN
].
i
;
M2
.
speed_dgain
=
control_table
[
A_MDRV_KNEE_PID_DGAIN
].
i
;
// reset internal PID variables
M2
.
i_error
=
0
;
M2
.
prev_p_error
=
0
;
M2
.
update_i_error
=
FALSE
;
// compliant mode, "damped spring" dynamics
M2
.
compliance_preload
=
control_table
[
A_MDRV_KNEE_SPRING_PRELOAD
].
i
;
M2
.
compliance_gain
=
M2_EXT_TRQ_2_INT_TRQ
(
control_table
[
A_MDRV_KNEE_SPRING_FORCE
].
i
);
M2
.
compliance_damping
=
control_table
[
A_MDRV_KNEE_SPRING_DAMPING
].
i
;
M2
.
max_trq
=
(
unsigned
int
)
control_table
[
A_MDRV_KNEE_MAX_TORQUE
].
i
;
// motor limits
// M2.pos_limit_CW = M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_CW_ANGLE_LIMIT].i);
// M2.pos_limit_CCW = M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_CCW_ANGLE_LIMIT].i);
M2
.
max_speed
=
(
unsigned
int
)
M2_MAX_SPEED_CNT
;
M2
.
max_accel
=
M2_EXT_ACCEL_2_INT_ACCEL
(
control_table
[
A_MDRV_KNEE_MAX_ACCELERATION
].
i
);
}
uint8
set_motor_limits
(
motor
*
M
,
long
pos_limit_CW
,
long
pos_limit_CCW
,
unsigned
int
max_speed
)
{
if
(
pos_limit_CW
<
pos_limit_CCW
)
return
MOTOR_LIMIT_SETTING_ERROR
;
M
->
pos_limit_CW
=
pos_limit_CW
;
M
->
pos_limit_CCW
=
pos_limit_CCW
;
M
->
max_speed
=
max_speed
;
return
NO_ERROR
;
}
// This mode has the goal to reach a certain position at the desired speed
// position in number of qei pulses, speed in pulses per interrupt-cycle
uint8
motor_position_control
(
motor
*
M
,
long
new_pos
,
int
new_speed
)
{
uint8
return_value
=
NO_ERROR
;
int
abs_new_speed
=
_ABS
(
new_speed
);
// limit speed to maximum allowed/possible speed
if
(
abs_new_speed
>
(
int
)
M
->
max_speed
)
{
abs_new_speed
=
(
int
)
M
->
max_speed
;
return_value
=
MOTOR_SPEED_OUT_OF_RANGE
;
}
// limit position
if
(
new_pos
>
M
->
pos_limit_CW
)
{
new_pos
=
M
->
pos_limit_CW
;
return_value
=
MOTOR_POSITION_OUT_OF_RANGE
;
}
else
if
(
new_pos
<
M
->
pos_limit_CCW
)
{
new_pos
=
M
->
pos_limit_CCW
;
return_value
=
MOTOR_POSITION_OUT_OF_RANGE
;
}
// disable movement during setting the parameters
M
->
moving_en
=
0
;
M
->
goal_pos
=
new_pos
;
// update goal position
if
(
new_pos
!=
M
->
moving_pos
)
{
// update speed settings
if
(
new_pos
>
M
->
moving_pos
)
{
// positive direction of movement
M
->
set_speed
=
abs_new_speed
;
}
else
{
// negative direction of movement
M
->
set_speed
=
-
abs_new_speed
;
}
// enable new movement
M
->
moving_en
=
1
;
}
return
return_value
;
}
// This mode computes motor speed based on the position that should be reached in the desired period of time
// Note that the goal position in this mode is the position limit
// (if the goal position would be the next position, the controller would decelerate towards this position)
// position in number of qei pulses, period time in number of system timer cycles
uint8
motor_speed_control
(
motor
*
M
,
int
speed
)
{
uint8
return_value
=
NO_ERROR
;
// limit speed to maximum allowed/possible speed
if
(
speed
>
M
->
max_speed
)
{
speed
=
M
->
max_speed
;
return_value
=
MOTOR_SPEED_OUT_OF_RANGE
;
}
else
if
(
speed
<
-
M
->
max_speed
)
{
speed
=
-
M
->
max_speed
;
return_value
=
MOTOR_SPEED_OUT_OF_RANGE
;
}
// disable movement during setting the parameters
M
->
moving_en
=
0
;
M
->
set_speed
=
speed
;
// check the direction of movement and set the desired position limit
if
(
M
->
set_speed
>
0
)
{
M
->
goal_pos
=
M
->
pos_limit_CW
;
if
(
M
->
moving_pos
<
M
->
goal_pos
)
M
->
moving_en
=
1
;
}
else
{
M
->
goal_pos
=
M
->
pos_limit_CCW
;
if
(
M
->
moving_pos
>
M
->
goal_pos
)
M
->
moving_en
=
1
;
}
// enable new movement
// M->moving_en = 1;
// Update control table
return
return_value
;
}
// This mode computes motor speed based on the position that should be reached in the desired period of time
// Note that the goal position in this mode is the position limit
// (if the goal position would be the next position, the controller would decelerate towards this position)
// position in number of qei pulses, period time in number of system timer cycles
uint8
motor_smooth_position_control
(
motor
*
M
,
long
next_pos
,
long
period
)
{
uint8
return_value
=
NO_ERROR
;
int
new_speed
;
// compute new desired speed, clamp within the range of integers
new_speed
=
(
int
)
_MAX
(
-
32768
,
_MIN
(
32767
,
(
next_pos
-
M
->
moving_pos
)
*
(
long
)(
1
<<
SPEED_SHIFT
)
/
period
));
// limit speed to maximum allowed/possible speed
if
(
new_speed
>
M
->
max_speed
)
{
new_speed
=
M
->
max_speed
;
return_value
=
MOTOR_SPEED_OUT_OF_RANGE
;
}
else
if
(
new_speed
<
-
M
->
max_speed
)
{
new_speed
=
-
M
->
max_speed
;
return_value
=
MOTOR_SPEED_OUT_OF_RANGE
;
}
// disable movement during setting the parameters
M
->
moving_en
=
0
;
M
->
set_speed
=
new_speed
;
M
->
goal_pos
=
M
->
moving_pos
+
(
next_pos
-
M
->
moving_pos
)
*
(
long
)(
MAX_JITTER
+
1
);
// limit position
if
(
M
->
goal_pos
>
M
->
pos_limit_CW
)
{
M
->
goal_pos
=
M
->
pos_limit_CW
;
// return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
else
if
(
M
->
goal_pos
<
M
->
pos_limit_CCW
)
{
M
->
goal_pos
=
M
->
pos_limit_CCW
;
// return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
// update goal position
if
(
M
->
goal_pos
!=
M
->
moving_pos
)
{
// enable new movement
M
->
moving_en
=
1
;
}
// Update control table
return
return_value
;
}
///////////////////////////////////////////////////////////////////////////////
void
motorcontrol_main
()
{
// LED_TX_ENABLE;
update_moving_position
(
&
M1
);
update_moving_position
(
&
M2
);
M1_compute_output
();
M2_compute_output
();
motorcontrol_main_update_control_table
();
// LED_TX_DISABLE;
}
void
motorcontrol_main_update_control_table
(
void
)
{
// update control table parameters that may have changed after executing the main control loop
if
(
!
M1
.
moving_en
)
// clear moving status flag
control_table_AND_mask
(
A_MDRV_HIP_MOTOR_CONTROL_MODE
,
~
(
STATUS_MOTOR_MOVING
));
if
(
!
M2
.
moving_en
)
// clear moving status flag
control_table_AND_mask
(
A_MDRV_KNEE_MOTOR_CONTROL_MODE
,
~
(
STATUS_MOTOR_MOVING
));
set_control_table_volatile
(
A_MDRV_HIP_PRESENT_DESIRED_POSITION
,
(
int
)
M1_INT_POS_2_EXT_POS
(
M1
.
moving_pos
));
set_control_table_volatile
(
A_MDRV_HIP_PRESENT_POSITION
,
(
int
)
M1_INT_POS_2_EXT_POS
(
M1
.
pos_cnt
));
set_control_table_volatile
(
A_MDRV_KNEE_PRESENT_DESIRED_POSITION
,
(
int
)
M2_INT_POS_2_EXT_POS
(
M2
.
moving_pos
));
set_control_table_volatile
(
A_MDRV_KNEE_PRESENT_POSITION
,
(
int
)
M2_INT_POS_2_EXT_POS
(
M2
.
pos_cnt
));
set_control_table_volatile
(
A_MDRV_HIP_PRESENT_DESIRED_SPEED
,
(
int
)
M1_INT_SPEED_2_EXT_SPEED
(
M1
.
moving_speed
));
set_control_table_volatile
(
A_MDRV_HIP_PRESENT_SPEED
,
(
int
)
M1_INT_SPEED_2_EXT_SPEED
(
M1
.
current_speed
));
set_control_table_volatile
(
A_MDRV_KNEE_PRESENT_DESIRED_SPEED
,
(
int
)
M2_INT_SPEED_2_EXT_SPEED
(
M2
.
moving_speed
));
set_control_table_volatile
(
A_MDRV_KNEE_PRESENT_SPEED
,
(
int
)
M2_INT_SPEED_2_EXT_SPEED
(
M2
.
current_speed
));
set_control_table_volatile
(
A_MDRV_HIP_PRESENT_TORQUE
,
M1
.
output_trq
);
set_control_table_volatile
(
A_MDRV_KNEE_PRESENT_TORQUE
,
M2
.
output_trq
);
}
///////////////////////////////////////////////////////////////////////////////
// Compute the reference position and speed of the motorshaft, based on the set_speed and acceleration settings
void
update_moving_position
(
motor
*
M
)
{
int
max_accel
;
// The desired position moves according to the speed and acceleration settings...
if
(
M
->
moving_en
)
{
// if no acceleration curve is used, then moving_speed is just the set_speed
// M->moving_speed = M->set_speed;
// => Acceleration curve computation
// compute when to switch off the set-speed in order to start descelerating such that zero speed is reached at the goal position
// maximum acceleration must not exceed the current moving speed (for correctly computing the speed auto-switch-off position)
max_accel
=
_MIN
(
_ABS
(
M
->
moving_speed
),
(
int
)
M
->
max_accel
);
// auto-switching off the set_speed should only be done if the current speed has the same direction as the set_speed
if
((
M
->
moving_speed
>
0
&&
M
->
set_speed
>
0
)
||
(
M
->
moving_speed
<
0
&&
M
->
set_speed
<
0
))
{
// if ((long)M->moving_speed*(long)M->set_speed > 0) {
// if (_ABS(M->moving_pos-M->goal_pos) < ((long)_ABS(M->moving_speed)*((_ABS(M->moving_speed-1)/max_accel)+1+1))/(2*(1<<SPEED_SHIFT))) {
// if ((_ABS(M->moving_pos-M->goal_pos)*(long)max_accel*(1<<SPEED_SHIFT)*2) < ((long)_ABS(M->moving_speed)*(_ABS(M->moving_speed) + max_accel-1))) {
if
((
_ABS
(
M
->
moving_pos
-
M
->
goal_pos
)
*
(
long
)
max_accel
)
<
((
long
)
_ABS
(
M
->
moving_speed
)
*
((
long
)
_ABS
(
M
->
moving_speed
)
+
(
long
)
max_accel
-
1
))
/
((
long
)(
1
<<
SPEED_SHIFT
)
*
2
))
{
// we don't set the desired speed to zero, but to the minimum acceleration setting in order to guarantee convergence
if
(
M
->
set_speed
>
0
)
M
->
set_speed
=
_MIN
((
int
)
M
->
max_accel
,
M
->
set_speed
);
else
M
->
set_speed
=
-
_MIN
((
int
)
M
->
max_accel
,
-
M
->
set_speed
);
}
}
// compute new speed according to the acceleration parameter
if
(
M
->
moving_speed
!=
M
->
set_speed
)
{
// LED_TX_ENABLE;
if
(
M
->
moving_speed
<
M
->
set_speed
)
{
M
->
moving_speed
+=
(
int
)
M
->
max_accel
;
if
(
M
->
moving_speed
>
M
->
set_speed
)
{
M
->
moving_speed
=
M
->
set_speed
;
}
}
else
{
M
->
moving_speed
-=
(
int
)
M
->
max_accel
;
if
(
M
->
moving_speed
<
M
->
set_speed
)
{
M
->
moving_speed
=
M
->
set_speed
;
}
}
}
// else LED_TX_DISABLE;
// compute new desired position for the current step
M
->
moving_pos
+=
M
->
moving_speed
>>
SPEED_SHIFT
;
// compensate for the bit errors due to the shifting action
M
->
resolution_error
+=
(
M
->
moving_speed
-
((
M
->
moving_speed
>>
SPEED_SHIFT
)
<<
SPEED_SHIFT
));
if
(
M
->
resolution_error
>
(
1
<<
SPEED_SHIFT
))
{
M
->
resolution_error
-=
(
1
<<
SPEED_SHIFT
);
M
->
moving_pos
+=
1
;
}
// check if the goal position is reached
if
(((
M
->
set_speed
>
0
)
&&
(
M
->
moving_pos
>=
M
->
goal_pos
))
||
((
M
->
set_speed
<
0
)
&&
(
M
->
moving_pos
<=
M
->
goal_pos
))
)
{
M
->
moving_pos
=
M
->
goal_pos
;
M
->
moving_speed
=
0
;
M
->
moving_en
=
0
;
// disable stepping when goal position is reached
// LED_TX_DISABLE;
}
}
}
//long prev_p_error = 0;
// compute new speed and torque settings
void
M1_compute_output
(
void
)
{
long
desired_speed
;
long
p_error
,
d_error
;
long
pid_output
;
unsigned
int
speed_output
;
unsigned
int
scaled_speed_output
;
long
i_max
;
long
required_trq
;
// ***************** Speed control section: velocity error *****************
desired_speed
=
M1
.
moving_pos
-
M1
.
pos_cnt
;
// compute new PID output
p_error
=
(
long
)
M1
.
current_speed
-
desired_speed
;
if
(
M1
.
update_i_error
)
M1
.
i_error
+=
p_error
;
// no integrate action in compliant mode
d_error
=
p_error
-
M1
.
prev_p_error
;
// if (M1.update_i_error) d_error = p_error - M1.prev_p_error; else d_error = 0;
pid_output
=
((
p_error
*
(
long
)
M1
.
speed_pgain
)
+
(
M1
.
i_error
*
(
long
)
M1
.
speed_igain
)
+
(
d_error
*
(
long
)
M1
.
speed_dgain
))
/
(
long
)
1024
;
M1
.
prev_p_error
=
p_error
;
// anti-windup
i_max
=
((
long
)(
MAX_PWM
-
1
)
*
(
long
)
1024
)
/
(
M1
.
speed_igain
*
2
);
if
(
M1
.
i_error
>
i_max
)
M1
.
i_error
=
i_max
;
else
if
(
M1
.
i_error
<
-
i_max
)
M1
.
i_error
=
-
i_max
;
// clamp output
speed_output
=
(
_ABS
(
pid_output
)
>
(
long
)(
MAX_PWM
-
1
))
?
(
unsigned
int
)(
MAX_PWM
-
1
)
:
(
unsigned
int
)
_ABS
(
pid_output
);
// set pwm output
M1_SET_PWM
(
speed_output
);
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
M1
.
output_trq
=
(((
desired_speed
)
*
(
long
)
M1
.
compliance_gain
)
/
512
)
-
(((
long
)(
M1
.
current_speed
-
(
long
)(
M1
.
moving_speed
>>
SPEED_SHIFT
))
*
(
long
)
M1
.
compliance_damping
)
/
64
);
// Add the spring preload term
if
(
desired_speed
>
0
)
M1
.
output_trq
+=
M1
.
compliance_preload
;
else
M1
.
output_trq
-=
M1
.
compliance_preload
;
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
required_trq
=
M1
.
output_trq
+
(
long
)
M1
.
current_accel
*
(
long
)
M1_INERTIA_COMP
/
64
;
// scale speed to torque dimensions
scaled_speed_output
=
(
long
)((
long
)
speed_output
*
MAX_DUTY
)
/
(
long
)
MAX_PWM
;
// Set direction of torque, depending on which factor is smallest
// if ((long)_ABS(required_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
if
((
unsigned
int
)
_ABS
(
M1
.
compliance_preload
)
<
scaled_speed_output
)
{
M1
.
update_i_error
=
0
;
if
(
required_trq
>
0
)
DIR1
=
0
;
else
DIR1
=
1
;
if
(
M1
.
compliance_gain
>
500
)
// problematic switching between fast and slow decay for very stiff spring settings
PWM_TRQ1
=
M1_trq_to_pwm
(
_ABS
(
required_trq
),
TRUE
);
else
PWM_TRQ1
=
M1_trq_to_pwm
(
_ABS
(
required_trq
),
FALSE
);
// LED_TX_ENABLE;
}
else
{
M1
.
update_i_error
=
1
;
// if (speed_output > 2)
if
(
pid_output
<
0
)
DIR1
=
0
;
else
DIR1
=
1
;
// force fast decay mode
// if (scaled_speed_output*4 < (unsigned int)_ABS(M1.compliance_preload))
// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
// else
// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output, TRUE);
PWM_TRQ1
=
M1_trq_to_pwm
(
_ABS
(
required_trq
),
TRUE
);
// LED_TX_DISABLE;
}
}
// compute new speed and torque settings
void
M2_compute_output
(
void
)
{
long
desired_speed
;
long
p_error
,
d_error
;
long
pid_output
;
unsigned
int
speed_output
;
unsigned
int
scaled_speed_output
;
long
i_max
;
long
required_trq
;
// ***************** Speed control section: velocity error *****************
desired_speed
=
M2
.
moving_pos
-
M2
.
pos_cnt
;
// compute new PID output
p_error
=
(
long
)
M2
.
current_speed
-
desired_speed
;
if
(
M2
.
update_i_error
)
M2
.
i_error
+=
p_error
;
// no integrate action in compliant mode
d_error
=
p_error
-
M2
.
prev_p_error
;
// if (M2.update_i_error) d_error = p_error - M2.prev_p_error; else d_error = 0;
pid_output
=
((
p_error
*
(
long
)
M2
.
speed_pgain
)
+
(
M2
.
i_error
*
(
long
)
M2
.
speed_igain
)
+
(
d_error
*
(
long
)
M2
.
speed_dgain
))
/
(
long
)
1024
;
M2
.
prev_p_error
=
p_error
;
// anti-windup
i_max
=
((
long
)(
MAX_PWM
-
1
)
*
(
long
)
1024
)
/
(
M2
.
speed_igain
*
2
);
if
(
M2
.
i_error
>
i_max
)
M2
.
i_error
=
i_max
;
else
if
(
M2
.
i_error
<
-
i_max
)
M2
.
i_error
=
-
i_max
;
// clamp output
speed_output
=
(
_ABS
(
pid_output
)
>
(
long
)(
MAX_PWM
-
1
))
?
(
unsigned
int
)(
MAX_PWM
-
1
)
:
(
unsigned
int
)
_ABS
(
pid_output
);
// set pwm output
M2_SET_PWM
(
speed_output
);
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
M2
.
output_trq
=
(((
desired_speed
)
*
(
long
)
M2
.
compliance_gain
)
/
512
)
-
(((
long
)(
M2
.
current_speed
-
(
long
)(
M2
.
moving_speed
>>
SPEED_SHIFT
))
*
(
long
)
M2
.
compliance_damping
)
/
64
);
// add the spring preload term
if
(
desired_speed
>
0
)
M2
.
output_trq
+=
M2
.
compliance_preload
;
else
M2
.
output_trq
-=
M2
.
compliance_preload
;
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
required_trq
=
M2
.
output_trq
+
(
long
)
M2
.
current_accel
*
(
long
)
M2_INERTIA_COMP
/
64
;
// scale speed to torque dimensions
scaled_speed_output
=
(
long
)((
long
)
speed_output
*
TORQUE_RESOLUTION
)
/
(
long
)
MAX_PWM
;
// Set direction of torque, depending on which factor is smallest
// if ((long)_ABS(M2.output_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
if
((
unsigned
int
)
_ABS
(
M2
.
compliance_preload
)
<
scaled_speed_output
)
{
M2
.
update_i_error
=
0
;
if
(
required_trq
>
0
)
DIR2
=
0
;
else
DIR2
=
1
;
if
(
M2
.
compliance_gain
>
500
)
// problematic switching between fast and slow decay for very stiff spring settings
PWM_TRQ2
=
M2_trq_to_pwm
(
_ABS
(
required_trq
),
TRUE
);
else
PWM_TRQ2
=
M2_trq_to_pwm
(
_ABS
(
required_trq
),
FALSE
);
}
else
{
M2
.
update_i_error
=
1
;
// if (speed_output > 2)
if
(
pid_output
<
0
)
DIR2
=
0
;
else
DIR2
=
1
;
// force fast decay mode
// if (scaled_speed_output*4 < (unsigned int)_ABS(M2.compliance_preload))
// PWM_TRQ2 = M2_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
// else
// PWM_TRQ2 = M2_trq_to_pwm((long)scaled_speed_output, TRUE);
PWM_TRQ2
=
M2_trq_to_pwm
(
_ABS
(
required_trq
),
TRUE
);
}
}
/*
// compute new torque settings based on a damped spring system
void M2_compute_output(void) {
long speed_error;
long desired_speed;
int tmp;
// ***************** Output speed control section *****************
desired_speed = M2.moving_pos - M2.pos_cnt;
speed_error = (((long)M2.current_speed - desired_speed)*(long)M2.speed_pgain)/256;
// Set motor speed
tmp = (_ABS(speed_error)>(long)MAX_PWM) ? (int)MAX_PWM : (int)_ABS(speed_error);
M2_SET_PWM(tmp);
// M2_SET_PWM(MAX_PWM); // relying solely on the torque-control does not work in high-stiffness settings and generally results in more vibrations
// ***************** Output torque control section *****************
// torque = spring force - spring damping + rotor inertia compensation
M2.output_trq = (((desired_speed)*(long)M2.compliance_gain)/512)
- (((long)(M2.current_speed-(long)(M2.moving_speed>>SPEED_SHIFT))*(long)M2.compliance_damping)/64)
+ (long)M2.current_accel*(long)M2_INERTIA_COMP/64;
// Add the spring preload term
// if (desired_speed > 0) {
// M2.output_trq += M2.compliance_preload;
// }
// else {
// M2.output_trq -= M2.compliance_preload;
// }
if (desired_speed > 0) {
M2.output_trq += (M2.compliance_preload*tmp)/MAX_PWM;
}
else {
M2.output_trq -= (M2.compliance_preload*tmp)/MAX_PWM;
}
// Set direction of torque
if (M2.output_trq>0) DIR2=0; else DIR2=1;
// Set motor torque
PWM_TRQ2 = M2_trq_to_pwm(_ABS(M2.output_trq), FALSE);
}
*/
// torque value pased to this function is assumed being a 12-bit value, with 4096 being the maximum available torque
unsigned
int
M1_trq_to_pwm
(
long
desired_trq
,
uint8
force_fast_decay
)
{
unsigned
int
pwm_out
;
desired_trq
=
(
desired_trq
*
4096
)
/
MAX_DUTY
;
// clamp desired_trq within safe range
if
(
desired_trq
>
8192
)
{
desired_trq
=
8192
;
}
// desired torque is a 12-bit value
// translate to 12-bit pwm-output using a piece-wise linear function
// this function compensates for the non-linear pwm to torque relation
if
(
desired_trq
<
2
)
pwm_out
=
((
unsigned
int
)
desired_trq
)
*
215
;
else
if
(
desired_trq
<
40
)
pwm_out
=
((
unsigned
int
)
desired_trq
-
2
)
*
7
/
2
+
430
;
else
if
(
desired_trq
<
86
)
pwm_out
=
((
unsigned
int
)
desired_trq
-
40
)
*
3
/
2
+
563
;
else
pwm_out
=
((
unsigned
int
)
desired_trq
-
86
)
*
7
/
8
+
632
;
// full pwm scale is now 12-bit, convert to pwm-range
pwm_out
=
(
unsigned
int
)(((
unsigned
long
)
pwm_out
*
(
unsigned
long
)
MAX_DUTY
)
/
TORQUE_RESOLUTION
);
// saturate at maximum torque
if
((
M1
.
max_trq
>
((
MAX_DUTY
*
M1_SLOWFAST_THRES
)
/
256
))
&&
(
!
force_fast_decay
))
{
if
(
pwm_out
>
M1
.
max_trq
+
M1_SLOWFAST_COMP
)
pwm_out
=
M1
.
max_trq
+
M1_SLOWFAST_COMP
;
}
else
{
// motor remains in the fast decay regime
if
(
pwm_out
>
M1
.
max_trq
)
pwm_out
=
M1
.
max_trq
;
}
if
(
force_fast_decay
)
{
MODE1
=
FAST_DECAY
;
// LED_TX_DISABLE;
}
else
{
// choose appropriate decay mode
if
(
pwm_out
>
(
unsigned
int
)((
MAX_DUTY
*
M1_SLOWFAST_THRES
)
/
256
))
{
MODE1
=
SLOW_DECAY
;
// LED_TX_ENABLE;
}
else
if
(
pwm_out
<
(
unsigned
int
)((
MAX_DUTY
*
(
M1_SLOWFAST_THRES
-
10
))
/
256
))
{
// small hysteresis
MODE1
=
FAST_DECAY
;
// LED_TX_DISABLE;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if
(
MODE1
==
SLOW_DECAY
)
pwm_out
-=
(
unsigned
int
)
M1_SLOWFAST_COMP
;
return
pwm_out
;
}
// torque value pased to this function is assumed being a 12-bit value, with 4096 being the maximum available torque
unsigned
int
M2_trq_to_pwm
(
long
desired_trq
,
uint8
force_fast_decay
)
{
unsigned
int
pwm_out
;
desired_trq
=
(
desired_trq
*
4096
)
/
MAX_DUTY
;
// clamp desired_trq within safe range
if
(
desired_trq
>
8192
)
{
desired_trq
=
8192
;
}
// desired torque is a 12-bit value
// translate to 12-bit pwm-output using a piece-wise linear function
// this function compensates for the non-linear pwm to torque relation
if
(
desired_trq
<
2
)
pwm_out
=
((
unsigned
int
)
desired_trq
)
*
215
;
else
if
(
desired_trq
<
40
)
pwm_out
=
((
unsigned
int
)
desired_trq
-
2
)
*
7
/
2
+
430
;
else
if
(
desired_trq
<
86
)
pwm_out
=
((
unsigned
int
)
desired_trq
-
40
)
*
3
/
2
+
563
;
else
pwm_out
=
((
unsigned
int
)
desired_trq
-
86
)
*
7
/
8
+
632
;
// full pwm scale is now 12-bit, convert to pwm-range
pwm_out
=
(
unsigned
int
)(((
unsigned
long
)
pwm_out
*
(
unsigned
long
)
MAX_DUTY
)
/
TORQUE_RESOLUTION
);
// saturate at maximum torque
if
(
M2
.
max_trq
>
((
MAX_DUTY
*
M2_SLOWFAST_THRES
)
/
256
))
{
if
(
pwm_out
>
M2
.
max_trq
+
M2_SLOWFAST_COMP
)
pwm_out
=
M2
.
max_trq
+
M2_SLOWFAST_COMP
;
}
else
{
// motor remains in the fast decay regime
if
(
pwm_out
>
M2
.
max_trq
)
pwm_out
=
M2
.
max_trq
;
}
if
(
force_fast_decay
)
{
MODE2
=
FAST_DECAY
;
}
else
{
// choose appropriate decay mode
if
(
pwm_out
>
(
unsigned
int
)((
MAX_DUTY
*
M2_SLOWFAST_THRES
)
/
256
))
{
MODE2
=
SLOW_DECAY
;
}
else
if
(
pwm_out
<
(
unsigned
int
)((
MAX_DUTY
*
(
M2_SLOWFAST_THRES
-
10
))
/
256
))
{
// small hysteresis
MODE2
=
FAST_DECAY
;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if
(
MODE2
==
SLOW_DECAY
)
pwm_out
-=
(
unsigned
int
)
M2_SLOWFAST_COMP
;
return
pwm_out
;
}
Event Timeline
Log In to Comment