Skip to content

Commit

Permalink
Fix ENCODER_DENOMINATOR handling to make param units corresponding to…
Browse files Browse the repository at this point in the history 
… hardware (#104)

* Fix ENCODER_DENOMINATOR handling

* Fix internal variable type

* Add comments for unit conversion

* Verify parameter file version

* Add test case for ENCODE_DENOMINATOR parameter support verification
  • Loading branch information
@at-wat
at-wat authored May 1, 2019
1 parent a7aa2ae commit 490c36c
Show file tree
Hide file tree
Showing 6 changed files with 201 additions and 127 deletions.
2 changes: 1 addition & 1 deletion include/param.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ int state(YPSpur_state id);
void enable_state(YPSpur_state id);
void disable_state(YPSpur_state id);
double p(YPSpur_param id, enum motor_id motor);
double isset_p(YPSpur_param id, enum motor_id motor);
int isset_p(YPSpur_param id, enum motor_id motor);
double *pp(YPSpur_param id, enum motor_id motor);
ParametersPtr get_param_ptr();
int option(ParamOptions option);
Expand Down
140 changes: 71 additions & 69 deletions include/ypparam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -350,75 +350,76 @@ typedef enum
0, \
}

#define YP_PARAM_COMMENT \
{ \
"Parameter file version", \
"[Integer Nm/Nm] Fixed-point position of PC-MCU communication", \
"[Nm] Calculation fineness of torque control", \
"[Counts] PWM cycle", \
"[Counts/rev] Encoder specification", \
"Encoder type (2:2-phase incremental, 3:3-phase incremental)", \
"[V] Power source voltage", \
"[s] Velocity control cycle", \
"[in/out] Gear ratio", \
"[ohm] Motor internal resistance", \
"[Nm/A] Motor torque constant", \
"[rpm/V] Motor speed constant", \
"Motor type (0:DC, 3:3phase-AC)", \
"[rad] Offset angle of AC motor phase", \
"[m] Wheel radius", \
"[m] Right wheel radius", \
"[m] Left wheel radius", \
"[m] Tread", \
"[s] Trajectory control cycle", \
"[m/s] Maximum velocity", \
"[rad/s] Maximum angular velocity", \
"[m/ss] Maximum acceleration", \
"[rad/ss] Maximum angular acceleration", \
"[m/ss] Centrifugal acceleration limit", \
"[m/s / rad/s] Deacceleration factor of trajectory control", \
"[rad/ss / m] Feedback gain for distance error", \
"[rad/ss / rad] Feedback gain for angular error", \
"[rad/ss / rad/s] Feedback gain for angular velocity", \
"[m] Clipping value of line following control", \
"[1/s] PI control parameter Kp", \
"[1/ss] PI control parameter Ki", \
"[Nm] Motor maximum torque", \
"[Nm] Constant friction", \
"[Nm/(rad/s)] Viscous friction", \
"[Nm] Constant friction (neg-direction)", \
"[Nm/(rad/s)] Viscous friction (neg-direction)", \
"[Nm] Motor torque limit", \
"[rev] Maximum integrated error of PI control", \
"[Nm] Offset value of motor torque", \
"[kg] Robot weight", \
"[kgm^2] Robot moment of inertia", \
"[kgm^2] Rotor moment of inertia of motor", \
"[kgm^2] Tire moment of inertia", \
"[m] Robot size of front", \
"[m] Robot size of rear", \
"[m] Robot size of left", \
"[m] Robot size of right", \
"PWS parameter A", \
"PWS parameter B", \
"PWS parameter C", \
"PWS parameter D", \
"PWS parameter E", \
"PWS parameter F", \
"[m] Linear feedback area of stop command", \
"[rad] Linear feedback area of spin command", \
"[rad] Linear feedback area of wheel_ang command", \
"[rad] Minimum wheel angle (for wheel_angle command)", \
"[rad] Maximum wheel angle (for wheel_angle command)", \
"Used for vehicle control (0: false, 1: true)", \
"[s] Time-span to resend control mode (0: don't resend)", \
"Motor load inertia", \
"Motor load cross inertia", \
"Encoder count divider 2^x (e.g. 4 means divide by 16)", \
"Encoder count denominator (COUNT_REV/ENCODER_DENOMINATOR is encoder resolution)", \
"[rad] Index signal rising edge angle at CW rotation", \
"[rad] Index signal falling edge angle at CW rotation", \
"[in/out] Index signal gear ratio", \
#define YP_PARAM_COMMENT \
{ \
"Parameter file version", \
"[Integer Nm/Nm] Fixed-point position of PC-MCU communication", \
"[Nm] Calculation fineness of torque control", \
"[Counts] PWM cycle", \
"[Counts/rev] Encoder specification", \
"Encoder type (2:2-phase incremental, 3:3-phase incremental)", \
"[V] Power source voltage", \
"[s] Velocity control cycle", \
"[in/out] Gear ratio", \
"[ohm] Motor internal resistance", \
"[Nm/A] Motor torque constant", \
"[rpm/V] Motor speed constant", \
"Motor type (0:DC, 3:3phase-AC)", \
"[rad] Offset angle of AC motor phase", \
"[m] Wheel radius", \
"[m] Right wheel radius", \
"[m] Left wheel radius", \
"[m] Tread", \
"[s] Trajectory control cycle", \
"[m/s] Maximum velocity", \
"[rad/s] Maximum angular velocity", \
"[m/ss] Maximum acceleration", \
"[rad/ss] Maximum angular acceleration", \
"[m/ss] Centrifugal acceleration limit", \
"[m/s / rad/s] Deacceleration factor of trajectory control", \
"[rad/ss / m] Feedback gain for distance error", \
"[rad/ss / rad] Feedback gain for angular error", \
"[rad/ss / rad/s] Feedback gain for angular velocity", \
"[m] Clipping value of line following control", \
"[1/s] PI control parameter Kp", \
"[1/ss] PI control parameter Ki", \
"[Nm] Motor maximum torque", \
"[Nm] Constant friction", \
"[Nm/(rad/s)] Viscous friction", \
"[Nm] Constant friction (neg-direction)", \
"[Nm/(rad/s)] Viscous friction (neg-direction)", \
"[Nm] Motor torque limit", \
"[rev] Maximum integrated error of PI control", \
"[Nm] Offset value of motor torque", \
"[kg] Robot weight", \
"[kgm^2] Robot moment of inertia", \
"[kgm^2] Rotor moment of inertia of motor", \
"[kgm^2] Tire moment of inertia", \
"[m] Robot size of front", \
"[m] Robot size of rear", \
"[m] Robot size of left", \
"[m] Robot size of right", \
"PWS parameter A", \
"PWS parameter B", \
"PWS parameter C", \
"PWS parameter D", \
"PWS parameter E", \
"PWS parameter F", \
"[m] Linear feedback area of stop command", \
"[rad] Linear feedback area of spin command", \
"[rad] Linear feedback area of wheel_ang command", \
"[rad] Minimum wheel angle (for wheel_angle command)", \
"[rad] Maximum wheel angle (for wheel_angle command)", \
"Used for vehicle control (0: false, 1: true)", \
"[s] Time-span to resend control mode (0: don't resend)", \
"Motor load inertia", \
"Motor load cross inertia", \
"Encoder count divider 2^x (e.g. 4 means divide by 16)", \
"Encoder count denominator" \
" (COUNT_REV/ENCODER_DENOMINATOR is encoder resolution for one electrical revolution)", \
"[rad] Index signal rising edge angle at CW rotation", \
"[rad] Index signal falling edge angle at CW rotation", \
"[in/out] Index signal gear ratio", \
}

enum motor_id
Expand All @@ -442,6 +443,7 @@ enum motor_id
// clang-format on

#define YP_PARAM_REQUIRED_VERSION 4.0
#define YP_PARAM_SUPPORTED_VERSION 5.0

/* パラメータの有効・無効 */
/* control_state用　　 */
Expand Down
4 changes: 2 additions & 2 deletions src/control_vehicle.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,14 +189,14 @@ void apply_motor_speed(SpurUserParamsPtr spur)
{
v = (double)(16 * spur->wheel_vel_smooth[i] *
p(YP_PARAM_GEAR, i) * p(YP_PARAM_COUNT_REV, i) * p(YP_PARAM_CYCLE, i) /
(2 * M_PI * p(YP_PARAM_ENCODER_DENOMINATOR, i)));
(2 * M_PI));
parameter_set(PARAM_w_ref_highprec, i, v);
}
else
{
v = (double)(spur->wheel_vel_smooth[i] *
p(YP_PARAM_GEAR, i) * p(YP_PARAM_COUNT_REV, i) * p(YP_PARAM_CYCLE, i) /
(2 * M_PI * p(YP_PARAM_ENCODER_DENOMINATOR, i)));
(2 * M_PI));
parameter_set(PARAM_w_ref, i, v);
}
break;
Expand Down
10 changes: 5 additions & 5 deletions src/odometry.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,15 +179,16 @@ void odometry(OdometryPtr xp, short *cnt, short *pwm, double dt, double time)

