Other Parts Discussed in Thread: MOTORWARE
Hi,
We have a problem when sending a zero reference speed to the motor as the motor starts to oscillate and vibrates. How can we disable such oscillation?
Things we have tried:
1. Hardcoding the speed values (that are within abs(100) rpm, or close to zero) within which we use the ForceAngle:
if (global_speed < 100 && global_speed > -100){
EST_setFlag_enableForceAngle(obj->estHandle,true);
gMotorVars.Flag_enableForceAngle = true;
}
else {
EST_setFlag_enableForceAngle(obj->estHandle, false);
gMotorVars.Flag_enableForceAngle = false;
}
2. Disabling PWM and DRV but I believe this is not a good solution.
What can we do to overcome this problem? I am attaching our main.c and user.h for you reference
src_main.c
include_user (1).h
/* --COPYRIGHT--,BSD
* Copyright (c) 2012, LineStream Technologies Incorporated
* Copyright (c) 2012, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * 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.
*
* * Neither the names of Texas Instruments Incorporated, LineStream
* Technologies Incorporated, 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.
* --/COPYRIGHT--*/
//! \file solutions/instaspin_motion/src/proj_lab05d.c
//! \brief InstaSPIN-MOTION SpinTAC Speed Controller
//!
//! (C) Copyright 2012, LineStream Technologies, Inc.
//! (C) Copyright 2011, Texas Instruments, Inc.
//! \defgroup PROJ_LAB05d PROJ_LAB05d
//@{
//! \defgroup PROJ_LAB05d_OVERVIEW Project Overview
//!
//! Running the SpinTAC Velocity Controller
//!
// **************************************************************************
// the includes
// system includes
#include <math.h>
#include "main.h"
#include "uavcan.h"
#ifdef FLASH
#pragma CODE_SECTION(mainISR,"ramfuncs");
#endif
// Include header files used in the main function
//#define LEARN
#define SCIENCE_MODE_HZ 10
#ifdef SCIENCE_MODE_HZ
int16_t gScienceCnt = 0;
#endif
// **************************************************************************
// the defines
#define LED_BLINK_FREQ_Hz 10
#define TACHO_SEND_FREQ_HZ 50
#define STATUS_SEND_FREQ_HZ 1
#define SPEED_RX_TIMEOUT_MS 500
#define LIFT_RX_TIMEOUT_MS 500
#define TEST_SPEED_TIMEOUT_HZ 0.5
uint32_t LIFT_ENC_LIMIT = 45000; // ticks
uint32_t enc = 0;
// **************************************************************************
// the globals
uint_least16_t gCounter_updateGlobals = 0;
bool Flag_Latch_softwareUpdate = true;
CTRL_Handle ctrlHandle;
HAL_Handle halHandle;
USER_Params gUserParams;
HAL_PwmData_t gPwmData = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
HAL_AdcData_t gAdcData;
_iq gMaxCurrentSlope = _IQ(0.0);
volatile int32_t tacho = 0;
int32_t global_speed = 0;
#ifdef FAST_ROM_V1p6
CTRL_Obj *controller_obj;
#else
CTRL_Obj ctrl; //v1p7 format
#endif
ST_Obj st_obj;
ST_Handle stHandle;
uint16_t gLEDcnt = 0;
uint16_t gTACHOcnt = 0;
uint16_t gTIMEOUT_SPEEDcnt = 0;
uint16_t gTIMEOUT_LIFTcnt = 0;
bool LIFT_MANUAL_FLAG = false;
int16_t gTestcnt = 0;
volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;
#ifdef FLASH
// Used for running BackGround in flash, and ISR in RAM
extern
uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
#endif
typedef struct{
uint16_t mbox_num;
uint32_t can_id;
uint16_t dlc;
}can_id_map_t;
can_id_map_t can_id_target_speed_rx_map;
can_id_map_t can_id_control_rx_map;
can_id_map_t can_id_auto_lift_rx_map;
can_id_map_t can_id_manual_lift_rx_map;
can_id_map_t can_id_tacho_tx_map;
can_id_map_t can_id_lift_done_tx_map;
//#define TL
#define TR
//#define BL
//#define BR
#ifdef TL
#define UAVCAN_PORT_TARGET_SPEED_RX 900
#define UAVCAN_PORT_CONTROL_RX 9001
#define UAVCAN_PORT_AUTO_LIFT_RX 9002
#define UAVCAN_PORT_MANUAL_LIFT_RX 9003
#define UAVCAN_PORT_TACHO_TX 1240
#define UAVCAN_PORT_LIFT_DONE_TX 9004
#define REVERSED 1
#endif
#ifdef TR
#define UAVCAN_PORT_TARGET_SPEED_RX 901
#define UAVCAN_PORT_CONTROL_RX 9101
#define UAVCAN_PORT_AUTO_LIFT_RX 9102
#define UAVCAN_PORT_MANUAL_LIFT_RX 9103
#define UAVCAN_PORT_TACHO_TX 1241
#define UAVCAN_PORT_LIFT_DONE_TX 9104
#define REVERSED 1
#endif
#ifdef BL
#define UAVCAN_PORT_TARGET_SPEED_RX 902
#define UAVCAN_PORT_CONTROL_RX 9201
#define UAVCAN_PORT_AUTO_LIFT_RX 9202
#define UAVCAN_PORT_MANUAL_LIFT_RX 9203
#define UAVCAN_PORT_TACHO_TX 1242
#define UAVCAN_PORT_LIFT_DONE_TX 9204
#define REVERSED 0
#endif
#ifdef BR
#define UAVCAN_PORT_TARGET_SPEED_RX 903
#define UAVCAN_PORT_CONTROL_RX 9301
#define UAVCAN_PORT_AUTO_LIFT_RX 9302
#define UAVCAN_PORT_MANUAL_LIFT_RX 9303
#define UAVCAN_PORT_TACHO_TX 1243
#define UAVCAN_PORT_LIFT_DONE_TX 9304
#define REVERSED 0
#endif
uint16_t pwm_cnt = 0;
uint32_t pwm_tics_num = (USER_ISR_FREQ_Hz / 1000); // pwm 1khz
uint16_t duty_cycle_percent = 0;
void pwm_generator(uint16_t duty_cycle_percent)
{
uint32_t pwm_high_tics = (uint32_t)((pwm_tics_num * duty_cycle_percent) / 100);
if(pwm_cnt <= pwm_high_tics){
GPIO_setHigh(halHandle->gpioHandle, LIFT_PWM.gpio_num);
}
else if(pwm_cnt < pwm_tics_num && pwm_cnt > pwm_high_tics){
GPIO_setLow(halHandle->gpioHandle, LIFT_PWM.gpio_num);
}
else if(pwm_cnt >= pwm_tics_num){
pwm_cnt = 0;
}
}
void sendParams(){
can_configure_tx_mbox(15, 0x00000001, 8);
int32_t kp_spd = _IQtoF(gMotorVars.Kp_spd) * 1e6;
int32_t ki_spd = _IQtoF(gMotorVars.Ki_spd) * 1e6;
uint64_t data_k_spd = (uint64_t)kp_spd << 32 | (uint32_t)ki_spd;//| ki_spd;
can_write(15, data_k_spd);
can_configure_tx_mbox(16, 0x00000002, 8);
int32_t kp_idq = _IQtoF(gMotorVars.Kp_Idq) * 1e6;
int32_t ki_idq = _IQtoF(gMotorVars.Ki_Idq) * 1e6;
uint64_t data_k_idq = (uint64_t)kp_idq << 32 | (uint32_t)ki_idq;
can_write(16, data_k_idq);
can_configure_tx_mbox(17, 0x00000003, 8);
int32_t i_bias0 = _IQtoF(gMotorVars.I_bias.value[0]) * 1e6;
int32_t v_bias0 = _IQtoF(gMotorVars.V_bias.value[0]) * 1e6;
uint64_t data_bias0 = (uint64_t)i_bias0 << 32 | (uint32_t)v_bias0;
can_write(17, data_bias0);
can_configure_tx_mbox(18, 0x00000004, 8);
int32_t i_bias1 = _IQtoF(gMotorVars.I_bias.value[1]) * 1e6;
int32_t v_bias1 = _IQtoF(gMotorVars.V_bias.value[1]) * 1e6;
uint64_t data_bias1 = (uint64_t)i_bias1 << 32 | (uint32_t)v_bias1;
can_write(18, data_bias1);
can_configure_tx_mbox(19, 0x00000005, 8);
int32_t i_bias2 = _IQtoF(gMotorVars.I_bias.value[2]) * 1e6;
int32_t v_bias2 = _IQtoF(gMotorVars.V_bias.value[2]) * 1e6;
uint64_t data_bias2 = (uint64_t)i_bias2 << 32 | (uint32_t)v_bias2;
can_write(19, data_bias2);
can_configure_tx_mbox(20, 0x00000006, 8);
int32_t rs_om = gMotorVars.Rs_Ohm * 1e6;
int32_t flux = gMotorVars.Flux_VpHz * 1e6;
uint64_t rs_flux = (uint64_t)rs_om << 32 | (uint32_t)flux;
can_write(20, rs_flux);
can_configure_tx_mbox(21, 0x00000007, 8);
uint32_t lsd = (uint32_t)(gMotorVars.Lsd_H * 1e9);
uint32_t lsq = (uint32_t)(gMotorVars.Lsq_H * 1e9);//(uint32_t)(0.0000114935112 * 1e9);
uint64_t lsd_lsq = (uint64_t)lsd << 32 | (uint32_t)lsq;
can_write(21, lsd_lsq);
can_configure_tx_mbox(22, 0x00000008, 8);
int32_t est_state = gMotorVars.EstState;
int32_t ctrl_state = gMotorVars.CtrlState;
uint64_t state = (uint64_t)ctrl_state << 32 | (uint32_t)est_state;
can_write(22, state);
can_configure_tx_mbox(23, 0x00000009, 8);
uint32_t vbus = _IQtoF(gMotorVars.VdcBus_kV) * 1e3;
can_write(23, vbus);
can_configure_tx_mbox(24, 0x00000010, 2);
uint16_t MotorIdentified = gMotorVars.Flag_MotorIdentified;
can_write(24, MotorIdentified);
}
void lift(uint16_t duty, uint16_t dir)
{
if(dir == 0x00) {
GPIO_setHigh(halHandle->gpioHandle, LIFT_CW_DIR.gpio_num);
GPIO_setLow(halHandle->gpioHandle, LIFT_CCW_DIR.gpio_num);
}
else if(dir == 0xFF) {
GPIO_setLow(halHandle->gpioHandle, LIFT_CW_DIR.gpio_num);
GPIO_setHigh(halHandle->gpioHandle, LIFT_CCW_DIR.gpio_num);
}
if(duty == 0){
duty_cycle_percent = duty;
GPIO_setLow(halHandle->gpioHandle,LIFT_CW_DIR.gpio_num);
GPIO_setLow(halHandle->gpioHandle,LIFT_CCW_DIR.gpio_num);
}
else if(duty > 100){
duty = 100;
}
duty_cycle_percent = duty;
}
void trace(unsigned char e) {
unsigned char i = 1;
GPIO_setHigh(halHandle->gpioHandle,GPIO_Number_8);
for (i = 0; i < e; ++i) {
GPIO_toggle(halHandle->gpioHandle,GPIO_Number_8);
}
GPIO_setLow(halHandle->gpioHandle,GPIO_Number_8);
}
_iq _IQdivRem(_iq in1, _iq in2)
{
_iq res = _IQdiv(in1, in2) & 0x00FFFFFF;
return res;
}
volatile _iq prev_angle = _IQ(0.0);
void tachoObs(_iq flux_angle)
{
_iq delta = flux_angle - prev_angle;
if(delta < 0 && delta <= _IQ(-0.5))
{
delta += _IQ(1.0);
}
else if(delta > 0 && delta >= _IQ(0.5))
{
delta -= _IQ(1.0);
}
if(delta > 0) {
if (delta >= _IQ(0.1666)) {
tacho += 1;
prev_angle = flux_angle;
}
}
else
{
if (delta <= _IQ(-0.1666)) {
tacho -= 1;
prev_angle = flux_angle;
}
}
}
//DRV_SPI_8323_Vars_t gDrvSpi8323Vars;
_iq gFlux_pu_to_Wb_sf;
_iq gFlux_pu_to_VpHz_sf;
_iq gTorque_Ls_Id_Iq_pu_to_Nm_sf;
_iq gTorque_Flux_Iq_pu_to_Nm_sf;
// **************************************************************************
// the functions
void clb(CANMboxNumber mbox_num) {
volatile CANMsgData data;
can_read(mbox_num, &data);
if(mbox_num == can_id_target_speed_rx_map.mbox_num) {
int32_t speed = getDataFromUavCan(data);
global_speed = speed;
if (speed > 5000){
speed = 5000;
}
else if (speed < -5000){
speed = -5000;
}
float sp_ref = (float) speed / 1000;
if (REVERSED == 1)
{
sp_ref = -1.0 * ((float)speed / 1000);
}
gTIMEOUT_SPEEDcnt = 0;
gMotorVars.SpeedRef_krpm = _IQ(sp_ref);
}
else if(mbox_num == can_id_control_rx_map.mbox_num) {
uint32_t control = getDataFromUavCan(data);
if (control == 1) {
gMotorVars.Flag_enableSys = true;
gMotorVars.Flag_Run_Identify = true;
} else if (control == 0) {
gMotorVars.Flag_enableSys = false;
gMotorVars.Flag_Run_Identify = false;
}
}
else if(mbox_num == can_id_auto_lift_rx_map.mbox_num) {
uint16_t lift_auto_data = getDataFromUavCan(data);
uint16_t lift_auto_duty = lift_auto_data & 0x00FF;
uint16_t lift_auto_dir = (lift_auto_data & 0xFF00) >> 8;
LIFT_MANUAL_FLAG = false;
enc = 0;
lift(lift_auto_duty, lift_auto_dir);
}
else if(mbox_num == can_id_manual_lift_rx_map.mbox_num) {
uint16_t lift_manual_data = getDataFromUavCan(data);
uint16_t lift_manual_duty = lift_manual_data & 0x00FF;
uint16_t lift_manual_dir = (lift_manual_data & 0xFF00) >> 8;
gTIMEOUT_LIFTcnt = 0;
LIFT_MANUAL_FLAG = true;
lift(lift_manual_duty, lift_manual_dir);
}
}
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
// trace(2);
#endif
GPIO_setHigh(halHandle->gpioHandle,RED_LED.gpio_num);
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
//USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
//HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
//HAL_turnLedOn(halHandle,(GPIO_Number_e)HAL_Gpio_LED3);
// do not allow code execution if there is a user parameter error
/*if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}*/
// initialize the user parameters
USER_setParams(&gUserParams);
//trace(3);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
//trace(4);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
/* Enable the eCAN interrupts at the PIE and CPU level. */
HAL_enableECanaInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
/* for (int i = 0; i < 10; ++i) {
for(int j = 0 ; j < 1e7; j++) {
GPIO_setLow(halHandle->gpioHandle,GREEN_LED.gpio_num);
GPIO_setLow(halHandle->gpioHandle,RED_LED.gpio_num);
}
for(int j = 0 ; j < 1e7; j++) {
GPIO_setHigh(halHandle->gpioHandle,GREEN_LED.gpio_num);
GPIO_setHigh(halHandle->gpioHandle,RED_LED.gpio_num);
}
}*/
//GPIO_setLow(halHandle->gpioHandle,GPIO_Number_12);
//GPIO_setLow(halHandle->gpioHandle,GPIO_Number_14);
// initialize the SpinTAC Components
stHandle = ST_init(&st_obj, sizeof(st_obj));
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
#ifndef LEARN
ST_setupVelMove(stHandle);
#endif
// turn on the DRV8323 if present
HAL_enableDrv(halHandle);
// initialize the DRV8323 interface
//HAL_setupDrvSpi(halHandle,&gDrvSpi8323Vars);
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
//trace(100);
//gPwmData.Tabc.value[0] = _IQ(0.5);
//gPwmData.Tabc.value[1] = _IQ(0.46);
//gPwmData.Tabc.value[2] = _IQ(0.5);
//HAL_writePwmData(halHandle,&gPwmData);
gMotorVars.Flag_enableSys = false;
gMotorVars.Flag_Run_Identify = false;
#ifdef LEARN
gMotorVars.Flag_enableUserParams = false; // false - learn
gMotorVars.Flag_enableForceAngle = true; // true - learn
#else
gMotorVars.Flag_enableUserParams = true; // false - learn
gMotorVars.Flag_enableForceAngle = true; // true - learn
#endif
gMotorVars.Flag_enableFieldWeakening = false;
gMotorVars.Flag_enableRsRecalc = true;
gMotorVars.Flag_enableOffsetcalc = true;
#ifndef LEARN
/*
gMotorVars.Ki_Idq = _IQ(0.192);//_IQ(0.192);//_IQ(0.00276); /// 0.292
gMotorVars.Kp_Idq = _IQ(0.0358);//_IQ(0.0358);//_IQ(2.810699); /// 0.0458
gMotorVars.Ki_spd = _IQ(0.08);//0.08, _IQ(10.0);
gMotorVars.Kp_spd = _IQ(12.0);// 4.0 _IQ(50.818);
gMotorVars.SpeedRef_krpm = _IQ(0.1);
gMotorVars.MaxAccel_krpmps = _IQ(0.1);
gMotorVars.SpinTAC.VelMoveCurveType = ST_MOVE_CUR_STCRV;
CTRL_setKi(ctrlHandle,CTRL_Type_PID_Id, gMotorVars.Ki_Idq);
CTRL_setKi(ctrlHandle,CTRL_Type_PID_Iq, gMotorVars.Ki_Idq);
CTRL_setKp(ctrlHandle,CTRL_Type_PID_Id, gMotorVars.Kp_Idq);
CTRL_setKp(ctrlHandle,CTRL_Type_PID_Iq, gMotorVars.Kp_Idq);
CTRL_setKi(ctrlHandle,CTRL_Type_PID_spd, gMotorVars.Ki_spd);
CTRL_setKp(ctrlHandle,CTRL_Type_PID_spd, gMotorVars.Kp_spd);
*/
gMotorVars.SpeedRef_krpm = _IQ(0.0);
gMotorVars.MaxAccel_krpmps = _IQ(3.0);
gMotorVars.SpinTAC.VelMoveCurveType = ST_MOVE_CUR_STCRV;
#endif
//-------------rx-----------------//
can_id_target_speed_rx_map.mbox_num = 26;
can_id_target_speed_rx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_TARGET_SPEED_RX, 10, UCP_Nominal);
can_id_target_speed_rx_map.dlc = 5;
can_id_control_rx_map.mbox_num = 27;
can_id_control_rx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_CONTROL_RX, 10, UCP_Nominal);
can_id_control_rx_map.dlc = 8;
can_id_auto_lift_rx_map.mbox_num = 28;
can_id_auto_lift_rx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_AUTO_LIFT_RX, 10, UCP_Nominal);
can_id_auto_lift_rx_map.dlc = 3;
can_id_manual_lift_rx_map.mbox_num = 29;
can_id_manual_lift_rx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_MANUAL_LIFT_RX, 10, UCP_Nominal);
can_id_manual_lift_rx_map.dlc = 3;
//-------------tx-----------------//
can_id_tacho_tx_map.mbox_num = 25;
can_id_tacho_tx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_TACHO_TX, 10, UCP_Nominal);
can_id_tacho_tx_map.dlc = 5;
can_id_lift_done_tx_map.mbox_num = 24;
can_id_lift_done_tx_map.can_id = getMsgIdUavCan(UAVCAN_PORT_LIFT_DONE_TX, 10, UCP_Nominal);
can_id_lift_done_tx_map.dlc = 2;
//-------------can_init-----------------//
can_init();
can_configure_tx_mbox(can_id_tacho_tx_map.mbox_num, can_id_tacho_tx_map.can_id, can_id_tacho_tx_map.dlc);
can_configure_rx_mbox(can_id_target_speed_rx_map.mbox_num, can_id_target_speed_rx_map.can_id, false);
can_configure_rx_mbox(can_id_control_rx_map.mbox_num, can_id_control_rx_map.can_id, false);
can_configure_rx_mbox(can_id_auto_lift_rx_map.mbox_num, can_id_auto_lift_rx_map.can_id, false);
can_configure_rx_mbox(can_id_manual_lift_rx_map.mbox_num, can_id_manual_lift_rx_map.can_id, false);
can_register_mbox_irq_callback(RX, clb);
can_init_irqs();
GPIO_setHigh(halHandle->gpioHandle,GREEN_LED.gpio_num);
//HAL_turnLedOff(halHandle,(GPIO_Number_e)HAL_Gpio_LED3);
/* for(;;)
{
trace(100);
}*/
for(;;)
{
//can_write(25, 0x01010101DEADBEEFull);
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// Dis-able the Library internal PI. Iq has no reference now
#ifdef LEARN
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false); // false - learn
#else
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true); // false - learn
#endif
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
ST_Obj *stObj = (ST_Obj *)stHandle;
//int32_t ang = (int32_t)(_IQtoF(EST_getAngle_pu(obj->estHandle)) * 1000);
//int32_t sp = (int32_t)(_IQtoF(gMotorVars.Speed_krpm) * 1000);
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
else
{
// set the current bias
HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));
// set the voltage bias
HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));
}
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
// enable the SpinTAC Speed Controller
STVELCTL_setEnable(stObj->velCtlHandle, true);
if(EST_getState(obj->estHandle) != EST_State_OnLine)
{
// if the estimator is not running, place SpinTAC into reset
STVELCTL_setEnable(stObj->velCtlHandle, false);
// if the estimator is not running, set SpinTAC Move start & end velocity to 0
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQ(0.0));
STVELMOVE_setVelocityStart(stObj->velMoveHandle, _IQ(0.0));
}
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
// initialize the watch window kp and ki current values with pre-calculated values
//gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
//gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
//gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
//gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
// initialize the watch window Bw value with the default value
#ifndef LEARN
gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);
#endif
// initialize the watch window with maximum and minimum Iq reference
gMotorVars.SpinTAC.VelCtlOutputMax_A = _IQmpy(STVELCTL_getOutputMaximum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
gMotorVars.SpinTAC.VelCtlOutputMin_A = _IQmpy(STVELCTL_getOutputMinimum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle, stHandle);
}
// update Kp and Ki gains
//updateKpKiGains(ctrlHandle);
// set the SpinTAC (ST) bandwidth scale
#ifndef LEARN
STVELCTL_setBandwidth_radps(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBw_radps);
#endif
// set the maximum and minimum values for Iq reference
STVELCTL_setOutputMaximums(stObj->velCtlHandle, _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
// enable/disable the forced angle
//EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
if (global_speed < 100 && global_speed > -100){
EST_setFlag_enableForceAngle(obj->estHandle,true);
gMotorVars.Flag_enableForceAngle = true;
}
else {
EST_setFlag_enableForceAngle(obj->estHandle, false);
gMotorVars.Flag_enableForceAngle = false;
}
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
//HAL_writeDrvData(halHandle,&gDrvSpi8323Vars);
//sendParams();
//HAL_readDrvData(halHandle,&gDrvSpi8323Vars);
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
#ifndef LEARN
ST_setupVelMove(stHandle);
#endif
} // end of for(;;) loop
} // end of main() function
uint16_t fl = 0;
bool prev_gpio_state = 0;
interrupt void mainISR(void)
{
GPIO_setHigh(halHandle->gpioHandle,GPIO_Number_8);
static uint16_t stCnt = 0;
pwm_generator(duty_cycle_percent);
pwm_cnt++;
bool lift_enc_state = GPIO_read(halHandle->gpioHandle,LIFT_ENC.gpio_num);
if(lift_enc_state == 0 && prev_gpio_state == 1)
{
enc++;
}
prev_gpio_state = lift_enc_state;
// toggle status LED
#ifdef SCIENCE_MODE_HZ
if(++gScienceCnt>= (uint_least32_t)(USER_ISR_FREQ_Hz / SCIENCE_MODE_HZ))
{
//gMotorVars.SpeedRef_krpm = _IQ(0.0);
//sendParams();
can_configure_tx_mbox(17, 0x00000003, 8);
can_write(17, enc);
can_configure_tx_mbox(18, 0x00000004, 8);
can_write(18, gMotorVars.Flag_enableSys);
can_configure_tx_mbox(10, 0x00000005, 4);
/* can_configure_tx_mbox(18, 0x00000004, 8);
can_write(18, lift_enc_state);
can_configure_tx_mbox(19, 0x00000005, 8);
can_write(19, prev_gpio_state);
*/
gScienceCnt = 0;
}
#endif
if(++gTIMEOUT_SPEEDcnt >= (uint_least32_t)(USER_ISR_FREQ_Hz / (1000 / SPEED_RX_TIMEOUT_MS)))
{
gMotorVars.SpeedRef_krpm = _IQ(0.0);
gTIMEOUT_SPEEDcnt = 0;
}
if(++gTIMEOUT_LIFTcnt >= (uint_least32_t)(USER_ISR_FREQ_Hz / (1000 / LIFT_RX_TIMEOUT_MS))){
//gMotorVars.SpeedRef_krpm = _IQ(0.0);
if (LIFT_MANUAL_FLAG == true){
lift(0 , 0xFF);
}
gTIMEOUT_LIFTcnt = 0;
}
if(LIFT_MANUAL_FLAG == false){
if(enc >= LIFT_ENC_LIMIT){
lift(0 , 0xFF);
can_write(can_id_lift_done_tx_map.mbox_num, getUavCanPayload(0x01));
enc = 0;
}
}
/*if(++gTestcnt >= (uint_least32_t)(USER_ISR_FREQ_Hz / TEST_SPEED_TIMEOUT_HZ))
{
if(fl == 0) {
#ifndef LEARN
gMotorVars.SpeedRef_krpm = _IQ(-0.1);
#endif
fl = 1;
}
else if (fl == 1)
{
#ifndef LEARN
gMotorVars.SpeedRef_krpm = _IQ(0.1);
#endif
fl = 0;
}
gTestcnt = 0;
}*/
if(++gLEDcnt >= (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
{
//GPIO_toggle(halHandle->gpioHandle,GREEN_LED.gpio_num);
GPIO_toggle(halHandle->gpioHandle,RED_LED.gpio_num);
gLEDcnt = 0;
}
tachoObs(EST_getAngle_pu(ctrlHandle->estHandle));
// Send tachometer on 100HZ
if(++gTACHOcnt >= (uint_least32_t)(USER_ISR_FREQ_Hz / TACHO_SEND_FREQ_HZ))
{
int32_t tacho_copy = tacho;
if (REVERSED == 1) tacho_copy *= -1;
can_write(can_id_tacho_tx_map.mbox_num, getUavCanPayload(tacho_copy));
gTACHOcnt = 0;
}
// acknowledge the ADC interrupt
HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
// convert the ADC data
HAL_readAdcData(halHandle,&gAdcData);
// Run the SpinTAC Components
if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
#ifndef LEARN
ST_runVelMove(stHandle, ctrlHandle);
#endif
ST_runVelCtl(stHandle, ctrlHandle);
stCnt = 1;
}
//trace(11);
// run the controller
CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);
// write the PWM compare values
HAL_writePwmData(halHandle,&gPwmData);
// setup the controller
CTRL_setup(ctrlHandle);
GPIO_setLow(halHandle->gpioHandle,GPIO_Number_8);
return;
} // end of mainISR() function
void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle sthandle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
ST_Obj *stObj = (ST_Obj *)sthandle;
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// get the magnetizing current
gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);
// get the rotor resistance
gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);
// get the stator resistance
gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);
// get the stator inductance in the direct coordinate direction
gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);
// get the stator inductance in the quadrature coordinate direction
gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);
// get the flux in V/Hz in floating point
gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
// get the Iq reference from the speed controller
gMotorVars.IqRef_A = _IQmpy(STVELCTL_getTorqueReference(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
// gets the Velocity Controller status
gMotorVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle);
// get the inertia setting
gMotorVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));
// get the friction setting
gMotorVars.SpinTAC.FrictionEstimate_Aperkrpm = _IQmpy(STVELCTL_getFriction(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));
// get the Velocity Controller error
gMotorVars.SpinTAC.VelCtlErrorID = STVELCTL_getErrorID(stObj->velCtlHandle);
#ifndef LEARN
// get the Velocity Move status
gMotorVars.SpinTAC.VelMoveStatus = STVELMOVE_getStatus(stObj->velMoveHandle);
// get the Velocity Move profile time
gMotorVars.SpinTAC.VelMoveTime_ticks = STVELMOVE_getProfileTime_tick(stObj->velMoveHandle);
// get the Velocity Move error
gMotorVars.SpinTAC.VelMoveErrorID = STVELMOVE_getErrorID(stObj->velMoveHandle);
#endif
return;
} // end of updateGlobalVariables_motor() function
void updateKpKiGains(CTRL_Handle handle)
{
if((gMotorVars.CtrlState == CTRL_State_OnLine) && (gMotorVars.Flag_MotorIdentified == true) && (Flag_Latch_softwareUpdate == false))
{
// set the kp and ki speed values from the watch window
CTRL_setKp(handle,CTRL_Type_PID_spd,gMotorVars.Kp_spd);
CTRL_setKi(handle,CTRL_Type_PID_spd,gMotorVars.Ki_spd);
// set the kp and ki current values for Id and Iq from the watch window
CTRL_setKp(handle,CTRL_Type_PID_Id,gMotorVars.Kp_Idq);
CTRL_setKi(handle,CTRL_Type_PID_Id,gMotorVars.Ki_Idq);
CTRL_setKp(handle,CTRL_Type_PID_Iq,gMotorVars.Kp_Idq);
CTRL_setKi(handle,CTRL_Type_PID_Iq,gMotorVars.Ki_Idq);
}
return;
} // end of updateKpKiGains() function
void ST_runVelMove(ST_Handle handle, CTRL_Handle ctrlHandle)
{
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Run SpinTAC Move
// If we are not in reset, and the SpeedRef_krpm has been modified
if((EST_getState(ctrlObj->estHandle) == EST_State_OnLine) && (_IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STVELMOVE_getVelocityEnd(stObj->velMoveHandle))) {
// Get the configuration for SpinTAC Move
STVELMOVE_setCurveType(stObj->velMoveHandle, gMotorVars.SpinTAC.VelMoveCurveType);
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setAccelerationLimit(stObj->velMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setJerkLimit(stObj->velMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
// Enable SpinTAC Move
STVELMOVE_setEnable(stObj->velMoveHandle, true);
// If starting from zero speed, enable ForceAngle, otherwise disable ForceAngle
//if(_IQabs(STVELMOVE_getVelocityStart(stObj->velMoveHandle)) < _IQ(ST_MIN_ID_SPEED_PU)) {
if (global_speed < 100 && global_speed > -100){
EST_setFlag_enableForceAngle(ctrlObj->estHandle, true);
gMotorVars.Flag_enableForceAngle = true;
}
else {
EST_setFlag_enableForceAngle(ctrlObj->estHandle, false);
gMotorVars.Flag_enableForceAngle = false;
}
}
STVELMOVE_run(stObj->velMoveHandle);
}
void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback, iqReference;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
// Run the SpinTAC Controller
#ifndef LEARN
STVELCTL_setVelocityReference(stObj->velCtlHandle, STVELMOVE_getVelocityReference(stObj->velMoveHandle));
STVELCTL_setAccelerationReference(stObj->velCtlHandle, STVELMOVE_getAccelerationReference(stObj->velMoveHandle));
#else
STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); // Internal ramp generator does not provide Acceleration Reference
#endif
STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
STVELCTL_run(stObj->velCtlHandle);
// select SpinTAC Velocity Controller
iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);
// Set the Iq reference that came out of SpinTAC Velocity Control
CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
//@} //defgroup
// end of file
