//---- INT Handlers ----//
void Timer0BIntHandler(void)
{
//
// Clear the timer interrupt flag.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMB_TIMEOUT);
//
// Update the periodic interrupt counter.
//
g_ui32Counter++;
//
// Once NUMBER_OF_INTS interrupts have been received, turn off the
// TIMER0B interrupt.
//
if(g_ui32Counter == NUMBER_OF_INTS)
{
//
// Disable the Timer0B interrupt.
//
ROM_IntDisable(INT_TIMER0B);
//
// Turn off Timer0B interrupt.
//
ROM_TimerIntDisable(TIMER0_BASE, TIMER_TIMB_TIMEOUT);
//
// Clear any pending interrupt flag.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMB_TIMEOUT);
}
}
void
ToneIntHandler(void)
{
ROM_TimerIntClear(TIMER4_BASE, TIMER_A);
//End of tone duration
if(--g_duration <= 0) {
noTone(current_pin);
ROM_TimerIntDisable(TIMER4_BASE, TIMER_TIMA_TIMEOUT);
ROM_TimerIntClear(TIMER4_BASE, TIMER_TIMA_TIMEOUT);
ROM_TimerDisable(TIMER4_BASE, TIMER_A);
}
}
//*****************************************************************************
//
// The interrupt handler for the fourth timer interrupt. (light)
//
//*****************************************************************************
void
Timer3IntHandler(void)
{
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER3_BASE, TIMER_TIMA_TIMEOUT);
// Get the timer value and reset it
ROM_TimerDisable(TIMER5_BASE, TIMER_A);
// Get the timer value
Timer = ROM_TimerValueGet(TIMER5_BASE, TIMER_A);
// Reset the timer value to 65000
ROM_TimerLoadSet(TIMER5_BASE, TIMER_A, 65000); // Timer5
ROM_TimerEnable(TIMER5_BASE, TIMER_A);
//
// Toggle the flag for the light timer.
//
HWREGBITW(&g_ui32InterruptFlags, 2) = 1;
//
// Update the interrupt status.
//
}
//*****************************************************************************
//
// The interrupt handler for the real time clock interrupt.
//
//*****************************************************************************
void
Timer0IntHandler(void)
{
//Real time clock interrupt, keep track of days, hours, minutes, seconds
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//Increment Timer A Count
TimerACount++;
RTC_Seconds = (TimerACount / 10);
RTC_Minutes = (RTC_Seconds / 60);
RTC_Hours = (RTC_Minutes / 60);
RTC_Days = RTC_Hours / 24;
HWREGBITW(&g_ui32PrintFlags, 0) = 1;
/*//
// Use the flags to Toggle the LED for this timer
//
GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_0, g_ui32Flags);*/
//
// Update the interrupt status.
//
}
void Timer0AIntHandler(void)
{
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
led_value ^= LED_BLUE;
ROM_GPIOPinWrite(GPIO_PORTF_BASE, LED_BLUE, led_value);
}
//*****************************************************************************
//
// The interrupt handler for timer 4. This will be called whenever the timer
// count reaches the match value (0 in this example).
//
// TODO: Make sure you hook your ISR to the correct vector in the application
// startup file.
//
//*****************************************************************************
void
Timer4IntHandler(void)
{
//
// Clear the timer interrupt.
//
// TODO: Rework this for the timer you are using in your application.
//
ROM_TimerIntClear(TIMER4_BASE, TIMER_CAPA_MATCH);
//
// TODO: Do whatever your application needs to do when the relevant
// number of edges have been counted.
//
ProcessInterrupt();
//
// The timer is automatically stopped when it reaches the match value
// so re-enable it here.
//
// TODO: Whether you reenable the timer here or elsewhere will be up to
// your particular application.
//
ROM_TimerEnable(TIMER4_BASE, TIMER_A);
}
///
/// routine di servizio del timer0
void Timer0ISR(void){
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Toggle the flag for the first timer.
//
///HWREGBITW(&g_ui32Flags, 0) ^= 1;
//PIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_2, GPIO_PIN_2);
procCom = 1;
tick++;
//
// Use the flags to Toggle the LED for this timer
//
//GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1, g_ui32Flags << 1);
//
// Update the interrupt status on the display.
//
}
void Timer0IntHandler(void){
// Used to countdown from entered time
// Clear the timer interrupt.
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
// Check if time has been reached
if(g_countdownTime == 0){
ROM_IntMasterDisable();
UARTprintf("Time's Up!\n\n");
ROM_IntMasterEnable();
ROM_TimerEnable(TIMER1_BASE, TIMER_A);
ROM_TimerIntDisable(TIMER0_BASE, TIMER_A);
return;
}
// Turn on LED
ROM_GPIOPinWrite(GPIO_PORTF_BASE, LED_RED, LED_RED);
ROM_TimerEnable(TIMER2_BASE, TIMER_A);
// Update the interrupt status on the display.
ROM_IntMasterDisable();
UARTprintf(" %i\n",g_countdownTime);
ROM_IntMasterEnable();
// Decrement counter
g_countdownTime--;
// Turn off LED
//ROM_GPIOPinWrite(GPIO_PORTF_BASE, LED_RED, 0);
}
//*****************************************************************************
//
//! Wide Timer interrupt to handle blinking effect of the RGB
//!
//! This function is called by the hardware interrupt controller on a timeout
//! of the wide timer. This function must be in the NVIC table in the startup
//! file. When called will toggle the enable flag to turn on or off the entire
//! RGB unit. This creates a blinking effect. A wide timer is used since the
//! blink is intended to be visible to the human eye and thus is expected to
//! have a frequency between 15 and 0.1 hz. Currently blink duty is fixed at
//! 50%.
//!
//! \return None.
//
//*****************************************************************************
void
RGBBlinkIntHandler(void)
{
static unsigned long ulFlags;
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(WTIMER5_BASE, TIMER_TIMB_TIMEOUT);
//
// Toggle the flag for the blink timer.
//
ulFlags ^= 1;
if(ulFlags)
{
RGBEnable();
}
else
{
RGBDisable();
}
}
void timer0_int_handler(void) {
// Clear the interrupt flag
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
// Pass the next value to the DAC
dac_write(*readPtr);
readPtr++;
if (readPtr == startPtr + BUFFER_SIZE)
{
readPtr = startPtr;
}
// // Debugging
// if (g_ulTimeStamp % 100 == 0)
// {
// if (testTimer0)
// {
// UARTprintf("Timer 0 is firing\n");
// testTimer0 = 0;
// }
// }
// else
// {
// testTimer0 = 1;
// }
}
void Mode_WTimer3BISR(void){
ROM_TimerIntClear(WTIMER3_BASE, TIMER_CAPB_EVENT);
// Doc trang thai canh ngat
if (GPIOPinRead(GPIO_PORTD_BASE,GPIO_PIN_3)&GPIO_PIN_3)
ui32T_Edgeup_Mode = ROM_TimerValueGet(WTIMER3_BASE, TIMER_B);
else
{
if (firstEdgeMode)
{
firstEdgeMode=0;
return;
}
ui32T_Edgedown_Mode = ROM_TimerValueGet(WTIMER3_BASE, TIMER_B);
i32DeltaT_Mode = (int32_t)(ui32T_Edgedown_Mode - ui32T_Edgeup_Mode);
// UARTPutn(UART0_BASE,i32DeltaT_SStop);
// UARTCharPut(UART0_BASE,'\n');
// if ((i32DeltaT_Mode<125000) && (i32DeltaT_Mode>115000))
// isAuto=true;
// else if ((i32DeltaT_Mode>135000) && (i32DeltaT_Mode<145000))
// isAuto=false;
}
}
//*****************************************************************************
//
// The interrupt handler for the periodic timer interrupt. When the uDMA
// channel is used, interrupts from the periodic timer are used as DMA
// requests, and this interrupt handler is invoked only at the end of all of
// the DMA transfers.
//
//*****************************************************************************
void
Timer0IntHandler(void)
{
unsigned long ulStatus;
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Read the uDMA channel status to verify it is done
//
ulStatus = uDMAChannelModeGet(UDMA_CHANNEL_TMR0A);
if(ulStatus == UDMA_MODE_STOP)
{
//
// Disable the periodic timer and set the done flag
//
ROM_TimerDisable(TIMER0_BASE, TIMER_A);
g_bDoneFlag = 1;
}
//
// Increment a counter to indicate the number of times this handler
// was invoked
//
g_ulTimer0AIntCount++;
}
//*****************************************************************************
//
// The interrupt handler for the first timer interrupt.
//
//*****************************************************************************
void
Timer0IntHandler(void)
{
char cOne, cTwo;
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Toggle the flag for the first timer.
//
HWREGBITW(&g_ui32Flags, 0) ^= 1;
//
// Use the flags to Toggle the LED for this timer
//
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1, g_ui32Flags << 1);
//
// Update the interrupt status on the display.
//
ROM_IntMasterDisable();
cOne = HWREGBITW(&g_ui32Flags, 0) ? '1' : '0';
cTwo = HWREGBITW(&g_ui32Flags, 1) ? '1' : '0';
UARTprintf("\rT1: %c T2: %c", cOne, cTwo);
ROM_IntMasterEnable();
}
// Triggered every SERVO_TIMER_RESOLUTION microseconds
void TimerIntHandlerServos(void) {
// Clear the interrupt
ROM_TimerIntClear(SERVO_TIMER, SERVO_TIMER_TRIGGER);
// SERVO_TIMER_RESOLUTION microseconds have passed, increment each counter by that
// to determine how long to set the pin high for
g_pulseTime += SERVO_TIMER_RESOLUTION;
if(g_pulseTime > SERVO_PERIOD) {
g_pulseTime = 0;
}
// Loop through al servo configs and see if they need to be set low yet
uint8_t i;
for(i=0; i<SERVO_MAX_COUNT; i++) {
servo_t *servo = &g_servos[i];
if(servo->state & SERVO_STATE_ENABLED) {
if(g_pulseTime >= servo->value) {
// End of pulse, set low
ROM_GPIOPinWrite(servo->port, servo->pin, 0);
} else {
// Beginning of pulse, set high
ROM_GPIOPinWrite(servo->port, servo->pin, servo->pin);
}
}
}
}
void ServoClass::ServoIntHandler(void)
{
// Clear the timer interrupt.
ROM_TimerIntClear(SERVO_TIMER, SERVO_TIMER_TRIGGER);
// Get the pulse width value for the current servo from the array
// if we have already serviced all servos (g_iServoNo = MAX_SERVO_NO)
// then this value should be the 20ms period value
unsigned long l_ulPulseWidth = g_ulTicksPerMicrosecond * g_ulServoPulse[g_iServoNo];
// Re-Load the timer with the new pulse width count value
ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, l_ulPulseWidth);
// End the servo pulse set previously (if any)
if(g_iServoNo > 0) // If not the 1st Servo....
{
if (g_ulServoPins[g_iServoNo - 1] != INVALID_SERVO_PIN)
digitalWrite(g_ulServoPins[g_iServoNo - 1], LOW);
}
// Set the current servo pin HIGH
if(g_iServoNo < SERVOS_PER_TIMER)
{
if (g_ulServoPins[g_iServoNo] != INVALID_SERVO_PIN)
digitalWrite(g_ulServoPins[g_iServoNo], HIGH);
g_iServoNo++; // Advance to next servo for processing next time
}
else
{
g_iServoNo = 0; // Start all over again
}
}
void stagInterrupt(void)
{
g_stag.moveLegs();
float a, b, c;
for (int i = 0; i < 4; i++)
{
g_stag.getLeg(i, &a, &b, &c);
/*
puts("Leg ");
puti(i+1);
puts(" Value1: ");
puti(servoPulseOffset[i*3] + a*angleToPulseFactor);
puts(" Value2: ");
puti(servoPulseOffset[i*3 + 1] + b*angleToPulseFactor);
puts(" Value3: ");
puti(servoPulseOffset[i*3 + 2] + c*angleToPulseFactor);
putln();
*/
servoWrite(legPins[i*3], servoPulseOffset[i*3] + a*angleToPulseFactor);
servoWrite(legPins[i*3 + 1], servoPulseOffset[i*3 + 1] + b*angleToPulseFactor);
servoWrite(legPins[i*3 + 2], servoPulseOffset[i*3 + 2] + c*angleToPulseFactor);
}
//putln();
// Sensor feeds are sent from here
ROM_TimerIntClear(WTIMER0_BASE, TIMER_TIMA_TIMEOUT);
}
void SStop_WTimer2AISR(void){
ROM_TimerIntClear(WTIMER2_BASE, TIMER_CAPA_EVENT);
// Doc trang thai canh ngat
if (GPIOPinRead(GPIO_PORTD_BASE,GPIO_PIN_0)&GPIO_PIN_0)
ui32T_Edgeup_SStop = ROM_TimerValueGet(WTIMER2_BASE, TIMER_A);
else
{
if (firstEdgeSStop)
{
firstEdgeSStop=0;
return;
}
ui32T_Edgedown_SStop = ROM_TimerValueGet(WTIMER2_BASE, TIMER_A);
i32DeltaT_SStop = (int32_t)(ui32T_Edgedown_SStop - ui32T_Edgeup_SStop);
// UARTPutn(UART0_BASE,i32DeltaT_SStop);
// UARTCharPut(UART0_BASE,'\n');
if ((i32DeltaT_SStop<125000) && (i32DeltaT_SStop>115000))
SSTOP_STOP;
else if ((i32DeltaT_SStop>135000) && (i32DeltaT_SStop<145000) && isRunning)
//both RF receiver and PC gui must start
SSTOP_START;
}
}
void Timer1IntHandler(void)
{
//debugled(10);
ROM_TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
//debugled(3);
int angle=0;
if(max2>0 && max1>0 && abs(maxi1-maxi2)<PULSE_SAMPLE)
{
//float dt=abs(maxi1-maxi2)/SAMPLING_FREQUENCY;
//angle=asin(dt*lambda/distance)*180/3.1412;
angle=abs(maxi1-maxi2);
}
//debugled(3);
acount++;
//rcount=0;
//ROM_IntMasterDisable();
snprintf(text,sizeof(text),"%3ld,%3ld",rcount,g_ulADCCount);
GrContextForegroundSet(&sDisplayContext, ClrDarkBlue);
GrRectFill(&sDisplayContext, &sRect1);
GrContextForegroundSet(&sDisplayContext, ClrWhite);
//GrRectDraw(&sDisplayContext, &sRect1);
GrContextFontSet(&sDisplayContext, g_pFontCm12);
GrStringDrawCentered(&sDisplayContext,text, -1,
GrContextDpyWidthGet(&sDisplayContext) / 2, 10, 0);
GrContextForegroundSet(&sDisplayContext, ClrDarkBlue);
GrRectFill(&sDisplayContext, &sRect2);
GrContextForegroundSet(&sDisplayContext, ClrWhite);
sprintf(text,"%4d,%4d",buffer_increment,rem);
GrContextFontSet(&sDisplayContext, g_pFontCm12/*g_pFontFixed6x8*/);
GrStringDrawCentered(&sDisplayContext, text, -1,
GrContextDpyWidthGet(&sDisplayContext) / 2,
((GrContextDpyHeightGet(&sDisplayContext) - 24) / 2) + 24,
0);
// GrFlush(&sDisplayContext);
max1=0;
max2=0;
res=0;
res1=0;
maxi1=0;
maxi2=0;
buffer_index=0;
for(i=0;i<buffer_size;i++)
{
buffer[0][i]=0;
buffer[1][i]=0;
}
i=0;
j=0;
ind2=buffer_index-buffer_increment+1;
}
void Timer1A_Interrupt_Handler(void){
// Clear the timer interrupt.
ROM_TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
// Spin the Clock Face
LCD_Clock();
}
void stop_Timer0B(void)
{
ROM_IntDisable(INT_TIMER0B);
ROM_TimerIntDisable(TIMER0_BASE, TIMER_TIMB_TIMEOUT);
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMB_TIMEOUT);
}
/** ISR for Timer 2
* @pre timer configured with initTimer
* @pre global interrupts enabled with HAL_ENABLE_INTERRUPTS()
*/
void Timer2IntHandler(void)
{
// Clear the timer interrupt
ROM_TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
// Call the timerIsr function pointer
timerIsr();
}
//定时器0A周期中断服务函数
void Timer0AIntHandler(void)
{
static uint8_t i = 0;
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT); //清除定时器中断标志
Get_Attitude();//读取MPU6050得到姿态角
if (Pitch > 45 || Pitch < -45) //俯仰角限幅
{
g_stop_fly = 1; //停止
}
if (Roll > 45 || Roll < -45) //横滚角限幅
{
g_stop_fly = 1;
}
X_MODE_Control(); //PID控制
if (g_start == 0)
{
//输出值最大值限幅
if (X_OUT1 > 5000) X_OUT1 = 5000;
else if (X_OUT1 < 2620) X_OUT1 = 2620;
if (X_OUT2 > 5000) X_OUT2 = 5000;
else if (X_OUT2 < 2620) X_OUT2 = 2620;
if (X_OUT3 > 5000) X_OUT3 = 5000;
else if (X_OUT3 < 2620) X_OUT3 = 2620;
if (X_OUT4 > 5000) X_OUT4 = 5000;
else if (X_OUT4 < 2620) X_OUT4 = 2620;
}
PWM_WidthSet(X_OUT1, X_OUT2, X_OUT3, X_OUT4);//PWM输出，电机控制
ui32Counter++; //更新周期中断计数
if (ui32Counter == NUMBER_OF_INTS) //1s闪灯标志
{
g_1s_flag = 1;
if (i == 0)
{
i = 1;
ui32Counter = 0;
LED_off;
}
else
{
i = 0;
ui32Counter = 0;
LED_on;
}
}
}
void Timer0IntHandler()
{
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
buffer[sample] = GPIO_PORTB_DATA_R;
if (++sample >= BUFFER_LEN)
{
stop();
}
}
void Timer4ISR(void) {
/// pulisce le interruzione del timer4
ROM_TimerIntClear(TIMER4_BASE, TIMER_TIMA_TIMEOUT);
/// disabilita la porta D a generare interruzioni
GPIOIntDisable(GPIO_PORTD_BASE, GPIO_INT_PIN_1);
/// disabilita il timer 4
TimerDisable(TIMER4_BASE, TIMER_A);
/// segnala lo scadere del timer
procCom4 = 1;
/// spento il dispositivo
HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + (GPIO_PIN_0 << 2))) = 0;
}
//*****************************************************************************
//
// The interrupt handler for the SoftUART transmit timer interrupt.
//
//*****************************************************************************
void
Timer0AIntHandler(void)
{
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Call the SoftUART transmit timer tick function.
//
SoftUARTTxTimerTick(&g_sUART);
}
/*---------------------------------------------------------------------------*/
void Timer0IntHandler(void)
{
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
count++;
if(etimer_pending()) {
etimer_request_poll();
}
if (--second_countdown == 0) {
current_seconds++;
second_countdown = CLOCK_SECOND;
}
}
void Throttle_WTimer2BISR(void){
ROM_TimerIntClear(WTIMER2_BASE, TIMER_CAPB_EVENT);
// Doc trang thai canh ngat
if (ROM_GPIOPinRead(GPIO_PORTD_BASE,GPIO_PIN_1)&GPIO_PIN_1)
ui32T_Edgeup_Throttle = ROM_TimerValueGet(WTIMER2_BASE, TIMER_B);
else
{
static int32_t sp=0,pre_sp=0;
if (firstEdgeThrottle)
{
firstEdgeThrottle=0;
return;
}
ui32T_Edgedown_Throttle = ROM_TimerValueGet(WTIMER2_BASE, TIMER_B);
i32DeltaT_Throttle = (int32_t)(ui32T_Edgedown_Throttle - ui32T_Edgeup_Throttle);
if ((i32DeltaT_Throttle<70000) || (i32DeltaT_Throttle>170000))
return;
#ifdef DEBUG_DELTAT_THROTTLE
printStep++;
if (printStep==5)
{
RFprint("Throttle sp:%d\r\n", i32DeltaT_Throttle);
printStep = 0;
}
#endif
sp = (i32DeltaT_Throttle-80000)/PULSE2VEL;
if (sp<0)
sp=0;
else if (sp>1000)
sp=1000;
if ((sp - pre_sp > 2)||(sp - pre_sp < -2))
throttleSet(sp);
pre_sp = sp;
#ifdef DEBUG_THROTTLE_SETPOINT
printStep++;
if (printStep==5)
{
RFprint("Throttle sp:%d\r\n", sp);
printStep = 0;
}
#endif
}
}
//*****************************************************************************
//
// The interrupt handler for the timer interrupt.
//
//*****************************************************************************
void
Timer0IntHandler(void)
{
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Toggle the LED.
//
ROM_GPIOPinWrite(TOGGLE_GPIO_BASE, TOGGLE_GPIO_PIN,
(ROM_GPIOPinRead(TOGGLE_GPIO_BASE, TOGGLE_GPIO_PIN) ^
TOGGLE_GPIO_PIN));
}
//*****************************************************************************
//
// The interrupt handler for the timer interrupt.
//
//*****************************************************************************
void
Timer0IntHandler(void)
{
//
// Clear the timer interrupt.
//
ROM_TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//
// Toggle the flag for the timer.
//
HWREGBITW(&g_ulFlags, 0) = 1;
GPIOPinWrite(STP1_BASE, STP1_PLS, STP1_PLS);
SysCtlDelay(SysCtlClockGet() / 500000);
GPIOPinWrite(STP1_BASE, STP1_PLS, 0x00);
}
void Timer5ISR(void)
{
static uint8_t NumSpdSet = 0;
static int32_t SpeedLeft, SpeedRight;
ROM_TimerIntClear(TIMER5_BASE, TIMER_TIMA_TIMEOUT);
SpeedLeft = ROM_QEIVelocityGet(QEI0_BASE) * ROM_QEIDirectionGet(QEI0_BASE);
SpeedRight = ROM_QEIVelocityGet(QEI1_BASE) * ROM_QEIDirectionGet(QEI1_BASE);
PosLeftCount = ((int32_t)ROM_QEIPositionGet(QEI0_BASE)) / 2;
PosRightCount = ((int32_t)ROM_QEIPositionGet(QEI1_BASE)) / 2;
#ifdef PID_SPEED
UARTPutn(UART_Bluetooth.PortName, (int32_t)SpeedLeft);
UARTCharPut(UART_Bluetooth.PortName, '\n');
#endif
#ifdef PID_POSITION
UARTPutn(UART_Bluetooth.PortName, (int32_t)PosLeftCount);
UARTCharPut(UART_Bluetooth.PortName, '\n');
#endif
if (PIDVerLeft.Enable)
{
PIDVerCalc(&PIDVerLeft, &SpeedLeft, 90);
SetPWM(MOTOR_LEFT, DEFAULT, (long)PIDVerLeft.PIDResult);
}
if (PIDVerRight.Enable)
{
PIDVerCalc(&PIDVerRight, &SpeedRight, 90);
SetPWM(MOTOR_RIGHT, DEFAULT, (long)PIDVerRight.PIDResult);
}
NumSpdSet++;
if (NumSpdSet == PIDVerLoop) //PID position
{
NumSpdSet = 0;
if (PIDPosLeft.Enable)
{
PIDPosCalc(&PIDPosLeft, PosLeftCount, avrSpeed);
PIDSpeedSet(&PIDVerLeft, (long)PIDPosLeft.PIDResult);
}
if (PIDPosRight.Enable)
{
PIDPosCalc(&PIDPosRight, PosRightCount, avrSpeed);
PIDSpeedSet(&PIDVerRight, (long)PIDPosRight.PIDResult);
}
}
}
