/*
** ===================================================================
** Method : Bits1_SetBit (component BitsIO)
**
** Description :
** This method sets (sets to one) the specified bit of the
** output value.
** [ It is the same as "PutBit(Bit,TRUE);" ]
** a) direction = Input : sets the specified bit to "1";
** this operation will be shown on
** output after the direction has
** been switched to output
** (SetDir(TRUE);)
** b) direction = Output : directly writes "1" to the
** appropriate pin
** Parameters :
** NAME - DESCRIPTION
** Bit - Number of the bit to set (0 to 1)
** Returns : Nothing
** ===================================================================
*/
void Bits1_SetBit(byte Bit)
{
register byte Mask=Bits1_GetMsk(Bit); /* Temporary variable - bit mask */
if (Mask) { /* Is bit mask correct? */
Shadow_GPIO_B_DATA |= Mask; /* Set appropriate bit in shadow variable */
setRegBits(GPIO_B_DATA,Mask); /* Set appropriate bit on port */
}
}
/*
** ===================================================================
** Method : Bits1_SetDir (component BitsIO)
**
** Description :
** This method sets direction of the bean.
** Parameters :
** NAME - DESCRIPTION
** Dir - Direction to set (FALSE or TRUE)
** FALSE = Input, TRUE = Output
** Returns : Nothing
** ===================================================================
*/
void Bits1_SetDir(bool Dir)
{
if (Dir) { /* Is given direction output? */
setReg(GPIO_B_DATA,((getReg(GPIO_B_DATA)) & ~Bits1_PIN_MASK)|(Shadow_GPIO_B_DATA & Bits1_PIN_MASK)); /* Restore correct value of output from shadow variable */
setRegBits(GPIO_B_DDIR,Bits1_PIN_MASK); /* Set direction to output */
}
else { /* Is direction input? */
clrRegBits(GPIO_B_DDIR,Bits1_PIN_MASK); /* Set direction to input */
}
}
/*
** ===================================================================
** Method : LED_SetDir (component BitIO)
**
** Description :
** This method sets direction of the bean.
** Parameters :
** NAME - DESCRIPTION
** Dir - Direction to set (FALSE or TRUE)
** FALSE = Input, TRUE = Output
** Returns : Nothing
** ===================================================================
*/
void LED_SetDir(bool Dir)
{
if (Dir) { /* Is given direction output? */
setReg(GPIO_A_DR,((getReg(GPIO_A_DR)) & ~LED_PIN_MASK) | (Shadow_GPIO_A_DR & LED_PIN_MASK)); /* Restore correct value of output from shadow variable */
setRegBits(GPIO_A_DDR,LED_PIN_MASK); /* Set direction to output */
}
else { /* Is given direction input? */
clrRegBits(GPIO_A_DDR,LED_PIN_MASK); /* Set direction to input */
}
}
/*
** ===================================================================
** Method : LED_PutVal (component BitIO)
**
** Description :
** This method writes the new output value.
** a) direction = Input : sets the new output value;
** this operation will be shown on
** output after the direction has
** been switched to output
** (SetDir(TRUE);)
** b) direction = Output : directly writes the value to the
** appropriate pin
** Parameters :
** NAME - DESCRIPTION
** Val - Output value. Possible values:
** FALSE - logical "0" (Low level)
** TRUE - logical "1" (High level)
** Returns : Nothing
** ===================================================================
*/
void LED_PutVal(bool Val)
{
if (Val) { /* Is it one to be written? */
Shadow_GPIO_A_DR |= LED_PIN_MASK; /* Set bit in shadow variable */
setRegBits(GPIO_A_DR,LED_PIN_MASK); /* Set bit on port */
}
else { /* Is it zero to be written? */
Shadow_GPIO_A_DR &= ~LED_PIN_MASK; /* Clear bit in shadow variable */
clrRegBits(GPIO_A_DR,LED_PIN_MASK); /* Clear bit on port */
}
}
/**
* sets the clock prescaler.
* @param presc is the prescaler value in range 0-3 that mean divisors 1 to 8.
* @return ERR_OK if successful.
*/
byte PWMC1_setPrescaler(byte presc)
{
if (presc < 4)
{
clrRegBits (PWMB_PMCTL, 0x00c0);
setRegBits (PWMB_PMCTL, (presc << 6));
///setRegBitGroup(PWMB_PMCTL, PRSC, presc);
return ERR_OK;
}
else
return ERR_RANGE;
}
/**
* initializes the timers C0(PHA), C1(PHB), D2(INDEX) for quadrature decoding.
*/
void QD3_init (void)
{
/* TMRD0_CTRL: CM=0,PCS=0,SCS=1,ONCE=0,LENGTH=0,DIR=0,Co_INIT=0,OM=0 */
setReg(TMRD0_CTRL,128); /* Set up mode */
/* TMRD0_SCR: TCF=0,TCFIE=0,TOF=0,TOFIE=0,IEF=0,IEFIE=0,IPS=0,INPUT=0,Capture_Mode=0,MSTR=0,EEOF=0,VAL=0,FORCE=0,OPS=0,OEN=0 */
setReg(TMRD0_SCR,0);
setReg(TMRD0_CNTR,32000); /* Reset counter register */
setReg(TMRD0_LOAD,0); /* Reset load register */
setReg(TMRD0_CMP1,65535); /* Set up compare 1 register */
setReg(TMRD0_CMP2,0); /* Set up compare 2 register */
setRegBits(TMRD0_CTRL,0x8000); /* Run counter */
}
/*
** ===================================================================
** Method : HWEnDi (component ADC)
**
** Description :
** Enables or disables the peripheral(s) associated with the
** component. The method is called automatically as a part of
** Enable, Disable and Init methods and several internal methods.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void HWEnDi(void)
{
if (AD1_ModeFlg) { /* Launch measurement? */
OutFlg = FALSE; /* Measured values are available */
setRegBits(ADC_ADSTAT,2048); /* Clear flag */
clrRegBit(ADC_ADCR1,STOP0); /* Normal operation mode */
setRegBit(ADC_ADCR1,START0); /* Launching of conversion */
}
else {
setRegBit(ADC_ADCR1,STOP0); /* Stop command issued */
}
}
void init_faults(bool internal_fault_enable, bool external_fault_enable, bool interrupts_enable)
{
#warning //so far we are not using the DRV1_2_3_4FAULT
setReg (PWMA_PMFCTL, 0);
setReg (PWMA_PMDISMAP1, 0);
setReg (PWMA_PMDISMAP2, 0);
setReg (PWMB_PMFCTL, 0);
setReg (PWMB_PMDISMAP1, 0);
setReg (PWMB_PMDISMAP2, 0);
if (internal_fault_enable==true)
{
// manual fault reset
// FAULT0 signal disables PWMx channels 0123
// FAULT1 signal disables PWMx channels 0123
setRegBits (PWMA_PMDISMAP1, 0x1111);//setRegBits (PWMA_PMDISMAP1, 0b0001000100010001);
setRegBits (PWMA_PMDISMAP2, 0x00);
setRegBits (PWMB_PMDISMAP1, 0x1111);
setRegBits (PWMB_PMDISMAP2, 0x00);
if (interrupts_enable==true)
{
setRegBits (PWMA_PMFCTL,PWMA_PMFCTL_FIE0_MASK |
PWMA_PMFCTL_FIE3_MASK);
setRegBits (PWMB_PMFCTL,PWMB_PMFCTL_FIE0_MASK |
PWMB_PMFCTL_FIE3_MASK);
}
}
if (external_fault_enable==true)
{
setRegBits (PWMA_PMDISMAP1, 0x8888);
setRegBits (PWMA_PMDISMAP2, 0);
setRegBits (PWMB_PMDISMAP1, 0x8888);
setRegBits (PWMB_PMDISMAP2, 0);
}
}
/*
** ===================================================================
** Method : HWEnDi (component SynchroMaster)
**
** Description :
** Enables or disables the peripheral(s) associated with the bean.
** The method is called automatically as a part of the Enable and
** Disable methods and several internal methods.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void HWEnDi(void)
{
if (EnUser) { /* Enable device? */
setRegBit(QSPI0_SCTRL,SPE); /* Enable device */
setRegBits(GPIO_C_PEREN,0x0200); /* Switch pin to peripheral */
if (SerFlag & FULL_TX) { /* Is any char in transmit buffer? */
setReg(QSPI0_DXMIT,BufferWrite); /* Store char to transmitter register */
SerFlag &= ~FULL_TX; /* Zeroize FULL_TX flag */
}
}
else {
clrRegBits(GPIO_C_PEREN,0x0200); /* Switch pin to GPIO */
clrRegBit(QSPI0_SCTRL,SPE); /* Disable device */
}
}
/*
** ===================================================================
** Method : Bits1_PutBit (component BitsIO)
**
** Description :
** This method writes the new value to the specified bit
** of the output value.
** a) direction = Input : sets the value of the specified
** bit; this operation will be
** shown on output after the
** direction has been switched to
** output (SetDir(TRUE);)
** b) direction = Output : directly writes the value of the
** bit to the appropriate pin
** Parameters :
** NAME - DESCRIPTION
** Bit - Number of the bit (0 to 1)
** Val - New value of the bit (FALSE or TRUE)
** FALSE = "0" or "Low", TRUE = "1" or "High"
** Returns : Nothing
** ===================================================================
*/
void Bits1_PutBit(byte Bit, bool Val)
{
register byte Mask=Bits1_GetMsk(Bit); /* Temporary variable - bit mask */
if (Mask) { /* Is bit mask correct? */
if (Val) { /* Is it one to be written? */
Shadow_GPIO_B_DATA |= Mask; /* Set appropriate bit in shadow variable */
setRegBits(GPIO_B_DATA,Mask); /* Set appropriate bit on port */
}
else { /* Is it zero to be written? */
Shadow_GPIO_B_DATA &= ~Mask; /* Clear appropriate bit in shadow variable */
clrRegBits(GPIO_B_DATA,Mask); /* Clear appropriate bit on port */
}
}
}
/**
* enables triggered sequential mode synchronous with the
* PWM generation signal.
*
**************************************************************************************/
byte AD_enableIntTriggerA(void)
{
if (ad_ModeFlgA != IDLE) /* Is the device in running mode? */
return ERR_BUSY;
/// starts sampling in triggered sequential mode
/// synchro with PWM generation.
setRegBits (ADCA_ADCR1, 0x04);
// clrRegBits (ADCA_ADCR1, 0x03);
ad_ModeFlgA = MEASURE; /* Set state of device to the measure mode */
HWEnDiA();
return ERR_OK;
}
/**
* This method reads a byte from the SPI bus
* @param Chr is the read byte
* @return ERR_OK always
***************************************************************************/
byte SPI1_RecvChar(byte *Chr)
{
int i=0;
byte ToRead=0;
*Chr=0;
//SCLK
setRegBits(GPIO_E_DR,0x10);
wait(4);
for(i=7;i>=0;i--)
{
//SCLK
clrRegBits(GPIO_E_DR,0x10);
wait(4);
ToRead= getRegBits(GPIO_E_DR,0x40);
wait(1);
*Chr |=(ToRead & 1)<<i;
//SCLK
setRegBits(GPIO_E_DR,0x10);
wait(4);
}
wait(4);
//SCLK
setRegBits(GPIO_E_DR,0x10);
}
/**
* gets the encoder position value as a 32 bit integer.
* @param Position is the pointer to the variable holding the value.
* @return ERR_OK always.
*/
byte QD3_getPosition (dword *Position)
{
dword TimerValue=getReg(TMRD0_CNTR);
if (TimerValue>=32000)
{
qd3_position=qd3_position+ (TimerValue-32000);
}
else
{
qd3_position =qd3_position-(32000 - TimerValue);
}
*Position=qd3_position;
setReg(TMRD0_CNTR,32000);
setRegBits(TMRD0_CTRL,0x8000); /* Run counter */
return ERR_OK;
}
/**
* initializes the counter/timer. the timer is initialized w/ 1ms period.
*/
void TD0_init (void)
{
/* TMRD0_CTRL: CM=0,PCS=0,SCS=0,ONCE=0,LENGTH=1,DIR=0,Co_INIT=0,OM=0 */
setReg (TMRD0_CTRL, 0x20); /* Stop all functions of the timer */
/* TMRD0_SCR: TCF=0,TCFIE=1,TOF=0,TOFIE=0,IEF=0,IEFIE=0,IPS=0,INPUT=0,Capture_Mode=0,MSTR=0,EEOF=0,VAL=0,FORCE=0,OPS=0,OEN=0 */
setReg (TMRD0_SCR, 0x4000);
setReg (TMRD0_LOAD, 0); /* Reset load register */
setReg (TMRD0_CMP1, 39999); /* Store appropriate value to the compare register according to the selected high speed CPU mode */
clrRegBits (TMRD0_CTRL, 0x1e00);
setRegBits (TMRD0_CTRL, 0x08 << 9); /* Set prescaler register according to the selected high speed CPU mode */
setReg (TMRD0_CNTR, 0); /* Reset counter */
clrRegBits (TMRD0_CTRL, 0xe000);
TD0_Enable(); /* counter on! */
}
/**
* initializes the PWM module w/ 30KHz indipendent mode.
*
**************************************************************************************/
void PWM_B_init(void)
{
/* PWMB_PMCTL: LDFQ=0,HALF=0,IPOL2=0,IPOL1=0,IPOL0=0,PRSC=0,PWMRIE=0,PWMF=0,ISENS=0,LDOK=0,PWMEN=0 */
setReg (PWMB_PMCTL, 0);
/* PWMB_PMOUT: PAD_EN=0,??=0,OUTCTL=111111,??=0,??=0,OUT=0 */
setReg (PWMB_PMOUT, 0x3F00);
/* PWMB_PMCCR: ENHA=0,??=0,MSK=0,??=0,??=0,VLMODE=0,??=0,SWP45=0,SWP23=0,SWP01=0 */
setReg (PWMB_PMCCR, 0);
/* PWMB_PMCFG: ??=0,??=0,??=0,EDG=0,??=0,TOPNEG45=0,TOPNEG23=0,TOPNEG01=0,??=0,BOTNEG45=0,BOTNEG23=0,BOTNEG01=0,INDEP45=1,INDEP23=1,INDEP01=1,WP=0 */
setReg (PWMB_PMCFG, 0x100E);
/* PWMB_PMDEADTM: ??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,PWMDT=0 */
setReg (PWMB_PMDEADTM, 0);
/* PWMB_PWMVAL0: PWMVAL=0 */
setReg (PWMB_PWMVAL0, 0x0);
/* PWMB_PWMVAL1: PWMVAL=0 */
setReg (PWMB_PWMVAL1, 0x0);
/* PWMB_PWMVAL2: PWMVAL=0 */
setReg (PWMB_PWMVAL2, 0);
/* PWMB_PWMVAL3: PWMVAL=0 */
setReg (PWMB_PWMVAL3, 0x0);
/* PWMB_PWMVAL4: PWMVAL=0 */
setReg (PWMB_PWMVAL4, 0);
/* PWMB_PWMVAL5: PWMVAL=0 */
setReg (PWMB_PWMVAL5, 0x0);
/* PWMB_PWMCM: ??=0,PWMCM=1333 i.e. 30KHz*/
setReg (PWMB_PWMCM, 0x535);
// PWMB_PMCTL: LDQF=2, LDOK=1,PWMEN=1
setRegBits (PWMB_PMCTL, 0x2003); //the load of the PWM duty is setted every 3 cycle (10KHz)
/* write protect on */
//setRegBits (PWMB_PMCFG, PWMB_PMCFG_WP_MASK);
}
/***********************************************************************
This is the main controller loop.
sequences of operations:
- reads from CAN bus or serial port 1.
- reads encoders (or ADC).
- computes the control value (a PID in this version).
- checks limits and other errors.
- sets PWM
- does extra functions (e.g. communicate with neighboring cards).
***********************************************************************/
void main(void)
{
Int32 PWMoutput [JN];
Int32 PWMoutput_old [JN];
byte i=0;
byte wi=0;
byte k=0;
UInt16 *value=0;
Int32 t1val=0;
Int32 PID_R= 2;
Int32 kpp=1;
Int16 current_turn=0;
Int16 print_number=0;
Int16 real_pos=0;
byte first_step=0;
#if (VERSION == 0x0351)
#define winSizeMax 32
#define initialWindowSize 4
#else
#define winSizeMax 32
#define initialWindowSize 30
#endif
byte divJntPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divJntVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte headJntPos[JN]=INIT_ARRAY(0); //current joint pos
byte tailJntPos[JN]=INIT_ARRAY(0);
byte headJntVel[JN]=INIT_ARRAY(0); //current joint vel
byte tailJntVel[JN]=INIT_ARRAY(0);
byte headMotPos[JN]=INIT_ARRAY(0); //current motor pos
byte tailMotPos[JN]=INIT_ARRAY(0);
byte headMotVel[JN]=INIT_ARRAY(0); //current motor vel
byte tailMotVel[JN]=INIT_ARRAY(0);
Int32 jntPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 jntVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int16 _safeband[JN]; //it is a value for reducing the JOINT limit of 2*_safeband [tick encoder]
#ifdef TEMPERATURE_SENSOR
byte TempSensCount1 = 0;
UInt32 TempSensCount2 = 0;
byte temp_sens_status=0;
overtemp[0]=0;
overtemp[1]=0;
errortemp[0]=0;
errortemp[1]=0;
#endif
/* gets the address of flash memory from the linker */
_flash_addr = get_flash_addr();
/* enable interrupts */
setReg(SYS_CNTL, 0);
// IPL channels from 0 to 6 enabled
// external interrupts IRQA and IRQB disabled
setRegBits(IPR, 0xFE00);
// enable FAULT
__ENIGROUP (61, 3);
#if (VERSION == 0x0254)
#else
__ENIGROUP (60, 3);
#endif
// enable SCI
__ENIGROUP (52, 4);
__ENIGROUP (53, 4);
__ENIGROUP (50, 4);
__ENIGROUP (51, 4);
// enable data flash
__ENIGROUP (13, 4);
// enable CAN
__ENIGROUP (14, 6);
__ENIGROUP (15, 6);
__ENIGROUP (16, 6);
__ENIGROUP (17, 6);
// enable ADCA/ADCB
__ENIGROUP (55, 6);
__ENIGROUP (54, 6);
//enable PWM reload
__ENIGROUP (59, 7); // PMWA
#if (VERSION == 0x0254)
#else
__ENIGROUP (58, 7); // PWMB
#endif
// enable timers
// TIMER_A
__ENIGROUP (45, 7); //Timer for the encoder commutation if used
__ENIGROUP (44, 7); //
__ENIGROUP (43, 7); //
__ENIGROUP (42, 4); //TI1 1ms delay main loop
// TIMER_B
__ENIGROUP (41, 7); //
__ENIGROUP (40, 7); //
__ENIGROUP (39, 7); //
__ENIGROUP (38, 7);
// TIMER_C
__ENIGROUP (37, 1);
__ENIGROUP (36, 1);
__ENIGROUP (35, 1);
__ENIGROUP (34, 1);
// TIMER_D
__ENIGROUP (33, 7); //1ms delay duty cycle
__ENIGROUP (32, 1);
__ENIGROUP (31, 1);
__ENIGROUP (30, 1);
__EI();
flash_interface_init (JN);
readFromFlash (_flash_addr);
if (_version==_flash_version)
{
}
else
{
writeToFlash(_flash_addr);
}
__DI();
#warning "debug"// ;
__EI();
init_leds ();
#if (VERSION == 0x0254)
Init_Brushless_Comm (1,HALL);
#else
Init_Brushless_Comm (JN,HALL);
#endif
can_interface_init (JN);
init_strain ();
init_position_abs_ssi ();
#if VERSION ==0x0257
init_relative_position_abs_ssi();
#endif
init_faults (true,true,true);
init_position_encoder ();
TI1_init ();
//variable init
mainLoopOVF=0;
_count=0;
for(i=0;i<JN;i++)
{
_received_pid[i].rec_pid=0;
}
BUS_OFF=false;
#warning "debug"// ;
//__EI();
// print_version ();
/* initialization */
for (i=0; i<JN; i++) _calibrated[i] = false;
/* reset trajectory generation */
for (i=0; i<JN; i++) abort_trajectory (i, 0);
///////////////////////////////////////
// reset of the ABS_SSI
// this is needed because the AS5045 gives the first value wrong !!!
for (i=0; i<JN; i++) _position[i]=(Int32) Filter_Bit(get_position_abs_ssi(i));
for (i=0; i<JN; i++) _max_real_position[i]=Filter_Bit(4095);
//////////////////////////////////////
/* initialize speed and acceleration to zero (useful later on) */
for (i=0; i<JN; i++) _position_old[i] = 0;
for (i=0; i<JN; i++) _speed[i] = 0;
for (i=0; i<JN; i++) _accel[i] = 0;
for (i=0; i<JN; i++) _safeband[i] =5; //5 ticks => 1 grado di AEA.
for (i=0; i<JN; i++) PWMoutput [i] = PWMoutput_old[i] = 0;
/* reset the recursive windows for storage of position and velocity data */
/* (for velocity and position estimates) */
for(i=0;i<JN;i++)
{
for(wi=0;wi<winSizeMax;wi++)
{
jntPosWindow[wi][i]=_position[i];
jntVelWindow[wi][i]=0;
motPosWindow[wi][i]=0;
motVelWindow[wi][i]=0;
}
}
//set_relative_position_abs_ssi(1,get_absolute_real_position_abs_ssi(1));
/* main control loop */
for(_counter = 0;; _counter ++)
{
if (_counter >= CAN_SYNCHRO_STEPS) _counter = 0;
led3_on
while (_wait);
_count=0;
led3_off
// BUS_OFF check
if (getCanBusOffstatus() )
{
#ifdef DEBUG_CAN_MSG
can_printf("DISABLE BUS OFF");
#endif
for (i=0; i<JN; i++) put_motor_in_fault(i);
led1_off
}
else
led1_on
// READING CAN MESSAGES
can_interface();
for (i=0; i<JN; i++)
if (_pad_enabled[i]==false && _control_mode[i]!=MODE_HW_FAULT) _control_mode[i]=MODE_IDLE;
//Position calculation
// This is used to have a shift of the zero-cross out of the
// joint workspace
//
// max_real_position is the limit of the joint starting from
// 4095 and going to decrease this number without zero-cross
// untill the joint limit is reached
#if VERSION == 0x0257
_position_old[0]=_position[0];
if(get_error_abs_ssi(0)==ERR_OK)
_position[0]=Filter_Bit (get_position_abs_ssi(0));
_position_old[1]=_position[1];
if(get_error_abs_ssi(1)==ERR_OK)
_position[1]=Filter_Bit (get_position_abs_ssi(1));
#else
for (i=0; i<JN; i++)
{
_position_old[i]=_position[i];
if(get_error_abs_ssi(i)==ERR_OK)
_position[i]=Filter_Bit (get_position_abs_ssi(i));
}
#endif
// get_commutations() is used to read the incremental encoder of the motors.
// the variable _motor_position is then used to estimate the rotor speed and
// compensate the back-EMF of the motor.
for (i=0; i<JN; i++) _motor_position[i]=get_position_encoder(i);//get_commutations(i);
///////////////////////////////////////////DEBUG////////////
#if (VERSION !=0x0254)
for (i=0; i<JN; i++)
{
if (get_error_abs_ssi(i)==ERR_ABS_SSI)
{
put_motor_in_fault(i);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",i);
#endif
}
}
#endif
#if (VERSION ==0x0254)
if (get_error_abs_ssi(0)==ERR_ABS_SSI)
{
put_motor_in_fault(0);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",0);
#endif
}
#endif
//DO NOTHING
// decoupling the position
decouple_positions();
/* velocity and acceleration estimators */
{
for (i=0; i<JN; i++)
{
//joint velocity estimator
tailJntPos[i]=headJntPos[i]+(winSizeMax-divJntPos[i]); if(tailJntPos[i]>=winSizeMax) tailJntPos[i]=tailJntPos[i]%winSizeMax;
_speed_old[i] = _speed[i];
jntPosWindow[headJntPos[i]][i]=_position[i];
_speed[i] = (Int32) (((jntPosWindow[headJntPos[i]][i] - jntPosWindow[tailJntPos[i]][i] ))<<_jntVel_est_shift[i]);
// _speed[i] <<= _jntVel_est_shift[i];
_speed[i] = (Int32)(_speed[i]) / divJntPos[i];
headJntPos[i]=headJntPos[i]+1; if(headJntPos[i]>=winSizeMax) headJntPos[i]=0;
/*
//joint acceleration estimator
tailJntVel[i]=headJntVel[i]+(winSizeMax-divJntVel[i]); if(tailJntVel[i]>=winSizeMax) tailJntVel[i]=tailJntVel[i]%winSizeMax;
_accel_old[i] = _accel[i];
jntVelWindow[headJntVel[i]][i]=_speed[i];
_accel[i] = ((jntVelWindow[headJntVel[i]][i] - jntVelWindow[tailJntVel[i]][i] ));
_accel[i] << _jntAcc_est_shift[i];
_accel[i] = (Int32)(_accel[i]) / divJntVel[i];
headJntVel[i]=headJntVel[i]+1; if(headJntVel[i]>=winSizeMax) headJntVel[i]=0;
*/
//motor velocity estimator
tailMotPos[i]=headMotPos[i]+(winSizeMax-divMotPos[i]); if(tailMotPos[i]>=winSizeMax) tailMotPos[i]=tailMotPos[i]%winSizeMax;
_motor_speed_old[i] = _motor_speed[i];
motPosWindow[headMotPos[i]][i]=_motor_position[i];
_motor_speed[i] = ((motPosWindow[headMotPos[i]][i] - motPosWindow[tailMotPos[i]][i] ));
_motor_speed[i] <<= _motVel_est_shift[i];
_motor_speed[i] = (_motor_speed[i]) / divMotPos[i];
headMotPos[i]=headMotPos[i]+1; if(headMotPos[i]>=winSizeMax) headMotPos[i]=0;
}
}
/* in position? */
#if (VERSION != 0x0254)
for (i=0; i<JN; i++) _in_position[i] = check_in_position(i);
#else
_in_position[0] = check_in_position(0);
#endif
/* in reference configuration for calibration? */
//for (i=0; i<JN; i++) check_in_position_calib(i);
//******************************************* POSITION LIMIT CHECK ***************************/
for (i=0; i<JN; i++) check_range(i, _safeband[i], PWMoutput);
//******************************************* COMPUTES CONTROLS *****************************/
//FT sensor watchdog update
for (i=0; i<STRAIN_MAX; i++)
if (_strain_wtd[i]>0) _strain_wtd[i]--;
for (i=0; i<JN; i++)
{
//computing the PWM value (PID)
PWMoutput[i] = compute_pwm(i);
// PWM filtering in torque control if there is no bemf compensation
#if (VERSION != 0x0351)
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
if (_useFilter[i] == 3) PWMoutput[i] = lpf_ord1_3hz (PWMoutput[i], i);
}
// saving the PWM value before the decoupling
_bfc_PWMoutput[i] = PWMoutput_old[i] = PWMoutput[i];
// applying the saturation to the PWM
if (_bfc_PWMoutput[i] < -MAX_DUTY) _bfc_PWMoutput[i]=-MAX_DUTY;
else if (_bfc_PWMoutput[i] > MAX_DUTY) _bfc_PWMoutput[i]= MAX_DUTY;
#endif //(VERSION != 0x0351)
}
//decouple PWM
decouple_dutycycle(PWMoutput);
//******************************************* SATURATES CONTROLS ***************************/
/* back emf compensation + controls saturation (if necessary) */
for (i=0; i<JN; i++)
{
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
#if (VERSION != 0x0351)
// Back emf compensation
//PWMoutput[i]+=compensate_bemf(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_bemf(i, _speed[i]); //use the joint speed
//add the coulomb friction compensation term
if (_kstp_torque[i] != 0 ||
_kstn_torque[i] != 0)
//PWMoutput[i]+=compensate_friction(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_friction(i, _speed[i]); //use the joint speed
// Protection for joints out of the admissible range during force control
check_range_torque(i, _safeband[i], PWMoutput);
// PWM saturation
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit_torque[i] );
#else //(VERSION != 0x0351)
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
#endif //(VERSION != 0x0351)
}
else
{
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
}
if (_pid[i] < -MAX_DUTY) _pid[i]=-MAX_DUTY;
else if (_pid[i] > MAX_DUTY) _pid[i]= MAX_DUTY;
}
/* generate PWM */
for (i=0; i<JN; i++)
{
if (!mode_is_idle(i)) {PWM_generate(i,_pid[i]);}
}
/* Check Current done in T1 */
/* do extra functions, communicate, etc. */
//send broadcast data
can_send_broadcast();
//send additional debug information
//can_send_broadcast_debug(1,1);
/***********************************************************************
// Check Current is made here
/***********************************************************************/
#if (VERSION != 0x0254)
for (i=0; i<JN; i++)
#else
for (i=0; i<1; i++)
#endif
{
if ((get_current(i)>=25000) || (get_current(i)<=-25000))
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d curr %f",i,get_current(i));
#endif
}
check_current(i, (_pid[i] > 0));
compute_i2t(i);
if (_filt_current[i] > MAX_I2T_CURRENT)
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d filtcurr %f",i,_filt_current[i]);
#endif
}
}
// Check for the MAIN LOOP duration
// t1val= (UInt16) TI1_getCounter();
if ( _count>0)
{
mainLoopOVF=1;
_count=0;
}
/* tells that the control cycle is completed */
_wait = true;
} /* end for(;;) */
//**********************************************************************
void TD0_Enable(void)
{
setRegBits (TMRD0_CTRL, 0x2000);
}
void PWM_B_Write_Protect()
{
// write protect on
setRegBits (PWMB_PMCFG, PWMB_PMCFG_WP_MASK);
}
/**
* This method inits the LED interface for the 4DC motor board
***************************************************************************/
void init_leds(void)
{
setRegBits(GPIO_A_DDR,0xF0);
clrRegBits(GPIO_A_PER,0xF0);
setRegBits(GPIO_A_DR, 0xF0);
}
void PWM_B_outputPadDisable (word mask)
{
mask &= 0x3F00;
setRegBits(PWMB_PMOUT, mask);
}
/**
* initializes the PWM module w/ 30KHz complementary mode and 8 clock tick dead time.
*/
void PWMC1_init(void)
{
/* PWMB_PMCTL: LDFQ=0,HALF=0,IPOL2=0,IPOL1=0,IPOL0=0,PRSC=0,PWMRIE=0,PWMF=0,ISENS=0,LDOK=0,PWMEN=0 */
setReg (PWMB_PMCTL, 0);
/* PWMB_PMFCTL: ??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,FIE3=0,FMODE3=1,FIE2=0,FMODE2=1,FIE1=0,FMODE1=1,FIE0=0,FMODE0=1 */
/* PWMB_PMDISMAP1: DISMAP=0 */
/* PWMB_PMDISMAP2: ??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,DISMAP=0 */
#ifndef EMERGENCY_DISABLED
setReg (PWMB_PMFCTL, 0x00);
#if VERSION == 0x0111 || VERSION == 0x0114
setReg (PWMB_PMDISMAP1, 0x3333);
#else
setReg (PWMB_PMDISMAP1, 0x2222);
#endif
setReg (PWMB_PMDISMAP2, 0x00);
#else
setReg (PWMB_PMFCTL, 0x55);
setReg (PWMB_PMDISMAP1, 0);
setReg (PWMB_PMDISMAP2, 0);
#endif
/* PWMB_PMOUT: PAD_EN=0,??=0,OUTCTL=0,??=0,??=0,OUT=0 */
setReg (PWMB_PMOUT, 0);
/* PWMB_PMCCR: ENHA=0,??=0,MSK=0,??=0,??=0,VLMODE=0,??=0,SWP45=0,SWP23=0,SWP01=0 */
setReg (PWMB_PMCCR, 0);
/* PWMB_PMCFG: ??=0,??=0,??=0,EDG=1,??=0,TOPNEG45=0,TOPNEG23=0,TOPNEG01=0,??=0,BOTNEG45=0,BOTNEG23=0,BOTNEG01=0,INDEP45=0,INDEP23=0,INDEP01=0,WP=0 */
setReg (PWMB_PMCFG, 0x1000);
/* PWMB_PMDEADTM: ??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,PWMDT=40 */
setReg (PWMB_PMDEADTM, 0x28);
/* PWMB_PWMVAL0: PWMVAL=0 */
setReg (PWMB_PWMVAL0, 0);
/* PWMB_PWMVAL1: PWMVAL=1333 */
setReg (PWMB_PWMVAL1, 0x0535);
/* PWMB_PWMVAL2: PWMVAL=0 */
setReg (PWMB_PWMVAL2, 0);
/* PWMB_PWMVAL3: PWMVAL=1333 */
setReg (PWMB_PWMVAL3, 0x0535);
/* PWMB_PWMVAL4: PWMVAL=0 */
setReg (PWMB_PWMVAL4, 0);
/* PWMB_PWMVAL5: PWMVAL=1333 */
setReg (PWMB_PWMVAL5, 0x0535);
/* PWMB_PWMCM: ??=0,PWMCM=1333 i.e. 30KHz*/
setReg (PWMB_PWMCM, 0x0535);
/* PWMB_PMCTL: LDOK=1,PWMEN=1 */
setRegBits (PWMB_PMCTL, 3);
/* write protect on */
setReg (PWMB_PMCFG, 0x1001);
}
/*
** ===================================================================
** Method : HWEnDi (component AsynchroSerial)
**
** Description :
** Enables or disables the peripheral(s) associated with the bean.
** The method is called automatically as a part of the Enable and
** Disable methods and several internal methods.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void HWEnDi(void)
{
setRegBits(SCI_CTRL1, (SCI_CTRL1_TE_MASK | SCI_CTRL1_RE_MASK)); /* Enable device */
}
/*
** ===================================================================
** Method : HWEnDi (component AsynchroSerial)
**
** Description :
** Enables or disables the peripheral(s) associated with the bean.
** The method is called automatically as a part of the Enable and
** Disable methods and several internal methods.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void HWEnDi(void)
{
setRegBits(SCI_SCICR, (SCI_SCICR_TE_MASK | SCI_SCICR_RE_MASK)); /* Enable device */
}
void PWMC1_InterruptOnReload(void)
{
setRegBits(PWM_SM0_STS,0x1000); /* Clear reload flag */
PWMC1_OnReload(); /* Invoke user event */
}