/**************************************************************************** * control/lib_motor.c * * Copyright (C) 2018 Gregory Nutt. All rights reserved. * Author: Mateusz Szafoni * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name NuttX nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #define POLE_CNTR_THR 0.0f /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: motor_openloop_init * * Description: * Initialize open-loop data * * Input Parameters: * op - (in/out) pointer to the openloop data structure * max - (in) maximum openloop speed * per - (in) period of the open-loop control * * Returned Value: * None * ****************************************************************************/ void motor_openloop_init(FAR struct openloop_data_s *op, float max, float per) { DEBUGASSERT(op != NULL); DEBUGASSERT(max > 0.0f); DEBUGASSERT(per > 0.0f); /* Reset openloop structure */ memset(op, 0, sizeof(struct openloop_data_s)); /* Initialize data */ op->max = max; op->per = per; } /**************************************************************************** * Name: motor_openloop * * Description: * One step of the open-loop control * * Input Parameters: * op - (in/out) pointer to the open-loop data structure * speed - (in) open-loop speed * dir - (in) rotation direction * * Returned Value: * None * ****************************************************************************/ void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir) { DEBUGASSERT(op != NULL); DEBUGASSERT(speed >= 0.0f); DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); float phase_step = 0.0f; /* REVISIT: what should we do if speed is greater than max open-loop speed? * Saturate speed or stop motor ? */ if (speed > op->max) { speed = op->max; } /* Get phase step */ phase_step = dir * speed * op->per; /* Update open-loop angle */ op->angle += phase_step; /* Normalize the open-loop angle to 0.0 - 2PI range */ angle_norm_2pi(&op->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX); } /**************************************************************************** * Name: motor_openloop_angle_get * * Description: * Get angle from open-loop controller * * Input Parameters: * op - (in/out) pointer to the open-loop data structure * * Returned Value: * Return angle from open-loop controller * ****************************************************************************/ float motor_openloop_angle_get(FAR struct openloop_data_s *op) { DEBUGASSERT(op != NULL); return op->angle; } /* In a multipolar electrical machines, mechanical angle is not equal * to mechanical angle which can be described as: * * electrical angle = (p) * mechanical angle * where p - number of poles pair * * * electrical * angle: * * poles = 4 * i = 0 1 2 3 0 1 2 3 0 * 2PI ______________________________________________ * /| /| /| /| /| /| /| /| /| * / | / | / | / | / | / | / | / | / | * / | / | / | / | / | / | / | / | / | * 0 /___|/___|/___|/___|/___|/___|/___|/___|/___|/ * . . . * mechanical . . . * angle: . . . * . . . * 2PI .__________________.___________________.______ * . o o * . o o o o * . o o o o * . o o o o o * 0 o__________________o___________________o______ * */ /**************************************************************************** * Name: motor_angle_init * * Description: * Initialize motor angle structure * * Input Parameters: * angle - (in/out) pointer to the motor angle strucutre * p - (in) number of the motor pole pairs * * Returned Value: * None * ****************************************************************************/ void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p) { DEBUGASSERT(angle != NULL); DEBUGASSERT(p > 0); /* Reset structure */ memset(angle, 0, sizeof(struct motor_angle_s)); /* Store pole pairs */ angle->p = p; angle->one_by_p = (float)1.0f/p; /* Initialize angle with 0.0 */ phase_angle_update(&angle->angle_el, 0.0f); } /**************************************************************************** * Name: motor_angle_e_update * * Description: * Update motor angle structure using electrical motor angle. * * Input Parameters: * angle - (in/out) pointer to the motor angle structure * angle_new - (in) new motor electrical angle in range <0.0, 2PI> * * Returned Value: * None * ****************************************************************************/ void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, float dir) { DEBUGASSERT(angle != NULL); DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX); DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); /* Check if we crossed electrical angle boundaries */ if (dir == DIR_CW && angle_new - angle->angle_el.angle < -POLE_CNTR_THR) { /* For CW direction - previous angle is greater than current angle */ angle->i += 1; } else if (dir == DIR_CCW && angle_new - angle->angle_el.angle > POLE_CNTR_THR) { /* For CCW direction - previous angle is lower than current angle */ angle->i -= 1; } /* Reset pole counter if needed */ if (angle->i >= angle->p) { angle->i = 0; } else if (angle->i < 0) { angle->i = angle->p-1; } /* Update electrical angle structure */ phase_angle_update(&angle->angle_el, angle_new); /* Calculate mechanical angle. * One electrical angle rotation is equal to one mechanical rotation divided * by number of motor pole pairs. */ angle->anglem = (MOTOR_ANGLE_E_RANGE * angle->i + angle->angle_el.angle) * angle->one_by_p; /* Normalize mechanical angle to <0, 2PI> and store */ angle_norm_2pi(&angle->anglem, MOTOR_ANGLE_M_MIN, MOTOR_ANGLE_M_MAX); } /**************************************************************************** * Name: motor_angle_m_update * * Description: * Update motor angle structure using mechanical motor angle * * Input Parameters: * angle - (in/out) pointer to the motor angle structure * angle_new - (in) new motor mechanical angle in range <0.0, 2PI> * * Returned Value: * None * ****************************************************************************/ void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new, float dir) { DEBUGASSERT(angle != NULL); DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX); DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); float angle_el = 0.0f; /* Store new mechanical angle */ angle->anglem = angle_new; /* Update pole counter */ angle->i = (uint8_t)(angle->anglem * angle->p/MOTOR_ANGLE_M_MAX); /* Get electrical angle */ angle_el = angle->anglem * angle->p - MOTOR_ANGLE_E_MAX * angle->i; /* Update electrical angle structure */ phase_angle_update(&angle->angle_el, angle_el); } /**************************************************************************** * Name: motor_angle_m_get * * Description: * Get motor mechanical angle * * Input Parameters: * angle - (in/out) pointer to the motor angle structure * * Returned Value: * Return motor mechanical angle * ****************************************************************************/ float motor_angle_m_get(FAR struct motor_angle_s *angle) { DEBUGASSERT(angle != NULL); return angle->anglem; } /**************************************************************************** * Name: motor_angle_e_get * * Description: * Get motor electrical angle * * Input Parameters: * angle - (in/out) pointer to the motor angle structure * * Returned Value: * Return motor electrical angle * ****************************************************************************/ float motor_angle_e_get(FAR struct motor_angle_s *angle) { DEBUGASSERT(angle != NULL); return angle->angle_el.angle; } /**************************************************************************** * Name: motor_phy_params_init * * Description: * Initialize motor physical parameters * * Input Parameters: * phy - (in/out) pointer to the motor physical parameters * poles - (in) number of the motor pole pairs * res - (in) average phase-to-neutral base motor resistance * (without temperature compensation) * ind - (in) average phase-to-neutral motor inductance * * Returned Value: * None * ****************************************************************************/ void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles, float res, float ind) { DEBUGASSERT(phy != NULL); phy->p = poles; phy->res_base = res; phy->ind = ind; phy->one_by_ind = 1.0f/ind; /* Initialize with zeros */ phy->res = 0.0f; phy->res_alpha = 0.0f; phy->res_temp_ref = 0.0f; } /**************************************************************************** * Name: motor_phy_params_temp_set * Description: * Initialize motor physical temperature parameters * * Input Parameters: * phy - (in/out) pointer to the motor physical parameters * res_alpha - (in) temperature coefficient of the winding resistance * res_temp_ref - (in) reference temperature for alpha * * Returned Value: * None * ****************************************************************************/ void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy, float res_alpha, float res_temp_ref) { DEBUGASSERT(phy != NULL); phy->res_alpha = res_alpha; phy->res_temp_ref = res_temp_ref; }