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

oncilla-motordriver-fw/src/config.h
df700679f13cload-bad-commit

Last Change
Raw File

config.h
View Options

#ifndef __CONFIG_H__
#define __CONFIG_H__
#include "control_table.h"
#define AMARSI_MOTORDRIVER_FIRMWARE_VERSION 0x01
// instruction cycle speed
#define FCY_FOR_BAUDRATE 40000000
#define FCY 40000000UL
// PWM frequency
#define FPWM 60000UL
// T1 interrupt frequency (Hz)
#define FT1 1000UL
#define FSYST 1000UL
// SBCP timer frequency in Hz: time-out timer stuff
#define FSBCPT 2000
// uart baudrate
#define BAUDRATE_BUS 3310000
// uart time out value (us) SHOULD NOT BE USED
//#define TIME_OUT 100
// common constants and macro's
#define MAX_PID_RANGE 32767
// communication protocol parameters
//TODO: allow multiple addresses (pairs of class and id)
#define SBCP_MY_CLASS 0xEA ///< binary protocol class
#define SBCP_MY_ID 0x09 ///< binary protocol ID
#define SBCP_IS_SLAVE
#define SBCP_BROADCAST_CLASS 0xEF ///< binary protocol broadcast class
#define SBCP_BROADCAST_ID 0xFF ///< binary protocol broadcast id
///////////////////////////////////////////////////////////////////////////////
// Motor combination parameters
// in order to improve movement resolution, speed is internally handled as a fixed point number
// with SPEED_SHIFT representing the number of digits after the comma
#define SPEED_SHIFT 4
// motor 1 parameters
// exact gearing ratio is 225:16
#define M1_QEI_RES 512UL
#define M1_HALL_RES 12UL
#define M1_GEAR_MULT 56UL
#define M1_GEAR_DIV 4UL
#define M1_MAX_SPEED 16200UL
#define M1_SLOWFAST_THRES 220 // torque threshold for switching from fast to slow decay
#define M1_SLOWFAST_COMP 200 // slow decay results in slightly more torque for the same current settings
#define M1_INERTIA_COMP 300 // negative acceleration feedback term compensates for motor inertia
// motor 2 parameters
// exact gearing ratio is 225:16
#define M2_QEI_RES 512UL
#define M2_HALL_RES 12UL
#define M2_GEAR_MULT 56UL
#define M2_GEAR_DIV 4UL
#define M2_MAX_SPEED 16200UL
#define M2_SLOWFAST_THRES 220 // torque threshold for switching from fast to slow decay
#define M2_SLOWFAST_COMP 200 // slow decay results in slightly more torque for the same current settings
#define M2_INERTIA_COMP 300 // negative acceleration feedback term compensates for motor inertia
///////////////////////////////////////////////////////////////////////////////
// Derived constants
// Maximum counter value of HW PWM counter
#define MAX_PWM (FCY/FPWM-1)
#define MAX_DUTY (2*FCY/FPWM-1)
// motor 1 derived parameters
#define OUT1_MAX_SPEED (M1_MAX_SPEED*M1_GEAR_DIV/M1_GEAR_MULT)
#define M1_MAX_QEI_CNT (M1_MAX_SPEED*M1_QEI_RES*4/(60*FT1))
#define M1_MAX_SPEED_CNT (M1_MAX_SPEED*M1_QEI_RES*(1<<SPEED_SHIFT)/(15*FT1))
#define M1_MAX_PID ((M1_MAX_SPEED*M1_QEI_RES*4*M1_INPUT_GAIN)/(60*FT1))
#define M1_CNT_PER_ROT (((unsigned long)M1_QEI_RES*4*(unsigned long)M1_GEAR_MULT)/M1_GEAR_DIV)
// motor 2 derived parameters
#define OUT2_MAX_SPEED (M2_MAX_SPEED*M2_GEAR_DIV/M2_GEAR_MULT)
#define M2_MAX_QEI_CNT (M2_MAX_SPEED*M2_QEI_RES*4/(60*FT1))
#define M2_MAX_SPEED_CNT (M2_MAX_SPEED*M2_QEI_RES*(1<<SPEED_SHIFT)/(15*FT1))
#define M2_MAX_PID ((M2_MAX_SPEED*M2_QEI_RES*4*M2_INPUT_GAIN)/(60*FT1))
#define M2_CNT_PER_ROT ((unsigned long)M2_QEI_RES*4*(unsigned long)M2_GEAR_MULT/M2_GEAR_DIV)
// motor 1 macro's
#define M1_SET_PWM(_PWM) (PWM_SPD1 = (MODE1==FAST_DECAY) ? (_PWM) + MAX_DUTY/2 : (_PWM)*2)
#define M1_RPM2QEI(_SPEED) (((_SPEED)*M1_GEAR_MULT*M1_QEI_RES*(1<<SPEED_SHIFT))/(15*FT1*M1_GEAR_DIV))
#define M1_EXT_POS_2_INT_POS(_EXT_POS) (((long)(_EXT_POS) * (long)M1_CNT_PER_ROT) / (long)ANGULAR_RESOLUTION)
#define M1_EXT_SPEED_2_INT_SPEED(_EXT_SPEED) (int)(((long)M1_EXT_POS_2_INT_POS(_EXT_SPEED) * (long)(1<<SPEED_SHIFT)) / (long)FT1)
#define M1_EXT_ACCEL_2_INT_ACCEL(_EXT_ACCEL) (unsigned int)(((long)M1_EXT_POS_2_INT_POS(_EXT_ACCEL) * (long)(1<<SPEED_SHIFT)) / (long)FT1*FT1)
#define M1_EXT_TRQ_2_INT_TRQ(_EXT_TRQ) (int)(((long)_EXT_TRQ * ((long)TORQUE_RESOLUTION * 512 / TORQUE_PER_ROTATION_SCALING)) / (long)M1_CNT_PER_ROT)
#define M1_EXT_DAMP_2_INT_DAMP(_EXT_DAMP) (int)((long)(M1_EXT_TRQ_2_INT_TRQ(_EXT_DAMP)) / (long)FT1)
#define M1_INT_POS_2_EXT_POS(_INT_POS) (((long)(_INT_POS) * (long)ANGULAR_RESOLUTION) / (long)M1_CNT_PER_ROT)
#define M1_INT_SPEED_2_EXT_SPEED(_INT_SPEED) (int)(((long)M1_INT_POS_2_EXT_POS(_INT_SPEED) * (long)FT1) / (long)(1<<SPEED_SHIFT))
#define M1_INT_ACCEL_2_EXT_ACCEL(_INT_ACCEL) (int)((long)M1_INT_SPEED_2_EXT_SPEED(_INT_ACCEL) * (long)FT1)
#define M1_INT_TRQ_2_EXT_TRQ(_INT_TRQ) (int)((long)_INT_TRQ * (long)M1_CNT_PER_ROT / (long)((long)TORQUE_RESOLUTION * 512 / (long)TORQUE_PER_ROTATION_SCALING))
#define M1_INT_DAMP_2_EXT_DAMP(_INT_DAMP) (int)((long)M1_INT_TRQ_2_EXT_TRQ(_INT_DAMP) * (long)FT1)
// motor 2 macro's
#define M2_SET_PWM(_PWM) (PWM_SPD2 = (MODE2==FAST_DECAY) ? (_PWM) + MAX_DUTY/2 : (_PWM)*2)
#define M2_RPM2QEI(_SPEED) (((_SPEED)*M2_GEAR_MULT*M2_QEI_RES*(1<<SPEED_SHIFT))/(15*FT1*M2_GEAR_DIV))
#define M2_EXT_POS_2_INT_POS(_EXT_POS) (((long)(_EXT_POS) * (long)M2_CNT_PER_ROT) / (long)ANGULAR_RESOLUTION)
#define M2_EXT_SPEED_2_INT_SPEED(_EXT_SPEED) (int)(((long)M2_EXT_POS_2_INT_POS(_EXT_SPEED) * (long)(1<<SPEED_SHIFT)) / (long)FT1)
#define M2_EXT_ACCEL_2_INT_ACCEL(_EXT_ACCEL) (unsigned int)(((long)M1_EXT_POS_2_INT_POS(_EXT_ACCEL) * (long)(1<<SPEED_SHIFT)) / (long)FT1*FT1)
#define M2_EXT_TRQ_2_INT_TRQ(_EXT_TRQ) (int)(((long)_EXT_TRQ * ((long)TORQUE_RESOLUTION * 512 / TORQUE_PER_ROTATION_SCALING))/(long)M2_CNT_PER_ROT)
#define M2_EXT_DAMP_2_INT_DAMP(_EXT_DAMP) (int)((long)M2_EXT_TRQ_2_INT_TRQ(_EXT_DAMP) / (long)FT1)
#define M2_INT_POS_2_EXT_POS(_INT_POS) (((long)(_INT_POS)*(long)ANGULAR_RESOLUTION) / (long)M2_CNT_PER_ROT)
#define M2_INT_SPEED_2_EXT_SPEED(_INT_SPEED) (int)(((long)M2_INT_POS_2_EXT_POS(_INT_SPEED) * (long)FT1)/(long)(1<<SPEED_SHIFT))
#define M2_INT_ACCEL_2_EXT_ACCEL(_INT_ACCEL) (int)((long)M2_INT_SPEED_2_EXT_SPEED(_INT_ACCEL) * (long)FT1)
#define M2_INT_TRQ_2_EXT_TRQ(_INT_TRQ) (int)((long)_INT_TRQ * (long)M2_CNT_PER_ROT / (long)((long)TORQUE_RESOLUTION * 512 / (long)TORQUE_PER_ROTATION_SCALING))
#define M2_INT_DAMP_2_EXT_DAMP(_INT_DAMP) (int)((long)M2_INT_TRQ_2_EXT_TRQ(_INT_DAMP) * (long)FT1)
#endif // __CONFIG_H__
c4science · Help