日韩靠逼,一级少妇片,色5月婷婷丁香六月

#include "headfile.h"
#include "KEA128_uart.h"
#include "KEA128_qiu.h"
#include "filter.h"
#include "All_Init.h"
#define Speed_set 0
#define zhong 2250
struct PID_Typedef //PID結構體
{
float P;
float I;
float D;
int Error;
int PreError;
int PreDError;//
int d_error;
int dd_error;
int Output;
int target;
int measure;
}PID_Typedef;
float Kp,Kd,Ks;
extern uint16 val_get_L;//左側電感電壓值 PTA8
extern uint16 val_get_R;//右側電感電壓值 PTA7
extern uint16 aveL,aveR;
extern float val_dev_to_one,val_sum_to_one,aveL_to_one,aveR_to_one;
//extern uint16 val_get_M;
extern uint8 time_flag,dir_time;
extern uint8 read_bianmaqi;
uint8 * itoa(int32 num);
uint16 SpeedSumL,SpeedSumR;
uint16 SpeedL,SpeedR;
uint16 uart_cnt=0;
uint16 sum_cnt;
double Speed_L;
double Speed_R;
float ActualErr=0.0;
float Err_L;
float Err_N;
float Error=0.0;
void speedout();
//void differential();
void ReadBianMaQi();
void Ad_deal();
void MOTOR_PID(struct PID_Typedef *PID,int target,int measure);
void PID_init();//pid參數賦值
struct PID_Typedef Left;
struct PID_Typedef Right;
int main(void)
{
get_clk(); //獲取時鐘頻率必須執行
ALL_Init();
PID_init();
pit_init_ms(pit0,1); //初始化pit0 周期設置為10ms
set_irq_priority(PIT_CH0_IRQn,1); //設置pit0優先級
enable_irq(PIT_CH0_IRQn); //開啟pit0中斷
EnableInterrupts;
for(;;)
{
ReadBianMaQi();//讀取脈沖10讀取一次，
speedout(); //40ms進行一次控速
if(time_flag==50)
{
time_flag=0;
//uart_cnt++;
//if(uart_cnt>=10)
//{
//uart_cnt=0;
//}
}
}
}
void ReadBianMaQi()
{
if(read_bianmaqi>=10)
{//10ms讀取一次編碼器
read_bianmaqi=0;
adc_get();
filter1();
adc_to_one();
Ad_deal();
SpeedL = ftm_count_get(ftm1);//讀取脈沖
ftm_count_clean(ftm1);//清除寄存器
SpeedR = ftm_count_get(ftm0);//讀取脈沖1
ftm_count_clean(ftm0);//清楚寄存器
SpeedSumL +=SpeedL;
SpeedSumR +=SpeedR;
SpeedL=0;
SpeedR=0;
}
}
void speedout()
{
if(sum_cnt>=40)
{//40ms進行一次速度控制
sum_cnt=0;
Left.measure=SpeedSumL;
Right.measure=SpeedSumR;
SpeedSumL =0;
SpeedSumR =0;
// Left.target=200;
// Right.target=200;
if( ActualErr>0)//在一定的范圍內才加差速
{
Right.target=Speed_set-ActualErr*Ks;
Left.target=Speed_set+ActualErr;
}
else
{
Left.target=Speed_set+ActualErr*Ks;
Right.target=Speed_set-ActualErr;
}
if(Left.target > 700) //限幅防止輸出過大
Left.target= 700;
if(Left.target <-700) //限幅防止輸出過大
Left.target=-700;
if(Right.target > 700)
Right.target= 700;
if(Right.target <-700)
Right.target=-700;
// MOTOR_PID(&Left,Left.target,Left.measure);
// MOTOR_PID(&Right,Right.target,Right.measure);
// Left.target=200;
// Right.target=200;
Left.Output=Left.target;
Right.Output=Right.target;
if(Left.Output>=900)//限上值但是在這里不管下限在計算里面管
Left.Output = 900;
else if(Left.Output<=-900)
Left.Output =-900;
if(Right.Output>=900)//限上值但是在這里不管下限在計算里面管
Right.Output = 900;
else if(Right.Output<=-900)
Right.Output =-900;
if(Left.Output>=0)
{
Left.Output=(uint16)Left.Output+0;
ftm_pwm_duty(ftm2,ftm_ch2,0);
ftm_pwm_duty(ftm2,ftm_ch3,Left.Output);
}
else
{
Left.Output=(uint16)(-Left.Output)+0;
ftm_pwm_duty(ftm2,ftm_ch3,0);
ftm_pwm_duty(ftm2,ftm_ch2,Left.Output);
}
if(Right.Output>=0)
{
Right.Output=(uint16)Right.Output+0;
ftm_pwm_duty(ftm2,ftm_ch0,0);
ftm_pwm_duty(ftm2,ftm_ch1,Right.Output);
}
else
{
Right.Output=(uint16)(-Right.Output)+0;
ftm_pwm_duty(ftm2,ftm_ch1,0);
ftm_pwm_duty(ftm2,ftm_ch0,Right.Output);
}
/*ftm_pwm_duty(ftm2,ftm_ch0,Left.Output); //，設置占空比，，右電機正轉,,在系統配置中已經將PWMIO口改成C0C1C2C3
ftm_pwm_duty(ftm2,ftm_ch1,0);
ftm_pwm_duty(ftm2,ftm_ch2,0); //左電機正轉
ftm_pwm_duty(ftm2,ftm_ch3,Right.Output); */
uart_cnt++;
if(uart_cnt>=25)
{//4*25=1000 1秒輸出一次上位機，方便觀察
uart_cnt=0;
uart_putstr(uart2,"L編碼器目標");
uart_putstr(uart2,itoa(Left.target));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"R編碼器目標");
uart_putstr(uart2,itoa(Right.target));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"L編碼器速度");
uart_putstr(uart2,itoa(Left.measure));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"R編碼器速度");
uart_putstr(uart2,itoa(Right.measure));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"左邊感量");
uart_putstr(uart2,itoa(aveL));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"右邊感量");
uart_putstr(uart2,itoa(aveR));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"LDuty");
uart_putstr(uart2,itoa(Left.Output));
uart_putstr(uart2,"\t\t");
uart_putstr(uart2,"RDuty");
uart_putstr(uart2,itoa(Right.Output));
uart_putstr(uart2,"\n");
}
}
}
//整數轉換為字符串（輸入整數，返回字符串地址）
uint8 * itoa(int32 num)
{
int32 i=0,isNegative=0;
static uint8 s[15]; //必須是全局變量或者是靜態變量
//如果是負數，先轉為正數
if((isNegative=num) < 0) num = -num;
//從個位開始變為字符，直到最高位，最后應該反轉
do
{
s[i++] = num%10 + '0';
num /= 10;
}while(num > 0);
//如果是負數，補上負號
if(isNegative < 0) s[i++] = '-';
//最后加上字符串結束符
s[i] = '\0';
/******翻轉字符串******/
//p指向s的頭部
uint8 *p = s;
//q指向s的尾部
uint8 *q = s;
while(*q) ++q;
q--;
//交換移動指針，直到p和q交叉
uint8 temp;
while(q > p)
{
temp = *p;
*p++ = *q;
*q-- = temp;
}
return s;
}
/*pid 調電機*/
/*****差速函數******/
/*void differential()
{
int chasu=0;
errorL=aveL-zhong;
errorR=aveR-zhong;
tempL=errorL*100;
tempL=tempL/zhong;
tempR=errorR*100;
tempR=tempR/zhong;
Right.target=(int)(speed_set-error*speed_set/100);
Left.target=(int)(speed_set-error*speed_set/100);
*/
/* if( ActualErr>0)//在一定的范圍內才加差速
{
Right.target=(int)speed_set+ ActualErr;
Left.target=(int)speed_set-ActualErr*Ks;
}
else
{
Right.target=(int)speed_set-ActualErr;
Left.target=(int)speed_set+ ActualErr*Ks;
}
if( Left.target> 700) //限幅防止輸出過大
Left.target= 700;
if( Left.target <-700) //限幅防止輸出過大
Left.target=-700;
if( Right.target > 700)
Right.target= 700;
if( Right.target <-700)
Right.target=-700;
//給各自的target賦值
}*/
/******電機PID部分程序**********/
/*增量式 pid ，慢慢調 */
void MOTOR_PID(struct PID_Typedef *PID,int target,int measure) //target目標期望值，measure目標測量值，輸出占空比改變量
{
PID->Error=target - measure; //計算偏差。
PID->d_error = PID->Error - PID->PreError; //偏差計算(比例)
PID->dd_error = PID->d_error - PID-> PreDError; //偏差計算(微分)
PID->PreError = PID->Error; //存儲當前偏差
PID->PreDError = PID->d_error;
if ((PID->Error > -5) && (PID->Error <5)) // 設置調節死區編碼器返回的值跟目標值在小誤差范圍內不調節防止超調過度起反作用
{
; // 不執行PID調節
}
else
{
PID->Output+= ((PID->P* PID->d_error) + (PID->I * PID->Error) + (PID->D * PID->dd_error));
}
if(PID->Output>=900)//限上值但是在這里不管下限在計算里面管
PID->Output = 900;
else if(PID->Output<=0)
PID->Output =0;
}
void Ad_deal()
{
float IncrementErr;
Error=val_dev_to_one*100/pow(val_sum_to_one,1.5); //水平電感偏差
if(Error>=100)
Error=100;
if(Error<=-100)
Error=-100;
// Error=(sqrt(AD3)-sqrt(AD1))/(AD1+AD3)*3800;
/****************增量式PD****************/
IncrementErr=Kp*(Error-Err_N)+Kd*(Error-2*Err_N+Err_L); //增量式PD
ActualErr+=IncrementErr; //
Err_L=Err_N; //
Err_N=Error;
// Lost_Err(); //防丟線程序
/*****************兩輪差速效果*****************/
if(Error<=0) //計算兩輪差速效果
Error=-Error; //
Ks=Error/10; //
if(Ks>1.3) //
Ks=1.3; //0.78 //增大后內輪減速明顯
}
void PID_init()
{
Left.P = 0.4; //左輪P
Left.I = 0.28; //左輪I
Left.D = 0; //
Right.P = 0.4; //右輪P
Right.I = 0.28; //右輪I
Right.D = 0; //右輪D
/* 電感差速 */
Error=0.0;
Err_N=0.0;
Err_L=0.0;
Kp=5.0;
Kd=20.0;
/*左右輪差速效果*/
Ks=0.0; //在arithmetic.c中賦值Error/10
}

復制代碼

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