/*
* Fonction qui reset les QEIs
*/
void resetQEIs(void){
QEIPositionSet(QEI0_BASE,0);
QEIPositionSet(QEI1_BASE,0);
position_m2 = 0;
position_m3 = 0;
previous_state_m2 = 0;
previous_state_m3 = 0;
}
// * enc_pos_set **************************************************************
// * Sets the signed position of the encoder, in units of encoder pulses. *
// * encoder should be ENC_1 or ENC_2. *
// ****************************************************************************
void enc_pos_set(unsigned char encoder, signed long position)
{
if(encoder == ENC_1)
{
QEIPositionSet(ENC_1_BASE, (unsigned long)position); // return encoder 1 reading
}
else if(encoder == ENC_2)
{
QEIPositionSet(ENC_2_BASE, (unsigned long)position); // return encoder 2 reading
}
// do nothing for invalid encoder ID
}
void EncoderWhile2Point(enum MOTOR motor,enum A3906Logic driction,unsigned long point)
{
unsigned long ulpos;
//tContext sContext;
unsigned long ulBase = (motor == PITCH_MOTOR ? QEI0_BASE : QEI1_BASE);
//
// Save the time.
//
ulpos = QEIPositionGet(ulBase);
if(ulpos > point)
GeneralPitchRoll(motor,A3906_REVERSE,30);
else
if(ulpos < point)
GeneralPitchRoll(motor,A3906_FORWARD,30);
//MotorBrakePositionSet(motor,point);
while(ulpos != point)
ulpos = QEIPositionGet(ulBase);
GeneralPitchRoll(motor,A3906_BRAKE,99);
QEIPositionSet(ulBase, 0);
}
void _EncoderInit(){
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI0);
SysCtlDelay(1);
HWREG(GPIO_PORTD_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(GPIO_PORTD_BASE + GPIO_O_CR) |= 0x80;
//Set Pins to be PHA0 and PHB0
GPIOPinConfigure(GPIO_PD6_PHA0);
GPIOPinConfigure(GPIO_PD7_PHB0);
GPIOPinTypeQEI(GPIO_PORTD_BASE, GPIO_PIN_6 | GPIO_PIN_7);
QEIDisable(QEI0_BASE);
QEIIntDisable(QEI0_BASE,QEI_INTERROR | QEI_INTDIR | QEI_INTTIMER | QEI_INTINDEX);
QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET | QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 1000);
QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1,80000000/12);
QEIEnable(QEI0_BASE);
QEIVelocityEnable(QEI0_BASE);
//Set position to a middle value so we can see if things are working
QEIPositionSet(QEI0_BASE, 500);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI1);
SysCtlDelay(1);
//Set Pins to be PHA0 and PHB0
GPIOPinConfigure(GPIO_PC5_PHA1);
GPIOPinConfigure(GPIO_PC6_PHB1);
GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_5 | GPIO_PIN_6);
QEIDisable(QEI1_BASE);
QEIIntDisable(QEI1_BASE,QEI_INTERROR | QEI_INTDIR | QEI_INTTIMER | QEI_INTINDEX);
QEIConfigure(QEI1_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET | QEI_CONFIG_QUADRATURE | QEI_CONFIG_SWAP), 1000);
QEIVelocityConfigure(QEI1_BASE, QEI_VELDIV_1,80000000/12);
QEIEnable(QEI1_BASE);
QEIVelocityEnable(QEI1_BASE);
//Set position to a middle value so we can see if things are working
QEIPositionSet(QEI1_BASE, 500);
}
void initQEI(void){
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); //PhA sur C4 et PhB sur C6
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); //PhA sur E3 et PhB sur E2
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI1);
//init QEI0 du micro pour moteur 0
GPIOPinConfigure(GPIO_PC4_PHA0);
GPIOPinConfigure(GPIO_PC6_PHB0);
GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_4); //Ph A
GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_6); //Ph B
QEIDisable(QEI0_BASE);
QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET|
QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 1000000); //64 counts par révolution de moteur avec un ratio 100:1
QEIEnable(QEI0_BASE);
QEIPositionSet(QEI0_BASE,0);
QEIVelocityDisable(QEI0_BASE);
QEIVelocityConfigure (QEI0_BASE, QEI_VELDIV_1, ROM_SysCtlClockGet()*dt);
QEIVelocityEnable(QEI0_BASE);
//init QEI1 du micro pour moteur 1
GPIOPinConfigure(GPIO_PE3_PHA1);
GPIOPinConfigure(GPIO_PE2_PHB1);
GPIOPinTypeQEI(GPIO_PORTE_BASE, GPIO_PIN_3); //Ph A
GPIOPinTypeQEI(GPIO_PORTE_BASE, GPIO_PIN_2); //Ph B
QEIDisable(QEI1_BASE);
QEIConfigure(QEI1_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET|
QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 1000000); //64 counts par révolution de moteur avec un ratio 100:1
QEIEnable(QEI1_BASE);
QEIPositionSet(QEI1_BASE,0);
QEIVelocityDisable(QEI1_BASE);
QEIVelocityConfigure (QEI1_BASE, QEI_VELDIV_1, ROM_SysCtlClockGet()*dt);
QEIVelocityEnable(QEI1_BASE);
//init decodeur fait a la mitaine pour moteur 2 et 3 (pins J4 et J5 pour moteur 2, J6 et J7 pour moteur 3)
GPIOPadConfigSet(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_6 | GPIO_PIN_7, GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD_WPD);
GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_6 | GPIO_PIN_7);
GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_6 | GPIO_PIN_7, GPIO_BOTH_EDGES);
GPIOPinIntEnable(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_6 | GPIO_PIN_7);
IntEnable(INT_GPIOE);
position_m0 = 0;
position_m1 = 0;
position_m2 = 0;
position_m3 = 0;
}
void SW_IntHandler(void)
{
GPIOPinIntClear(GPIO_PORTC_BASE, GPIO_PIN_5);
printf("Switch PUSH\r\n");
QEIPositionSet(QEI0_BASE, 0);
}
//*****************************************************************************
//
// This function ``sets'' the position of the encoder. This is the position
// against which all further movements of the encoder are measured in a
// relative sense.
//
//*****************************************************************************
void
Encoder1PositionSet(long lPosition)
{
//
// Convert the position into the number of encoder lines.
//
//lPosition = MathMul16x16(lPosition, g_ulEncoderLines * 4);
lPosition = lPosition*( g_ulEncoder1Lines * 4);
//
// Set the encoder position in the quadrature encoder module.
//
QEIPositionSet(QEI1_BASE, lPosition);
}
//*****************************************************************************
//
// This function prepares the quadrature encoder module for capturing the
// position and speed of the motor.
//
//*****************************************************************************
void
Encoder_Init(unsigned long ulBase)
{
if(ulBase == QEI0_BASE) // J6 PITCH Encoder
{
//
// Enable the peripherals QEI example.
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI0);
// unlock GPIO PD7
GPIO_PORTD_LOCK_R = GPIO_LOCK_KEY;
GPIO_PORTD_CR_R = GPIO_PIN_7;
//Enable GPIO for QEI0
//PD3-> IDX0
//PD6-> PHA0
//PD7-> PHB0
GPIOPinConfigure(GPIO_PD3_IDX0);
GPIOPinConfigure(GPIO_PD6_PHA0);
GPIOPinConfigure(GPIO_PD7_PHB0);
GPIOPinTypeQEI(QEI_PITCH_PHA_PORT,QEI_PITCH_PHA_PIN);
GPIOPinTypeQEI(QEI_PITCH_PHB_PORT,QEI_PITCH_PHB_PIN);
GPIOPinTypeQEI(QEI_PITCH_INDEX_PORT,QEI_PITCH_INDEX_PIN);
// Set D 0 and 1 to alternative use
GPIODirModeSet(GPIO_PORTD_BASE, GPIO_PIN_3, GPIO_DIR_MODE_HW);//GPIO_DIR_MODE_HW
GPIODirModeSet(GPIO_PORTD_BASE, GPIO_PIN_6, GPIO_DIR_MODE_HW);
GPIODirModeSet(GPIO_PORTD_BASE, GPIO_PIN_7, GPIO_DIR_MODE_HW);
//
// Configure the QEI module.
//
QEIConfigure(QEI0_BASE,
(QEI_CONFIG_NO_RESET | QEI_CONFIG_CAPTURE_A_B |
QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), QEI0_POSCNT_MAX-1);
//Set to 0
QEIPositionSet(QEI0_BASE, 0);
QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, 500000); //Configure the Velocity capture Module //500000 is 10ms at 50MHz
QEIVelocityEnable(QEI0_BASE); //Enable the Velocity capture Module
QEIIntEnable(QEI0_BASE, QEI_INTDIR | QEI_INTINDEX); //Enable Interrupt when the Timer is reach 0 on Valocity capture mode
QEIEnable(QEI0_BASE);
//
// Configure the encoder input to generate an interrupt on every rising
// edge.
//
//GPIOIntTypeSet(QEI_PITCH_PHA_PORT, QEI_PITCH_PHA_PIN, GPIO_RISING_EDGE);
//GPIOPinIntEnable(QEI_PITCH_PHA_PORT, QEI_PITCH_PHA_PIN);
//IntEnable(QEI_PITCH_PHA_INT);
//Interrupt Enable
//IntEnable(INT_QEI1);
}
else
if(ulBase == QEI1_BASE) // J8 ROLL Encoder
{
//
// Enable the peripherals QEI example.
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI1);
//Enable GPIO for QEI1
//PC4-> IDX0
//PC5-> PHA0
//PC6-> PHB0
GPIOPinConfigure(GPIO_PC4_IDX1);
GPIOPinConfigure(GPIO_PC5_PHA1);
GPIOPinConfigure(GPIO_PC6_PHB1);
GPIOPinTypeQEI(QEI_ROLL_PHA_PORT,QEI_ROLL_PHA_PIN);
GPIOPinTypeQEI(QEI_ROLL_PHB_PORT,QEI_ROLL_PHB_PIN);
GPIOPinTypeQEI(QEI_ROLL_INDEX_PORT,QEI_ROLL_INDEX_PIN);
// Set F 0 and 1 to alternative use
GPIODirModeSet(GPIO_PORTC_BASE, GPIO_PIN_4, GPIO_DIR_MODE_HW);//GPIO_DIR_MODE_HW
GPIODirModeSet(GPIO_PORTC_BASE, GPIO_PIN_5, GPIO_DIR_MODE_HW);
GPIODirModeSet(GPIO_PORTC_BASE, GPIO_PIN_6, GPIO_DIR_MODE_HW);
//
// Configure the QEI module.
//
QEIConfigure(QEI1_BASE,
(QEI_CONFIG_NO_RESET | QEI_CONFIG_CAPTURE_A_B |
QEI_CONFIG_QUADRATURE | QEI_CONFIG_SWAP), QEI1_POSCNT_MAX-1);
//Set to 0
QEIPositionSet(QEI1_BASE, 0);
QEIVelocityConfigure(QEI1_BASE, QEI_VELDIV_1, 500000); //Configure the Velocity capture Module //500000 is 10ms at 50MHz
QEIVelocityEnable(QEI1_BASE); //Enable the Velocity capture Module
QEIIntEnable(QEI1_BASE, QEI_INTDIR | QEI_INTINDEX); //Enable Interrupt when the Timer is reach 0 on Valocity capture mode
QEIEnable(QEI1_BASE);
//
// Configure the encoder input to generate an interrupt on every rising
// edge.
//
//GPIOIntTypeSet(QEI_ROLL_PHA_PORT, QEI_ROLL_PHA_PIN, GPIO_RISING_EDGE);
//GPIOPinIntEnable(QEI_ROLL_PHA_PORT, QEI_ROLL_PHA_PIN);
//IntEnable(QEI_ROLL_PHA_INT);
//Interrupt Enable
//IntEnable(INT_QEI1);
}
}
void EncoderInitReset(unsigned long ulBase,enum MOTOR motor,enum A3906Logic logic)
{
unsigned long start;
tBoolean encChanged;
start = g_ulTickCount;
// move until encoders show no change
encChanged = true;
prevenc = QEIPositionGet(ulBase);
GeneralPitchRoll(motor,logic,motor == PITCH_MOTOR ? ((ulPeriod*30)/100): ((ulPeriod*40)/100));
while(encChanged)
{
while ((g_ulTickCount - start) < 1000); // encoder samples at 500Hz, we check at 10Hz
encChanged = (prevenc != QEIPositionGet(ulBase));
prevenc = QEIPositionGet(ulBase);
start = g_ulTickCount;
}
GeneralPitchRoll(motor,A3906_BRAKE,ulPeriod);
if(ulBase == QEI0_BASE) // PITCH_MOTOR
{
if(logic == A3906_FORWARD)
{
//
// Indicate that an full edge has been seen.
//
//Motor[motor].max_position = QEIPositionGet(QEI0_BASE);
//Motor[motor].avg_position = QEIPositionGet(QEI0_BASE)/2;
if( QEIPositionGet(QEI0_BASE) > FULL_SCALE_PITCH_MOTOR)
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_EDGE_FUALSE) = 0;
else
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_EDGE_FUALSE) = 1;
}
else
{
QEIPositionSet(ulBase, 0);
Motor[motor].min_position = 1000;
Motor[motor].max_position = 120000;
Motor[motor].relative = 45263;
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_EDGE_FUALSE) = 0;
}
}
else // ROLL_MOTOR
{
if(logic == A3906_FORWARD)
{
QEIPositionSet(ulBase, 0);
Motor[motor].min_position = 315500;
Motor[motor].max_position = 156000;
Motor[motor].relative = 230770;
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_EDGE_FUALSE) = 0;
}
else
{
//Motor[motor].max_position = QEIPositionGet(QEI1_BASE);
//Motor[motor].avg_position = (QEIPositionGet(QEI1_BASE)/2)+5500; // Fix symmetry
if( QEIPositionGet(QEI1_BASE) < FULL_SCALE_ROLL_MOTOR)
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_EDGE_FUALSE) = 1;
else
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_EDGE_FUALSE) = 0;
}
}
}