www.99日本精品片com,久久成人一区二区,免费超碰在线

/* main.c file
編寫者：lisn3188
編譯環境：MDK-Lite Version: 4.23
測試：本程序已在第七實驗室的mini IMU上完成測試
功能：
1.初始化各個傳感器，
2.運行姿態解算和高度測量
3.將解算的姿態和各個傳感器的輸出上傳到 MiniIMU AHRS 測試軟件
4.響應 PC發送的命令
------------------------------------*/
/*----------------------------------------------------------------
修改備忘：(2014.08.28 by flotox)
1/去掉氣壓傳感器數據接口
2/保持幀格式
3/數據輸出頻率調整為20hz
4/調整機頭方向
5/保留陀螺儀標零偏
6/去掉UART1數據輸出
7/只輸出姿態數據結算幀
8/在數據融合時去掉磁力計的影響
9/上電數據即可用,不用等待
----------------------------------------------------------------*/
#include "common.h" //包含所有的驅動頭文件
//上傳數據的狀態機
#define REIMU 0x01 //上傳解算的姿態數據
#define REMOV 0x02 //上傳傳感器的輸出
#define REHMC 0x03 //上傳磁力計的標定值
#define Upload_Speed 100 //數據上傳速度單位 Hz
#define upload_time (1000000/Upload_Speed) //計算上傳的時間。單位為us
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t hx, hy, hz;
int32_t Temperature = 0, Pressure = 0, Altitude = 0;
uint32_t system_micrsecond;
int16_t hmcvalue[3];
u8 state= REIMU; //發送特定幀的狀態機
/**************************實現函數********************************************
*函數原型: int main(void)
*功　　能: 主程序
*******************************************************************************/
int main(void)
{
int16_t Math_hz=0;
unsigned char PC_comm; //PC 命令關鍵字節
float ypr[3]; // yaw pitch roll
/* 配置系統時鐘為72M 使用外部8M晶體+PLL*/
//SystemInit();
delay_init(72); //延時初始化
Initial_LED_GPIO(); //初始化STM32-SDK板子上的LED接口
Initial_PWMLED();
Initial_UART2(115200L);
load_config(); //從flash中讀取配置信息 -->eeprom.c
IIC_Init(); //初始化I2C接口
delay_ms(300); //等待器件上電
//UART1_Put_String("Initialize...\r\n");
IMU_init(); //初始化IMU和傳感器
system_micrsecond=micros();
while(1){ //主循環
//delay_ms(1); //延時，不要算那么快。
IMU_getYawPitchRoll(ypr); //姿態更新
Math_hz++; //解算次數 ++
//-------------上位機------------------------------
//是否到了更新上位機的時間了？
if((micros()-system_micrsecond)>upload_time){
switch(state){
case REIMU:
UART2_ReportIMU((int16_t)(ypr[0]*10.0),(int16_t)(ypr[1]*10.0),
(int16_t)(ypr[2]*10.0),Altitude/10,Temperature,Pressure/10,Math_hz*Upload_Speed);
Math_hz=0;
// state = REMOV; //更改狀態。
break;
case REMOV:
MPU6050_getlastMotion6(&ax, &ay, &az, &gx, &gy, &gz);
HMC58X3_getlastValues(&hx,&hy,&hz);
UART2_ReportMotion(ax,ay,az,gx,gy,gz,hx,hy,hz);
state = REIMU;
if(HMC5883_calib)state = REHMC; //需要發送當前磁力計標定值
break;
default:
UART2_ReportHMC(HMC5883_maxx,HMC5883_maxy,HMC5883_maxz,
HMC5883_minx,HMC5883_miny,HMC5883_minz,0);//發送標定值
state = REIMU;
break;
}//switch(state)
system_micrsecond=micros(); //取系統時間單位 us
LED_Change(); //LED1改變亮度
}
//--------------------------------------------------
//處理PC發送來的命令
if((PC_comm=UART2_CommandRoute())!=0xff)
{
switch(PC_comm){ //檢查命令標識
case Gyro_init: MPU6050_InitGyro_Offset(); break; //讀取陀螺儀零偏
case High_init: break; //氣壓高度清零
case HMC_calib_begin: HMC5883L_Start_Calib(); break; //開始磁力計標定
case HMC_calib: HMC5883L_Save_Calib(); break; //保存磁力計標定
}
}// 處理PC 發送的命令
}//主循環 while(1) 結束
} //main
//------------------End of File----------------------------

復制代碼

