久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费
標題:
KEA128電磁直立例程
[打印本頁]
作者:
captains
時間:
2018-5-17 16:27
標題:
KEA128電磁直立例程
這是我上周做的智能車電磁直立例程
單片機源程序如下:
/*!
*
* @file main.c
* @brief 山外KEA 平臺主程序
* @author 山外科技
* @version v6.0
* @date 2017-12-10
*/
#include "common.h"
#include "include.h"
/*!
* @brief main函數
* @since v6.0
* @note **********直立3隊***********************
**********2018年3月4日***********************
**********電磁直立程序************************
*/
void main(void)
{
summary_init();
while(1)
{
// electro_data_handle();
key_scan();
menu_show();
}
}
復制代碼
#include "electro.h"
/******************編程思想*****************************************************
*******************起跑前尋找最大值和最小值**************************************
*******************電感采集(存入Collect_ADi[9])*********************************
*******************中值濾波(存入AD_filter[2])***********************************
*******************歸一化值(存入ADf_returnone[2]和AD_returnone[2])**************
*******************差除和計算***************************************************
*******************各電感配合使用,求偏差,轉向PWM*******************************
*******************丟線處理,彎道重疊處理(?????),圓環處理******************************/
uint16 Collect_AD1[5],Collect_AD2[5];//首次采集存入數組
//uint16 Collect_AD[2];
uint16 left_electro,right_electro;
uint16 left_one,right_one;
//歸一化參數
uint16 AD_filter[2]={0,0};//電感濾波值
//uint8 AD_returnone[2]={0,0};//歸一化值
//float ADf_returnone[2]={0};//歸一化值
//uint16 min_v[2]={3000,3000};//起跑前2電感采集最小值
//uint16 max_v[2]={726,726};//起跑前2電感采集最大值
// uint16 elcmin,elcmax;
//uint16 frist_Collect_AD_min[2]={0};
// uint16 frist_Collect_AD_max[2]={0};
uint8 left_flag,right_flag;
float difference_division_sum_data=0;
float last_difference_division_sum_data=0;
void Collect_electro_filter()//電感采集 中值濾波
{
uint8 i,filter_i,filter_j;
uint16 temp;
for(i=0;i<5;i++) //L1
{
Collect_AD1[i] =adc_once(ADC0_SE0,ADC_12bit);//A0;
}
for(i=0;i<5;i++) //L2
{
Collect_AD2[i] =adc_once(ADC0_SE1,ADC_12bit);//A1;
}
for(filter_i=0;filter_i<5;filter_i++) //中值為AD[3]
{
for(filter_j=0;filter_j<5-filter_i;filter_j++)
{
if(Collect_AD1[filter_j]>Collect_AD1[filter_j+1])
{
temp=Collect_AD1[filter_j];
Collect_AD1[filter_j]=Collect_AD1[filter_j+1];
Collect_AD1[filter_j+1]=temp;
}
}
for(filter_j=0;filter_j<5-filter_i;filter_j++)
{
if(Collect_AD2[filter_j]>Collect_AD2[filter_j+1])
{
temp=Collect_AD2[filter_j];
Collect_AD2[filter_j]=Collect_AD2[filter_j+1];
Collect_AD2[filter_j+1]=temp;
}
}
}
AD_filter[0]=Collect_AD1[2];
AD_filter[1]=Collect_AD2[2];
left_electro=AD_filter[0];
right_electro=AD_filter[1];
}
/*
void find_L_min()//起跑前找到2電感最小值
{
uint8 i;
for(i=0;i<10;i++)
{
frist_Collect_AD_min[0]=adc_once(ADC0_SE0, ADC_12bit);
if(frist_Collect_AD_min[0]<min_v[0]) min_v[0]=frist_Collect_AD_min[0];
}
for(i=0;i<10;i++)
{
frist_Collect_AD_min[1]=adc_once(ADC0_SE1, ADC_12bit);
if(frist_Collect_AD_min[1]<min_v[1]) min_v[1]=frist_Collect_AD_min[1];
}
}
void find_L_max()//起跑前找到2電感最大值
{
uint32 i;
DELAY_MS(25);
for(i=0;i<200000;i++)
{
frist_Collect_AD_max[0]=adc_once(ADC0_SE0, ADC_12bit);
if(frist_Collect_AD_max[0]>max_v[0])max_v[0]=frist_Collect_AD_max[0];
frist_Collect_AD_max[1]=adc_once(ADC0_SE1, ADC_12bit);
if(frist_Collect_AD_max[1]>max_v[1])max_v[1]=frist_Collect_AD_max[1];
//DELAY_MS(3);
}
// for(i=0;i<600;i++)
// {
// frist_Collect_AD_max[1]=adc_once(ADC0_SE1, ADC_12bit);
// if(frist_Collect_AD_max[1]>max_v[1])max_v[1]=frist_Collect_AD_max[1];
// DELAY_MS(3);
// }
elcmin=max_v[0];
elcmax=max_v[1];
}
void sensor_to_one()//電感濾波值歸一化處理
{
uint8 i;
float sensor_to_one[2];
for(i=0;i<2;i++)
{
sensor_to_one[i]=(float)(AD_filter[i]-min_v[i])/(float)(max_v[i]-min_v[i]);
if(sensor_to_one[i]<=0.0)
sensor_to_one[i]=0.001;
if(sensor_to_one[i]>1.0)
sensor_to_one[i]=1.0;
ADf_returnone[i]=(100*sensor_to_one[i]);// AD[i]歸一后的值。0~100之間
AD_returnone[i]=(uint8)ADf_returnone[i];
left_flag=AD_returnone[0];
right_flag=AD_returnone[1];
left_one=AD_returnone[0];
right_one=AD_returnone[1];
}
}
*/
void difference_division_sum()//差除和計算
{
difference_division_sum_data=(float)((AD_filter[0]-AD_filter[1])*(AD_filter[0]-AD_filter[1]))/(float)((AD_filter[0]+AD_filter[1])*(AD_filter[0]+AD_filter[1]));
if(AD_filter[0]>AD_filter[1])
{
difference_division_sum_data=-difference_division_sum_data;
}
if(AD_filter[0]<AD_filter[1])
{
difference_division_sum_data=difference_division_sum_data;
}
if(difference_division_sum_data>1) difference_division_sum_data=1;
if(difference_division_sum_data<-1) difference_division_sum_data=-1;
if(AD_filter[0]<260)
difference_division_sum_data=0.34;
if(AD_filter[1]<260)
difference_division_sum_data=-0.34;
// last_difference_division_sum_data=difference_division_sum_data;
}
void electro_data_handle()
{
Collect_electro_filter();
// sensor_to_one();
difference_division_sum();
}
復制代碼
#include "control.h"
/*************************************************
****************參數定義*****************
***********************************************/
float Angle_Middle=9.6;
//float Angle_Middle=10;
float b_kp=1500,b_kd=2.125;
float v_kp=0.006,v_ki=0.000025;
//float v_kp=0,v_ki=0;
//float t_kp=600.0,t_kd=1.0,t_kg=0.06;
float t_kp=1150,t_kd=7,t_kg=0;
float angle_deviation=0;
int16 Set_velocity=-650;//設定速度必須為負值
int16 Balance_PWM=0,Velocity_PWM=0,Turn_PWM=0;
int16 encoder_right=0,encoder_left=0;
int16 encoder_sum=0;
float velocity_deviation,velocity_proportion,velocity_integral;
/*****************PWM輸出********************/
void PWM_out(int16 x11,int16 x12,int16 x21,int16 x22)
{
if(x11<1)x11=0;if(x11>9999)x11=9999;
if(x12<1)x12=0;if(x12>9999)x12=9999;
if(x21<1)x21=0;if(x21>9999)x21=9999;
if(x22<1)x22=0;if(x22>9999)x22=9999;
if(x11==0||x12==0)
{
if(x21==0||x22==0)
{
ftm_pwm_init(FTM2, FTM_CH0,10000,x11);
ftm_pwm_init(FTM2, FTM_CH1,10000,x12);
ftm_pwm_init(FTM2, FTM_CH2,10000,x22);
ftm_pwm_init(FTM2, FTM_CH3,10000,x21);
}
}
}
/****************編碼器采集*******************/
void encoder_collect()
{
encoder_left= ftm_pulse_get(FTM0);
if(PTB2_IN == 1)
{
encoder_left=-encoder_left;
}
else
{
encoder_left=encoder_left;
}
ftm_pulse_clean(FTM0); //清空計數。
encoder_right= ftm_pulse_get(FTM1);
if(PTB3_IN == 1)
{
encoder_right=encoder_right;
}
else
{
encoder_right=-encoder_right;
}
ftm_pulse_clean(FTM1); //清空計數。
if(encoder_left>9999) encoder_left=9999;
if(encoder_left<-9999) encoder_left=-9999;
if(encoder_right>9999) encoder_right=9999;
if(encoder_right<-9999) encoder_right=-9999;
if(encoder_left>5000&&encoder_right<5000)encoder_left =encoder_right;
if(encoder_left<5000&&encoder_right>5000)encoder_right=encoder_left;
encoder_sum=encoder_left+encoder_right;
}
/******************速度控制*************************/
float velocity_control(int16 encoder_sum1)//速度閉環控制
{
int16 encoder_deviation;
static float last_integral=0;
encoder_deviation=Set_velocity+encoder_sum1;
velocity_proportion=encoder_deviation*v_kp;//比例
velocity_integral=encoder_deviation*v_ki;//積分
velocity_integral+=last_integral;
if(velocity_integral<-1.0)velocity_integral=-1.0;//積分限幅
if(velocity_integral> 1.0)velocity_integral= 1.0;//積分限幅
last_integral=velocity_integral;
velocity_deviation=velocity_proportion+velocity_integral;
// velocity_deviation=velocity_proportion;
if(velocity_deviation>10)velocity_deviation=10;
if(velocity_deviation<-10)velocity_deviation=-10;
return velocity_deviation;
}
/******************************************/
int16 velocity_out(uint8 count,float velocity,float last_velocity)
{
int16 velocity_pwm;
float velocity_data;
velocity_data=(velocity-last_velocity)/20.0*count+last_velocity;
velocity_pwm=(int16)(velocity_data);
return velocity_pwm;
}
/**********************************************/
int16 turn_out(uint8 count,float new_turn,float last_turn)
{
int16 turn_pwm;
float turn_data;
turn_data=(new_turn-last_turn)/20.0*count+last_turn;
turn_pwm=(int16)(turn_data);
return turn_pwm;
}
/***********平衡速度串級控制*******************/
int16 balance_control(float Angle,float Gyro_Y)
{
int16 balance;
static uint8 v_count=1;
static float last_velocity=0.0;
static float this_velocity=0.0;
angle_deviation=Angle-Angle_Middle;
v_count++;
if(v_count==21)
{
v_count=1;
encoder_collect();
last_velocity=this_velocity;
this_velocity=velocity_control(encoder_sum);
}
Velocity_PWM=velocity_out(v_count,this_velocity,last_velocity);
balance=(int16)(b_kp*(angle_deviation+Velocity_PWM)+b_kd*Gyro_Y);
//balance=(int16)(b_kp*angle_deviation+b_kd*Gyro_Y);
if(balance> 9999)balance= 9999;
if(balance<-9999)balance=-9999;
return balance;
}
/**************轉向控制*********************/
int16 turn_control(float turn_deviation,float gyro_turn)
{
int16 turn;
float turn_proportion,turn_differential;
static float last_deviation;
turn_proportion=t_kp*turn_deviation;
turn_differential=t_kd*(turn_deviation-last_deviation);
last_deviation=turn_deviation;
turn=(int16)(turn_proportion+turn_differential+t_kg*gyro_turn);
return turn;
}
/************輸出*****************/
void balance_out(int16 balance_pwm,int16 turn_pwm) //合成
{
if(balance_pwm>=0)
{
PWM_out((int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm-turn_pwm),0);
}
else if(balance_pwm<0)
{
balance_pwm=-balance_pwm;
PWM_out(0,(int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm-turn_pwm));
}
}
/*
void balance_out(int16 balance_pwm,int16 turn_pwm) //合成
{
if(balance_pwm>=0)
{
if(turn_pwm>=0)
PWM_out((int16)(balance_pwm-turn_pwm/2),0,(int16)(balance_pwm-turn_pwm),0);
if(turn_pwm<0)
PWM_out((int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm+turn_pwm/2),0);
}
else if(balance_pwm<0)
{
balance_pwm=-balance_pwm;
if(turn_pwm>=0)
PWM_out(0,(int16)(balance_pwm-turn_pwm/2),0,(int16)(balance_pwm-turn_pwm));
if(turn_pwm<0)
PWM_out(0,(int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm+turn_pwm/2));
}
}
*/
void summary_control()
{
Balance_PWM=balance_control(Angle_Balance,Gyro_Balance);
Turn_PWM =turn_control(difference_division_sum_data,Gyro_Turn);
balance_out(Balance_PWM,Turn_PWM);
// led_turn(LED1); //LED1翻轉
}
復制代碼
所有資料51hei提供下載:
電磁直立(KEA).rar
(188.69 KB, 下載次數: 69)
2018-5-18 02:07 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
歡迎光臨 (http://m.zg4o1577.cn/bbs/)
Powered by Discuz! X3.1
主站蜘蛛池模板:
亚洲成人一区二区在线
|
欧美福利视频一区
|
日韩国产精品一区二区三区
|
www.av在线
|
亚洲视频在线免费观看
|
91在线精品一区二区
|
在线免费观看成人
|
在线一区二区三区
|
91精品国产综合久久久密闭
|
v片网站
|
www.操.com
|
欧美日韩中文字幕
|
天天干天天想
|
日韩在线精品强乱中文字幕
|
2018中文字幕第一页
|
国产91久久久久久
|
99精品一区二区三区
|
国产97视频在线观看
|
免费一级黄色电影
|
成人免费视频一区
|
婷婷丁香在线视频
|
久久久青草婷婷精品综合日韩
|
亚欧性视频
|
国产黄a一级
|
91精品国产一区二区三区
|
亚洲国产精品久久久久久
|
精品99久久久久久
|
国产精品成人一区二区三区
|
欧美一区二区三区四区视频
|
亚洲三区在线
|
视频一区二区三区在线观看
|
成人福利视频网站
|
久久亚洲欧美日韩精品专区
|
麻豆a级片
|
久草在线
|
涩涩视频网站在线观看
|
欧美亚洲一级
|
成人a视频片观看免费
|
成人午夜网站
|
欧美日韩1区2区
|
久热免费在线
|