Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix ENCODER_DENOMINATOR handling to make param units corresponding to hardware #104

Merged
merged 12 commits into from
May 1, 2019
Merged

Fix ENCODER_DENOMINATOR handling to make param units corresponding to hardware #104

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
9a87ee2
Fix ENCODER_DENOMINATOR handling to make param units correspond to ha…
at-wat Apr 5, 2019
ed0b21c
Fix internal variable type
at-wat Apr 5, 2019
d660db2
Fix joint angle calculation
at-wat Apr 5, 2019
2cfa10f
Update TORQUE_UNIT init
at-wat Apr 5, 2019
dc9f710
Add comments for unit conversion
at-wat Apr 8, 2019
ed59b2d
Tweak torque calculation
at-wat Apr 8, 2019
a5f3389
Update comments about parameter conversion
at-wat Apr 9, 2019
be46dc9
Verify parameter file version
at-wat Apr 24, 2019
b991e63
Add test case for ENCODE_DENOMINATOR parameter support verification
at-wat Apr 25, 2019
413c764
Fix motor number count
at-wat Apr 25, 2019
b3687b9
Merge branch 'master' into fix-encoder-denominator-param-usage
at-wat Apr 30, 2019
55fba18
Fix parameter validation for non-existing motor
at-wat May 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/param.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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