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,((ulPeriod*30)/100));
else
if(ulpos < point)
GeneralPitchRoll(motor,A3906_FORWARD,((ulPeriod*30)/100));
//MotorBrakePositionSet(motor,point);
while(ulpos != point)
{
ulpos = QEIPositionGet(ulBase);
if((ulpos > point))
GeneralPitchRoll(motor,A3906_REVERSE,((ulPeriod*30)/100));
else
if((ulpos < point))
GeneralPitchRoll(motor,A3906_FORWARD,((ulPeriod*30)/100));
}
GeneralPitchRoll(motor,A3906_BRAKE,ulPeriod);
//QEIPositionSet(ulBase, 0);
}
//*****************************************************************************
//
// Gets the current position of the encoder, return
// angle value that represents a number of full revolutions.
//
//*****************************************************************************
float
EncoderDegPositionGet(unsigned long ulBase)
{
//
// Convert the encoder position to a degrees of revolutions and return it.
//
return( QEIPositionGet(ulBase)/ 462.222f);
}
// * enc_pos_get **************************************************************
// * Returns the signed position of the encoder, in units of encoder pulses. *
// * encoder should be ENC_1 or ENC_2. *
// ****************************************************************************
signed long enc_pos_get(unsigned char encoder)
{
signed long retval; // holds value to be returned
if(encoder == ENC_1)
{
retval = (signed long)QEIPositionGet(ENC_1_BASE); // return encoder 1 reading
}
else if(encoder == ENC_2)
{
retval = (signed long)QEIPositionGet(ENC_2_BASE); // return encoder 2 reading
}
else
{
retval = 0; // return for invalid encoder ID
}
return retval; // return the encoder position
}
//*****************************************************************************
//
// Gets the current position of the encoder, specified as a signed 16.16 fixed-
// point value that represents a number of full revolutions.
//
//*****************************************************************************
float
Encoder1PositionGet(void)
{
//
// Convert the encoder position to a number of revolutions and return it.
//
//return(MathDiv16x16(QEIPositionGet(QEI0_BASE), g_ulEncoderLines * 4));
return(QEIPositionGet(QEI1_BASE)/ (g_ulEncoder1Lines * 4));
}
//*****************************************************************************
//
// Gets the current position of the encoder, specified as a signed 16.16 fixed-
// point value that represents a number of full revolutions.
//
//*****************************************************************************
float
EncoderPositionGet(unsigned long ulBase)
{
//
// Convert the encoder position to a number of revolutions and return it.
//
//return(MathDiv16x16(QEIPositionGet(QEI0_BASE), g_ulEncoderLines * 4));
return(QEIPositionGet(ulBase)/ (g_ulEncoderLines * 4));
}
double calc_commanded_angle(int32_t commanded_pos)
{
double ret_angle=0.0;
double error=0.0, pos=0.0;
int32_t pos_right, pos_left;
pos_right = QEIPositionGet(QEI0_BASE);
pos_left = QEIPositionGet(QEI1_BASE);
pos = (double)(pos_right + pos_left);
error = (double)(commanded_pos - pos);
if(abs(error) > 4000.0)
ret_angle = commanded_ang - error / 600.0;
else if(abs(error) > 2000.0)
ret_angle = commanded_ang - error / 800.0;
else if(abs(error) > 500.0)
ret_angle = commanded_ang - error / 1000.0;
else
ret_angle = commanded_ang - error / 500.0;
return ret_angle;
}
//*****************************************************************************
//
// This function is called periodically to determine when the encoder has
// stopped rotating (based on too much time passing between edges).
//
//*****************************************************************************
void
EncoderTick(unsigned long ulBase)
{
if(ulBase == QEI0_BASE)
{
#if 0
//
// See if an edge has been seen since the last call.
//
if(HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_EDGE) == 1)
{
//
// Clear the edge flag.
//
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_EDGE) = 0;
//
// Reset the delay counter.
//
g_usEncoderCount = ENCODER_WAIT_TIME;
}
//
// Otherwise, see if the delay counter is still active.
//
else if(g_usEncoderCount != 0)
{
//
// Decrement the delay counter.
//
g_usEncoderCount--;
//
// If the delay counter has reached zero, then indicate that there are
// no valid speed values from the encoder.
//
if(g_usEncoderCount == 0)
{
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_PREVIOUS) = 0;
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_VALID) = 0;
}
}
#endif
Motor[PITCH_MOTOR].position = QEIPositionGet(QEI0_BASE);
Motor[PITCH_MOTOR].direction= QEIDirectionGet(QEI0_BASE);
Motor[PITCH_MOTOR].aangle= ((Motor[PITCH_MOTOR].position - Motor[PITCH_MOTOR].zero_pos) / 924.444f); // 180
//
// If the previous edge time was valid, then indicate that the time between
// edges is also now valid.
//
if(HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_PITCH) == 1)
{
if(QEIPositionGet(QEI0_BASE) == Motor[PITCH_MOTOR].stowpoint)
{
GeneralPitchRoll(PITCH_MOTOR,A3906_BRAKE,ulPeriod);
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_PITCH) = 0;
}
else
if((QEIPositionGet(QEI0_BASE) > Motor[PITCH_MOTOR].stowpoint))
GeneralPitchRoll(PITCH_MOTOR,A3906_REVERSE,((ulPeriod*25)/100));
else
if((QEIPositionGet(QEI0_BASE) < Motor[PITCH_MOTOR].stowpoint))
GeneralPitchRoll(PITCH_MOTOR,A3906_FORWARD,((ulPeriod*25)/100));
}
}
else
if(ulBase == QEI1_BASE)
{
#if 0
//
// See if an edge has been seen since the last call.
//
if(HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_EDGE) == 1)
{
//
// Clear the edge flag.
//
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_EDGE) = 0;
//
// Reset the delay counter.
//
g_usEncoder1Count = ENCODER_WAIT_TIME;
}
//
// Otherwise, see if the delay counter is still active.
//
else if(g_usEncoder1Count != 0)
{
//
// Decrement the delay counter.
//
g_usEncoder1Count--;
//
// If the delay counter has reached zero, then indicate that there are
// no valid speed values from the encoder.
//
if(g_usEncoder1Count == 0)
{
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_PREVIOUS) = 0;
HWREGBITH(&g_usEncoder1Flags, ENCODER_FLAG_VALID) = 0;
}
}
#endif
Motor[ROLL_MOTOR].position = QEIPositionGet(QEI1_BASE);
Motor[ROLL_MOTOR].direction= QEIDirectionGet(QEI1_BASE);
Motor[ROLL_MOTOR].aangle= ((Motor[ROLL_MOTOR].position - Motor[ROLL_MOTOR].zero_pos) / 924.444f); // 180
//
// If the previous edge time was valid, then indicate that the time between
// edges is also now valid.
//
if(HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_ROLL) == 1)
{
if(QEIPositionGet(QEI1_BASE) == Motor[ROLL_MOTOR].stowpoint)
{
GeneralPitchRoll(ROLL_MOTOR,A3906_BRAKE,ulPeriod);
HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_ROLL) = 0;
}
else
if((QEIPositionGet(QEI1_BASE) < Motor[ROLL_MOTOR].stowpoint))
GeneralPitchRoll(ROLL_MOTOR,A3906_FORWARD,((ulPeriod*25)/100));
else
if((QEIPositionGet(QEI1_BASE) > Motor[ROLL_MOTOR].stowpoint))
GeneralPitchRoll(ROLL_MOTOR,A3906_REVERSE,((ulPeriod*25)/100));
}
}
}
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;
}
}
}
int main(void)
{
long dir;
unsigned long vel, pos;
//
// Set the clocking to run directly from the crystal.
//
SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_8MHZ);
//
// Set up low level I/O for printf()
//
llio_init(115200);
//
// Set up GPIO for LED
//
LED_INIT();
//
// Set up QEI peripheral
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_6);
QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET |
QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 0xffffffff);
QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, SysCtlClockGet() / 10);
QEIEnable(QEI0_BASE);
QEIVelocityEnable(QEI0_BASE);
//
// Set up GPIO for encoder push switch
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeGPIOInput(GPIO_PORTC_BASE, GPIO_PIN_5);
GPIOPadConfigSet(GPIO_PORTC_BASE, GPIO_PIN_5, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);
GPIOIntTypeSet(GPIO_PORTC_BASE, GPIO_PIN_5, GPIO_FALLING_EDGE);
GPIOPortIntRegister(GPIO_PORTC_BASE, SW_IntHandler);
GPIOPinIntEnable(GPIO_PORTC_BASE, GPIO_PIN_5);
//
// Set up the period for the SysTick timer.
//
SysTickPeriodSet(SysCtlClockGet() / 10); // 10Hz SysTick timer
//
// Enable the SysTick Interrupt.
//
SysTickIntEnable();
SysTickIntRegister(SysTickIntHandler);
//
// Enable SysTick.
//
SysTickEnable();
printf("\r\nEncoder example\r\n");
//
// Loop forever.
//
while(1)
{
SysCtlSleep(); // sleep here till interrupt
LED_TOGGLE();
dir = QEIDirectionGet(QEI0_BASE);
vel = QEIVelocityGet(QEI0_BASE);
pos = QEIPositionGet(QEI0_BASE);
printf("%d,%d,%d\r\n", dir, vel, pos);
}
}
void Stabilize(enum MOTOR motor)
{
if (motor == ROLL_MOTOR)
{
///////////////////////////////////////////////////////////////////////////////////
//////////////////// ROLL /////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
#ifdef STABILAZER_RELEASE
if( ( QEIPositionGet(QEI1_BASE) >= Motor[motor].min_position) && (horizontal > 0.0f) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod);
else
if( (QEIPositionGet(QEI1_BASE) <= Motor[motor].max_position) && (horizontal < 0.0f) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod); // Set BIT
else
#endif
{
ref_pos_roll = omega_horizontal;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ADD HERE THE CODE WHICH DISABLES THE MOTOR WHEN IT REACHES THE LIMITATION
#ifdef USE_BMA180_INTERRUPT
y_rate_roll = ((ITG3200values[Y_AXIS] - Y_AT_REST) * (FULL_SCALE_RANGE / 32768.0f))*cosf(Thetac) - ((ITG3200values[X_AXIS] - X_AT_REST) * (FULL_SCALE_RANGE / 32768.0f))*sinf(Thetac);
//i_rate_roll+= k_ig_roll*(y_rate_roll - ;
#else
y_rate_roll = ((ITG3200values[Y_AXIS] - Y_AT_REST) * (FULL_SCALE_RANGE / 32768.0f)) - ((ITG3200values[X_AXIS] - X_AT_REST) * (FULL_SCALE_RANGE / 32768.0f));
#endif
#if 1
if ((y_rate_roll < k_8_roll) && (y_rate_roll > k_9_roll)) // 2.0f
{
//k_g_roll = k_g_roll_0;
k_i_roll = k_i_roll_0;
//g_bKdirection = TRUE;
//io_rate_roll = g_fmindutycycle[ROLL_MOTOR][A3906_FORWARD];
//DEBUG_LED1(0);
}
else
k_i_roll = k_i_roll_1;
//k_g_roll = k_g_roll_1;
#endif
#ifdef USE_BMA180_INTERRUPT
//y_rate_roll - stores current rolling speed
y_pos_roll = Phic * RAD2DEG;
e_pos_roll = ref_pos_roll - y_pos_roll; // in degrees
e_rate_roll = k_a_roll*e_pos_roll-k_g_roll*y_rate_roll;
#else
e_rate_roll = -k_g_roll*y_rate_roll;
#endif
#ifdef STABILAZER_RELEASE
if (fabs(horizontal) > 0.0f)
{
e_rate_roll += horizontal;
omega_horizontal = (Phic * RAD2DEG);
}
#endif
io_rate_roll += e_rate_roll*k_i_roll;
if (io_rate_roll > IL_ROLL_SAT)
io_rate_roll = IL_ROLL_SAT;
else if (io_rate_roll < -IL_ROLL_SAT)
io_rate_roll =-IL_ROLL_SAT;
u_rate_roll = e_rate_roll + io_rate_roll;
if (u_rate_roll > DL_ROLL_SAT)
out_rate_roll = DL_ROLL_SAT;
else if (u_rate_roll < -DL_ROLL_SAT)
out_rate_roll =-DL_ROLL_SAT;
else
out_rate_roll = u_rate_roll;
//if (fabs(out_rate_roll) < k_7_roll) // 0.2
//GeneralPitchRoll(ROLL_MOTOR,A3906_DISABLE,dcycle[motor]);
//else
//{
if (out_rate_roll > 0.0f)
{
#ifdef LOAD_IO
if((mdirection[ROLL_MOTOR] != A3906_FORWARD) && (horizontal > 0))
io_rate_roll = g_fmindutycycle[ROLL_MOTOR][A3906_FORWARD];
#endif
g_uldutycycle = ( fabs(out_rate_roll) * ulPeriod ) / 100.0f;
if( (QEIPositionGet(QEI1_BASE) >= Motor[motor].min_position) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod);
else
GeneralPitchRoll(motor,A3906_FORWARD, g_uldutycycle);
}
else
{
#ifdef LOAD_IO
if((mdirection[ROLL_MOTOR] != A3906_REVERSE) && (horizontal < 0))
io_rate_roll = g_fmindutycycle[ROLL_MOTOR][A3906_REVERSE];
#endif
g_uldutycycle = ( fabs(out_rate_roll) * ulPeriod ) / 100.0f;
if( (QEIPositionGet(QEI1_BASE) <= Motor[motor].max_position) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod); // Set BIT
else
GeneralPitchRoll(motor,A3906_REVERSE, g_uldutycycle);
}
//}
}
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////// END of ROLL ////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
else
{
//////////////////////////////////////////////////////////////////////////////////
//////////////////// PITCH ///////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
#ifdef STABILAZER_RELEASE
if( (QEIPositionGet(QEI0_BASE) >= Motor[motor].max_position) && (vertical > 0.0f) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod);
else
if( (QEIPositionGet(QEI0_BASE) <= Motor[motor].min_position) && (vertical < 0.0f) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod); // Set BIT
else
#endif
{
ref_pos_pitch = omega_vertical;
y_rate_pitch = ((ITG3200values[Z_AXIS] - Z_AT_REST)*(FULL_SCALE_RANGE/32768.0f));
#if 1
if ((y_rate_pitch < k_8_pitch) && (y_rate_pitch > k_9_pitch)) // 2.0f
{
//k_g_roll = k_g_roll_0;
k_i_pitch = k_i_pitch_0;
//g_bKdirection = TRUE;
//io_rate_roll = g_fmindutycycle[ROLL_MOTOR][A3906_FORWARD];
//DEBUG_LED1(0);
}
else
k_i_pitch = k_i_pitch_1;
//k_g_roll = k_g_roll_1;
#endif
#ifdef USE_BMA180_INTERRUPT
y_pos_pitch = Thetac * RAD2DEG;
e_pos_pitch = ref_pos_pitch - y_pos_pitch;
e_rate_pitch = k_a_pitch*e_pos_pitch-k_g_pitch*y_rate_pitch;
#else
e_rate_pitch = -k_g_pitch*y_rate_pitch;
#endif
#ifdef STABILAZER_RELEASE
if (fabs(vertical) > 0.0f)
{
//if( (vertical > 0) && (pre_vertical < 0))
//u_rate_pitch = vertical;
//else if( (vertical < 0) && (pre_vertical > 0))
//u_rate_pitch = vertical;
//else
e_rate_pitch += vertical;
//pre_vertical = vertical;
omega_vertical = (Thetac * RAD2DEG);
}
#endif
io_rate_pitch += e_rate_pitch*k_i_pitch;
if (io_rate_pitch > IL_PITCH_SAT)
io_rate_pitch = IL_PITCH_SAT;
else if (io_rate_pitch < -IL_PITCH_SAT)
io_rate_pitch =-IL_PITCH_SAT;
u_rate_pitch = e_rate_pitch + io_rate_pitch;
if (u_rate_pitch > DL_PITCH_SAT)
out_rate_pitch = DL_PITCH_SAT;
else if (u_rate_pitch < -DL_PITCH_SAT)
out_rate_pitch = -DL_PITCH_SAT;
else
out_rate_pitch = u_rate_pitch;
//if (fabs(out_rate_pitch) < k_7_pitch) // 0.2
//GeneralPitchRoll(PITCH_MOTOR,A3906_DISABLE,dcycle[motor]);
//else
//{
if (out_rate_pitch > 0.0f)
{
#ifdef LOAD_IO
if((mdirection[PITCH_MOTOR] != A3906_FORWARD) && (vertical > 0))
io_rate_pitch = g_fmindutycycle[PITCH_MOTOR][A3906_FORWARD];
#endif
g_uldutycycle = ( fabs(out_rate_pitch) * ulPeriod ) / 100.0f;
if( (QEIPositionGet(QEI0_BASE) >= Motor[motor].max_position) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod);
else
GeneralPitchRoll(motor,A3906_FORWARD, g_uldutycycle);
}
else
{
#ifdef LOAD_IO
if((mdirection[PITCH_MOTOR] != A3906_REVERSE) && (vertical < 0))
io_rate_pitch = g_fmindutycycle[PITCH_MOTOR][A3906_REVERSE];
#endif
g_uldutycycle = ( fabs(out_rate_pitch) * ulPeriod ) / 100.0f;
if( (QEIPositionGet(QEI0_BASE) <= Motor[motor].min_position) && (ignorejoystick == false) )
GeneralPitchRoll(motor,A3906_DISABLE,ulPeriod); // Set BIT
else
GeneralPitchRoll(motor,A3906_REVERSE, g_uldutycycle);
}
//}
}
}
///////////////////////////////////////////////////////////////////////////////////
//////////////////// END of PITCH //////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
}
