|
|
/**
|
|
|
******************************************************************************
|
|
|
* @file Project/STM32F10x_StdPeriph_Template/stm32f10x_it.c
|
|
|
* @author MCD Application Team
|
|
|
* @version V3.5.0
|
|
|
* @date 08-April-2011
|
|
|
* @brief Main Interrupt Service Routines.
|
|
|
* This file provides template for all exceptions handler and
|
|
|
* peripherals interrupt service routine.
|
|
|
******************************************************************************
|
|
|
* @attention
|
|
|
*
|
|
|
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
|
|
|
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
|
|
|
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
|
|
|
* DIRECT, INDIRECT OR CONSEQUENTI
|
|
|
|
|
|
AL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
|
|
|
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
|
|
|
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
|
|
|
*
|
|
|
* <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
|
|
|
******************************************************************************
|
|
|
*/
|
|
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
|
#include "stm32f10x_it.h"
|
|
|
#include "inv_mpu.h"
|
|
|
#include "inv_mpu_dmp_motion_driver.h"
|
|
|
#include "dmpKey.h"
|
|
|
#include "dmpmap.h"
|
|
|
#include "usart.h"
|
|
|
#include "oled.h"
|
|
|
#include "app.h"
|
|
|
#include "usart.h"
|
|
|
|
|
|
/** @addtogroup STM32F10x_StdPeriph_Template
|
|
|
* @{
|
|
|
*/
|
|
|
|
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
|
/* Private define ------------------------------------------------------------*/
|
|
|
/* Private macro -------------------------------------------------------------*/
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
|
/* Private functions ---------------------------------------------------------*/
|
|
|
|
|
|
/******************************************************************************/
|
|
|
/* Cortex-M3 Processor Exceptions Handlers */
|
|
|
/******************************************************************************/
|
|
|
|
|
|
|
|
|
//---------<EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD>գģͣ<EFBFBD><EFBFBD>ж<EFBFBD>---------------
|
|
|
extern uint8_t parameter[48];
|
|
|
union dataConvert PAValue;
|
|
|
union dataConvert IAValue;
|
|
|
union dataConvert DAValue;
|
|
|
union dataConvert PBValue;
|
|
|
union dataConvert IBValue;
|
|
|
union dataConvert DBValue;
|
|
|
extern AnglePid AnglePidA;
|
|
|
extern SpeedPid SpeedPidA;
|
|
|
extern TurnPid TurnPidA;
|
|
|
extern int Movement;
|
|
|
extern int DeathArea;
|
|
|
void DMA1_Channel5_IRQHandler(void) //
|
|
|
{
|
|
|
uint8_t i;
|
|
|
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ
|
|
|
DMA_ClearFlag(DMA1_FLAG_TC5);
|
|
|
DMA_ClearITPendingBit(DMA1_IT_TC5);
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
PAValue.table[i] = parameter[i];
|
|
|
}
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
IAValue.table[i] = parameter[8+i];
|
|
|
}
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
DAValue.table[i] = parameter[i+16];
|
|
|
}
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
PBValue.table[i] = parameter[i+24];
|
|
|
}
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
IBValue.table[i] = parameter[32+i];
|
|
|
}
|
|
|
for(i=0;i<8;i++)
|
|
|
{
|
|
|
DBValue.table[i] = parameter[i+40];
|
|
|
}
|
|
|
// AnglePidA.Kp = PAValue.num;AnglePidA.Ki = IAValue.num;AnglePidA.Kd = DAValue.num;
|
|
|
// SpeedPidA.Kp = PBValue.num;SpeedPidA.Ki = IBValue.num;SpeedPidA.Kd = DBValue.num;
|
|
|
TurnPidA.Kp = PAValue.num;Movement = IAValue.num;TurnPidA.Kd=DAValue.num;
|
|
|
TurnPidA.SetSpeedSum = PBValue.num;
|
|
|
// DeathArea = PAValue.num*10;
|
|
|
}
|
|
|
|
|
|
MPU6050_DMP_PACKET mpu6050_info;
|
|
|
int speed2,speed4;
|
|
|
void EXTI4_IRQHandler(void)
|
|
|
{
|
|
|
if(EXTI_GetITStatus(EXTI_Line4)==SET)
|
|
|
{
|
|
|
EXTI_ClearITPendingBit(EXTI_Line4);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
|
|
|
void TIM1_UP_IRQHandler(void)
|
|
|
{
|
|
|
int balance=0;
|
|
|
int speedout=0;
|
|
|
int turn=0;
|
|
|
int outputpwm2,outputpwm4;
|
|
|
int diffSpeedSum;
|
|
|
static uint8_t SpeedControlPeriod=0;
|
|
|
static uint8_t TurnControlPeriod=0;
|
|
|
static int Speed2Sum=0,Speed4Sum=0;
|
|
|
|
|
|
if(TIM_GetFlagStatus(TIM1, TIM_FLAG_Update)==SET)
|
|
|
{
|
|
|
mpu_dmp_get_data(&mpu6050_info);
|
|
|
if(myabs(mpu6050_info.pitch)<55) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ž<EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
{
|
|
|
/*-------------------<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>---------------------*/
|
|
|
SpeedControlPeriod++;
|
|
|
speedout = SpeedControlOutput(&SpeedPidA,SpeedControlPeriod);
|
|
|
TurnControlPeriod++;
|
|
|
turn = TurnControlOutput(&TurnPidA,TurnControlPeriod);
|
|
|
balance = AnglePID_Realize(&AnglePidA,mpu6050_info.pitch,mpu6050_info.gyro[1]); //<EFBFBD>Ƕȿ<EFBFBD><EFBFBD>ƣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5ms
|
|
|
|
|
|
|
|
|
if(SpeedControlPeriod >= SPEED_CONTROL_PERIOD) //<EFBFBD>ٶȺ<EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>100ms
|
|
|
{
|
|
|
SpeedControlPeriod = 0;
|
|
|
TurnControlPeriod = 0;
|
|
|
speed2 = MOTOR2_GetSpeed(); //<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
speed4 = MOTOR4_GetSpeed();
|
|
|
Speed2Sum += speed2; Speed4Sum += speed4;
|
|
|
diffSpeedSum = Speed2Sum - Speed4Sum;
|
|
|
TurnPID_Realize(&TurnPidA,diffSpeedSum,mpu6050_info.gyro[2]);
|
|
|
SpeedPID_Realize(&SpeedPidA,speed2+speed4);
|
|
|
}
|
|
|
|
|
|
|
|
|
outputpwm2 = balance + speedout + turn;
|
|
|
outputpwm4 = balance + speedout - turn;
|
|
|
MotorOutput(outputpwm2,outputpwm4);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
MotorOutput(0,0);
|
|
|
}
|
|
|
|
|
|
|
|
|
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);//
|
|
|
}
|
|
|
}
|
|
|
/**
|
|
|
* @brief This function handles NMI exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void NMI_Handler(void)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles Hard Fault exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void HardFault_Handler(void)
|
|
|
{
|
|
|
/* Go to infinite loop when Hard Fault exception occurs */
|
|
|
while (1)
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles Memory Manage exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void MemManage_Handler(void)
|
|
|
{
|
|
|
/* Go to infinite loop when Memory Manage exception occurs */
|
|
|
while (1)
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles Bus Fault exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void BusFault_Handler(void)
|
|
|
{
|
|
|
/* Go to infinite loop when Bus Fault exception occurs */
|
|
|
while (1)
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles Usage Fault exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void UsageFault_Handler(void)
|
|
|
{
|
|
|
/* Go to infinite loop when Usage Fault exception occurs */
|
|
|
while (1)
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles SVCall exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void SVC_Handler(void)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles Debug Monitor exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void DebugMon_Handler(void)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles PendSVC exception.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void PendSV_Handler(void)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles SysTick Handler.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
void SysTick_Handler(void)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
/******************************************************************************/
|
|
|
/* STM32F10x Peripherals Interrupt Handlers */
|
|
|
/* Add here the Interrupt Handler for the used peripheral(s) (PPP), for the */
|
|
|
/* available peripheral interrupt handler's name please refer to the startup */
|
|
|
/* file (startup_stm32f10x_xx.s). */
|
|
|
/******************************************************************************/
|
|
|
|
|
|
/**
|
|
|
* @brief This function handles PPP interrupt request.
|
|
|
* @param None
|
|
|
* @retval None
|
|
|
*/
|
|
|
/*void PPP_IRQHandler(void)
|
|
|
{
|
|
|
}*/
|
|
|
|
|
|
/**
|
|
|
* @}
|
|
|
*/
|
|
|
|
|
|
|
|
|
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
|
|
|
|