/* 角速度計算 */
mvel[i] = 2.0 * M_PI *
((double)cnt_diff) * pow(2, p(YP_PARAM_ENCODER_DIV, i)) *
p(YP_PARAM_ENCODER_DENOMINATOR, i) / (p(YP_PARAM_COUNT_REV, i) * dt);
((double)cnt_diff) * pow(2, p(YP_PARAM_ENCODER_DIV, i)) /
(p(YP_PARAM_COUNT_REV, i) * dt);
wvel[i] = mvel[i] / p(YP_PARAM_GEAR, i);

/* トルク推定 */
volt[i] = (double)pwm[i] * p(YP_PARAM_VOLT, i) / (p(YP_PARAM_PWM_MAX, i) * (dt / p(YP_PARAM_CYCLE, i)));
vc[i] = (p(YP_PARAM_MOTOR_VC, i) / 60) * 2 * M_PI; // [rpm/V] => [(rad/s)/V]
// TC [Nm/A]
mtorque[i] = p(YP_PARAM_MOTOR_TC, i) * ((volt[i] - mvel[i] / vc[i]) / p(YP_PARAM_MOTOR_R, i));
mtorque[i] = (p(YP_PARAM_MOTOR_TC, i) * (volt[i] - mvel[i] / vc[i])) /
(p(YP_PARAM_MOTOR_R, i) * p(YP_PARAM_ENCODER_DENOMINATOR, i));
/* 摩擦補償の補償 */
if (wvel[i] > 0)
{
Expand Down Expand Up @@ -262,8 +263,7 @@ void process_int(
((unsigned int)xp->enc[id] << ((int)p(YP_PARAM_ENCODER_DIV, id))) & 0xFFFF;
const short enc_diff = (short)enc_div - (short)value;
const double ang_diff =
enc_diff * 2.0 * M_PI * p(YP_PARAM_ENCODER_DENOMINATOR, id) /
(p(YP_PARAM_COUNT_REV, id) * p(YP_PARAM_GEAR, id));
enc_diff * 2.0 * M_PI / (p(YP_PARAM_COUNT_REV, id) * p(YP_PARAM_GEAR, id));

const double index_ratio = p(YP_PARAM_INDEX_GEAR, id) / p(YP_PARAM_GEAR, id);
double ref_ang;
Expand Down
Loading

0 comments on commit 490c36c

Please sign in to comment.