/**---------------------------------------------------------------
項目：兩輪自平衡小車(Two-Wheels Auto-Balancing Vechile)
版本：v1.0.0
文件: Balancing.c
功能：自平衡計算
作者：flotox@yeah.net
日期：2014.9.5
更新：2014.9.5
----------------------------------------------------------------*/
#include "stm32f10x.h"
#include "GlobalVars.h"
#include "MotorDriver.h"
// float Temp[200] = {0};
// uint32_t Temp_i = 0;
/**
* @brief 自平衡計算外設初始化
* @param none
* @retval none
*/
void Balancing_Init(void){
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
/*開啟GPIOC外設時鐘*/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
/*開啟TIM4外設時鐘*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
/*配置PC.15端口為Out_PP模式*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
/*TIM4 NVIC設置*/
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //2 bits for pre-emption priority 2 bits for subpriority
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //指定搶占式優先級別,可取0~3
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //指定響應式優先級別,可取0~3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_DeInit(TIM4);
/*定時器基本配置*/
TIM_TimeBaseStructure.TIM_Prescaler = 0; //時鐘預分頻數0,TIM4的時鐘計數頻率為72MHz
TIM_TimeBaseStructure.TIM_Period = 20 - 1; //自動重裝載寄存器數20,10ms溢出1次
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //采樣分頻
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上計數
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_PrescalerConfig(TIM4, 0x8c9f, TIM_PSCReloadMode_Immediate); //時鐘分頻系數36000,定時器時鐘為2KHz
TIM_ARRPreloadConfig(TIM4, DISABLE); //禁止ARR預裝載緩沖器
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //使能Update中斷
TIM_Cmd(TIM4, ENABLE); //開啟時鐘
}
/**
* @brief 自平衡計算循環子程序(10ms被調用一次)
* @param none
* @retval none
*/
void Balancing_Loop(void){
uiCount_Led++;
/*Led閃爍*/
if(uiCount_Led >= 40){
uiCount_Led = 0;
GPIO_WriteBit(GPIOC,GPIO_Pin_15,(BitAction)(1-GPIO_ReadOutputDataBit(GPIOC,GPIO_Pin_15)));
}
switch(iMotion_Type){
case MOTION_TYPE_NONE:
fTarget_Speed = 0;
fTarget_Turn = 0;
break;
case MOTION_TYPE_FORWARDS:
fTarget_Speed = -150;
fTarget_Turn = 0;
break;
case MOTION_TYPE_BACKWARDS:
fTarget_Speed = 150; //150
fTarget_Turn = 0;
break;
case MOTION_TYPE_TURN_LEFT:
fTarget_Speed = 0;
fTarget_Turn = -210;
break;
case MOTION_TYPE_TURN_RIGHT:
fTarget_Speed = 0;
fTarget_Turn = 210;
break;
case MOTION_TYPE_FORWARDS_TURN_LEFT:
fTarget_Speed = -150;
fTarget_Turn = -210;
break;
case MOTION_TYPE_FORWARDS_TURN_RIGHT:
fTarget_Speed = -150;
fTarget_Turn = 210;
break;
case MOTION_TYPE_BACKWARDS_TURN_LEFT:
fTarget_Speed = 150;
fTarget_Turn = -210;
break;
case MOTION_TYPE_BACKWARDS_TURN_RIGHT:
fTarget_Speed = 150;
fTarget_Turn = 210;
break;
}
/*動作改變時增加中間的過度過程,過度過程為50ms*/
if((iMotion_Type != iMotion_Type_Last) && (iCount_Motion_Type_Change == 0)){
iCount_Motion_Type_Change = 10;
iMotion_Type_Last = iMotion_Type;
}
if(iCount_Motion_Type_Change != 0){
iCount_Motion_Type_Change --;
fTarget_Speed = 0;
fTarget_Turn = 0;
}
/*速度和位移一階低通濾波*/
fSpeed_Vechile = (iCount_L + iCount_R) * 0.5;
fSpeed_Vechile_F = fSpeed_Vechile_F * 0.7 + fSpeed_Vechile * 0.3;
/*累加求位移*/
fPosition += fSpeed_Vechile_F;
/*位移調節量*/
fPosition += fTarget_Speed;
/*位移限制,上下限待調節*/
if(fPosition > 5000){ //5000
fPosition = 5000;
}
else if(fPosition < -5000){
fPosition = -5000;
}
/*左輪和右輪光柵脈沖計數器清零*/
iCount_L = 0;
iCount_R = 0;
/*角度限制*/
if( (fPitchAngle < 40.0) && (fPitchAngle > -40.0) ){
/*俯仰角速度,單位deg/s,大于0則角速度方向向前,小于0則角速度方向向后*/
fAngle_Vel = -(fPitchAngle - fPitchAngle_Last);
/*保存此次俯仰角度*/
fPitchAngle_Last = fPitchAngle;
/*計算PWM輸出控制量*/
fPWM = (-fpid_angle) * fPitchAngle +
fpid_angle_vel * fAngle_Vel +
fpid_speed * fSpeed_Vechile_F +
fPosition * fpid_position;
/*旋轉調節量*/
fPWM_L = fPWM + fTarget_Turn;
fPWM_R = fPWM - fTarget_Turn;
/*左輪控制*/
if(fPWM_L > 0){
MotorDriver_L_Turn_Forward();
}
else{
MotorDriver_L_Turn_Reverse();
fPWM_L = -fPWM_L;
}
/*右輪控制*/
if(fPWM_R > 0){
MotorDriver_R_Turn_Forward();
}
else{
MotorDriver_R_Turn_Reverse();
fPWM_R = -fPWM_R;
}
/*加上死區電壓*/
fPWM_L += 40;
fPWM_R += 10;
/*限制PWM*/
if(fPWM_L > 1000){
fPWM_L = 1000;
}
if(fPWM_R > 1000){
fPWM_R = 1000;
}
/*輸出PWM控制量*/
TIM_SetCompare2(TIM8, (uint16_t)fPWM_L); //左輪
TIM_SetCompare2(TIM1, (uint16_t)fPWM_R); //右輪
}
/*傾角過大時的姿態數據不參與計算*/
else{
TIM_SetCompare2(TIM8, 0); //左輪
TIM_SetCompare2(TIM1, 0); //右輪
}
}

復制代碼

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费