/* * 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; } } }