Skip to content

Commit

Permalink
examples/foc: set direction when updating new setpoint
Browse files Browse the repository at this point in the history
  • Loading branch information
raiden00pl committed Nov 13, 2023
1 parent ac64c7d commit 987543a
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 10 deletions.
12 changes: 7 additions & 5 deletions examples/foc/foc_motor_b16.c
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@ static int foc_motor_torq(FAR struct foc_motor_b16_s *motor, uint32_t torq)
tmp1 = itob16(motor->envp->cfg->torqmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_INTF_SCALE), tmp1);

motor->torq.des = b16muli(tmp2, torq);
motor->torq.des = b16mulb16(motor->dir, b16muli(tmp2, torq));

return OK;
}
Expand All @@ -466,7 +466,7 @@ static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel)
tmp1 = itob16(motor->envp->cfg->velmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_INTF_SCALE), tmp1);

motor->vel.des = b16muli(tmp2, vel);
motor->vel.des = b16mulb16(motor->dir, b16muli(tmp2, vel));

return OK;
}
Expand All @@ -491,7 +491,7 @@ static int foc_motor_pos(FAR struct foc_motor_b16_s *motor, uint32_t pos)
tmp1 = itob16(motor->envp->cfg->posmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_INTF_SCALE), tmp1);

motor->pos.des = b16muli(tmp2, pos);
motor->pos.des = b16mulb16(motor->dir, b16muli(tmp2, pos));

return OK;
}
Expand Down Expand Up @@ -914,7 +914,9 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
motor->torq.set = b16mulb16(motor->dir, motor->torq.des);
/* Torque setpoint */

motor->torq.set = motor->torq.des;

q_ref = motor->torq.set;
d_ref = 0;
Expand All @@ -931,7 +933,7 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
/* Run velocity ramp controller */

ret = foc_ramp_run_b16(&motor->ramp,
motor->dir * motor->vel.des,
motor->vel.des,
motor->vel.now,
&motor->vel.set);
if (ret < 0)
Expand Down
12 changes: 7 additions & 5 deletions examples/foc/foc_motor_f32.c
Original file line number Diff line number Diff line change
Expand Up @@ -431,7 +431,7 @@ static int foc_motor_torq(FAR struct foc_motor_f32_s *motor, uint32_t torq)

/* Update motor torque destination */

motor->torq.des = (torq * SETPOINT_INTF_SCALE *
motor->torq.des = (motor->dir * torq * SETPOINT_INTF_SCALE *
motor->envp->cfg->torqmax / 1000.0f);

return OK;
Expand All @@ -449,7 +449,7 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)

/* Update motor velocity destination */

motor->vel.des = (vel * SETPOINT_INTF_SCALE *
motor->vel.des = (motor->dir * vel * SETPOINT_INTF_SCALE *
motor->envp->cfg->velmax / 1000.0f);

return OK;
Expand All @@ -467,7 +467,7 @@ static int foc_motor_pos(FAR struct foc_motor_f32_s *motor, uint32_t pos)

/* Update motor position destination */

motor->pos.des = (pos * SETPOINT_INTF_SCALE *
motor->pos.des = (motor->dir * pos * SETPOINT_INTF_SCALE *
motor->envp->cfg->posmax / 1000.0f);

return OK;
Expand Down Expand Up @@ -897,7 +897,9 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
motor->torq.set = motor->dir * motor->torq.des;
/* Torque setpoint */

motor->torq.set = motor->torq.des;

q_ref = motor->torq.set;
d_ref = 0.0f;
Expand All @@ -914,7 +916,7 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
/* Run velocity ramp controller */

ret = foc_ramp_run_f32(&motor->ramp,
motor->dir * motor->vel.des,
motor->vel.des,
motor->vel.now,
&motor->vel.set);
if (ret < 0)
Expand Down

0 comments on commit 987543a

Please sign in to comment.