Homec4science
Diffusion Oncilla Motordriver Firmware (load-bad-commit) Browse

oncilla-motordriver-fw/src/mdv_internal.h
edc3f097732eload-bad-commit

Last Change
Raw File

mdv_internal.h
View Options

#ifndef MDV_INTERNAL_H_
#define MDV_INTERNAL_H_
#include "motordriver.h"
#include "config.h"
#include "ring-buffer.h"
//==============================================================================
// DEFINES
//==============================================================================
//nb increment of QEI encoder
#define QEI_RES 512UL
#define QEI_CNT_PER_TURN (QEI_RES * 4)
//compute the gear coeff, not done automatically !!!
// GEAR_MULT_COEFF 512 * 4 1
//----------------- = --------- = ---
// GEAR_DIV_COEFF 4096 2
#define GEAR_MULT_COEFF 1
#define GEAR_DIV_COEFF 2
#define HALL_RES 12UL
//max speed of the motor in RPM
#define MAX_SPEED_RPM 16200UL
//==============================================================================
// CONTROL DATA FOR MOTORDRIVER
//==============================================================================
enum mdv_flags {
MDV_F_NO_FLAGS = 0,
MDV_F_MOVING_ENABLE = 1 << 0,
MDV_F_UPDATE_I_ERROR = 1 << 1,
MDV_F_NEW_POSITION_AVAILABLE = 1 << 2,
MDV_F_NEW_SPEED_AVAILABLE = 1 << 3
};
typedef enum mdv_flags mdv_flags;
#define mdv_set_flag(m,f) do{ (m)->flags |= (f); }while(0)
#define mdv_clear_flag(m,f) do { (m)->flags &= ~(f); }while(0)
#define mdv_flag(m,f) ( (m->flags) & f )
#define mdv_set_fast_decay(mdv) \
do{ \
gpio_clear(mdv->pins.mode); /*fast decay when low */ \
}while(0)
#define mdv_set_slow_decay(mdv) \
do{ \
gpio_set(mdv->pins.mode); /* slow decay when high */ \
}while(0)
#define mdv_in_fast_decay_mode(mdv) (! gpio_read(mdv->pins.mode))
#define mdv_in_slow_decay_mode(mdv) ( gpio_read(mdv->pins.mode))
struct mdv_calibration_parameters {
unsigned int pos_limit;
unsigned int min_region;
unsigned int max_region;
int offset;
pos_evaluator_fptr pos_evaluator;
unsigned int update_period;
unsigned int ellapsed_ts;
unsigned int pos_evaluator_max_value;
};
struct motordriver {
mdv_pin pins;
mdv_parameters params;
struct mdv_calibration_parameters calib;
mdv_status_and_control sc;
mdv_flags flags;
sbcp_reg_address address_start;
volatile unsigned int * pos_counter;
/**
* \name qei position parameters
*/
///@{
/**
* previous (relative) position counter of quadrature
* input. dimension QEI increments
*/
volatile int previous_cnt;
/**
* relative present position counter of quadrature
* input. dimension QEI increments
*/
volatile long pos_cnt;
/**
* current speed of quadrature input. dimension QEI increments per
* 1/FT1
*/
volatile int current_speed;
/**
* current acceleration of quadrature input. dimension QEI
* increments per (1/FT1) ^2
*/
volatile int current_accel;
///@}
// move controlling parameters
int user_goal_pos; //buffer goal position
long goal_pos; // goal position
long goal_speed; // desired speed of the motor during the motion
// QEI incremet / ts (1ms)
//commands buffer to avoid jitter
RingBuffer commandsBuffer; //the ring buffer containing the commands
int startSendOrders; //boolean to start sending the orders when half the capacity is reached
int timeCounter; //a counter to log commands only every 5ms.
// internally controlled variables
long moving_pos; // current "desired" position during move
long moving_speed; // current "desired" moving speed of the motor
// 1
int resolution_error; // compensates for rounding errors
long i_error; // speed integral error
long prev_p_error; // previous proportional error, used to compute the derivative error
int max_trq; // maximum torque setting
// control output parameters
long output_trq; // current torque request
// motor limits
long pos_limit; // maximum positive limit
long max_speed; // maximum speed the motor may reach
int max_accel; // 1 / 2 ^
int gear_mult;
int gear_div;
struct {
int t;
int t_residue;
int iter;
} sine;
};
/**
* \name conversion macro
* Thes macro convert dimension for several value
*/
///@{
/**
* Transforms 1/4096 of turn in QEI increments
* \params mdv the motordriver
* \params pos the position to transform
*/
#define mdv_e2i_position(mdv,pos) ((long) ( (mdv)->gear_mult * ((long) (pos)) / (mdv)->gear_div ))
/**
* Transforms QEI increments per (1/FT1) to 1 / 2 ^ SPEED_BINARY_PRECISION
* QEI increments per per (1/FT1)
* \params speed the speed to transform
*/
#define mdv_lo2hi_res_speed(spd) ( (spd) * (1 << SPEED_BINARY_PRECISION) )
/**
* Transforms 1/4096 of turn per second to 1/ 2 ^
* SPEED_BINARY_PRECISION QEI increments per (1/FT1) (with higher
* precision)
* \param mdv the motordriver
* \param speed the speed
*/
#define mdv_e2i_speed(mdv,spd) (mdv_lo2hi_res_speed(mdv_e2i_position(mdv,spd)) / FT1)
/**
* Transforms 1/4096 of turn pers second^2 to 1 / 2 ^
* SPEED_BINARY_PRECISION QEI per (1/ FT1)^2
* \param mdv the motordriver
* \param a the acceleration
*/
#define mdv_e2i_accel(mdv,a) (mdv_e2i_speed(mdv, a ) * 128 / FT1 )
/**
*
*/
#define mdv_e2i_torque(mdv,trq) (trq)
/**
* Transforms QEI increments in 1/4096 of turn.
* \params mdv the motordriver
* \params pos the position to transform
*/
#define mdv_i2e_position(mdv,pos) ((long) ( (mdv)->gear_div * ((long) (pos)) / (mdv)->gear_mult ))
/**
* Transforms 1 / 2 ^ SPEED_BINARY_PRECISION QEI increments per (1/FT1) in QEI
* increments per (1/FT1).
* \params speed the speed to transform
*/
#define mdv_hi2lo_res_speed(spd) ( (spd) / ( 1 << SPEED_BINARY_PRECISION ) )
/**
* Transforms 1/ 2 ^ SPEED_BINARY_PRECISION QEI increments per (1/FT1) in
* 1/4096 of turn per second.
* \param mdv the motordriver
* \param speed the speed
*/
#define mdv_i2e_speed(mdv,speed) ( FT1 * mdv_i2e_position(mdv,mdv_hi2lo_res_speed(speed)) )
/**
* Transforms 1 / 2 ^ SPEED_BINARY_PRECISION QEI per (1/FT1)^2 in 1/4096 of
* turn pers second^2.
* \param mdv the motordriver
* \param a the acceleration
*/
#define mdv_i2e_accel(mdv,a) ( mdv_i2e_speed(mdv,a) / 128 * FT1)
/**
*
*/
#define mdv_i2e_torque(mdv,trq) (trq)
///@}
//==============================================================================
// CONTROL MODE SWITCHING
//==============================================================================
/**
* Process function for a control mode.
*/
typedef void (*mdv_ctrl_process_fptr)(motordriver *);
/**
* Init function for a control mode.
*/
typedef void (*mdv_ctrl_init_fptr)(motordriver *);
typedef struct mdv_ctrl_fptr {
mdv_ctrl_process_fptr process;
mdv_ctrl_init_fptr init;
} mdv_ctrl_fptr;
/**
* General cleanup function. It resets a mode so all PID is clean and
* unintegrated.
*/
void mdv_cleanup(motordriver * mdv);
/**
* A mdv_ctrl_process_fptr function that does nothing.
*/
void mdv_null_process(motordriver * mdv);
void mdv_set_pos_count(motordriver * mdv, long pos);
extern mdv_ctrl_fptr mdv_ctrl[MDV_NUMBER_OF_CONTROL_MODE];
void mdv_set_control_mode_private(motordriver * mdv , mdv_control_mode c);
#endif
c4science · Help