K60_CCD直立小車源碼(二等獎程序)

ID:327139 · 發表于 2018-5-10 21:07

k60 ccd二等獎程序

本程序配套4按鍵直立一體板

按鍵功能：
E0：參數加
E1：參數減
E2：切換顯示及功能
E3：回到最初顯示及功能

調試直立

A、修改直立靜態偏移量： calcultion.c s32 GRAVITY_OFFSET=485;//230  MPU6550為平衡時Z軸角度，放大10倍
B、main.c里 run()函數中：  修改  PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//電機輸出
                        為： PWMout=MotorSpeedOut(AAngPWM,0,0);//關閉速度環和轉向環分量，直立后再加上兩個分量

系統連線說明：

單片機源程序如下:

#include "include.h"
#include "calculation.h"
#include"VisualScope.h"
#include "CCD.h"
#include "LQ12864.h"
#include "MPU6050.h"
int16 angletmp,angle_g,angle_w; //16位
int16 angle6550; //MPU6550計算
extern byte F6x8[][6];
extern s16 g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma;
extern float GYROSCOPE_ANGLE_RATIO;
s32 TurnPosition;//實際位置
s32 TurnPositionOld; //上一次位置
int dline; //左右邊界差
float ANGLE_CONTROL_P=240;//* 260 //直立P 260
float ANGLE_CONTROL_D=30;//1.5;//* 4.8 //直立D 5.8
int16 xgout=1000; //MPU6050六軸參數 x軸角速度
int16 ygout=1000;
int16 zgout=1000;
int16 xaout=-1000; //x軸加速度
int16 yaout=-1000;
int16 zaout=-1000;
uint8 PWMDEADL = 0; //死區電壓左路電機
uint8 PWMDEADR = 0; //死區電壓右路電機
uint16 MAXTURNPWM= 200; //電機轉向最大PWM值
#define SAVEADRESS (0X1C)
extern s32 GRAVITY_OFFSET;
uint8 disen; //允許上傳CCD圖像
uint16 A_P,A_D,V_P,V_I,T_P,T_D; //從FLASH中讀出的參數
uint32 G_RATIO;
uint8 flashreadbuf[6];
u16 SpeedKP = 24;//4.4;//* //速度P 放大10倍
u16 SpeedKI = 6;//* //速度D 放大10倍
u16 TurnP1 = 80; //直道前進時舵機PD值，放大10倍
u16 TurnD1 = 2;
//u16 TurnP2 = 34;//70; //彎道時舵機轉角比例系數放大10倍
//u16 TurnD2 = 140;//30; //彎道時舵機轉角積分系數放大100倍
//
//u16 TurnP3 = 40; //大彎時轉角比例
//u16 TurnD3 = 110; //大彎時轉角積分
//
//u16 TurnP4 = 48; //大彎時轉角比例
//u16 TurnD4 = 90; //大彎時轉角積分
u16 TurnP; //* 94 //方向P
u16 TurnD; //* 8.4 //方向D
u16 turnsetmax; //轉向最大PWM值
u16 turnpwmmax;
u8 disccd[128][4];
u8 paraadjflag; //參數修改組切換
extern int leftline, rightline; //左右邊界值
extern int leftlineold,rightlineold; //左右邊界上一次值
extern int yuzhizhi;//動態閾值
extern signed int sumlspeed,sumrspeed;
s16 SetSpeed= -250;
s16 AmSpeed;//* //目標速度用于外部設置速度時在這里設置
s16 g_Speedgoal=0;//用于內部自加速使用
//時間標志位
extern u8 TIME0flag_5ms,TIME0flag_10ms,TIME0flag_20ms;
extern u8 TIME1flag_100ms,flag_1ms ;
extern u8 TIME1flag_1s ; //PT1口1s定時標志位
extern s16 whitelength;//91; //具體看CCD圖像，賽道寬度
s32 ATimeCount=700;//100ms進入標志由20個5ms構成，速度PID時使用
s32 TimeCount=0 ;//1ms中斷標志
//角度傳感器
s32 GYROSCOPE_OFFSET,GYROSCOPE_turn_OFFSET;//陀螺儀靜止時的零點
s32 AD_ACC_Z;//加速度計的Z軸
s32 AD_GYRO;//平衡陀螺儀
s32 AD_GYRO_turn;//轉向陀螺儀
s32 AAngPWM=0,LastAAngPWM=0,AAngPeriodCount=0,MotorAAngPWM=0 ;
//速度變量
float g_SpeedControlOutNew,g_SpeedControlOutOld ;
s16 SpeedPeriodCount=0;
s32 MotorSpeedPWM=0 ;
s32 PWMout ;
int start_flag=0,stop_jiasu=0;
//CCD變量
int ccd_count=0;
uint8_t Pixel[128];
int ccd_flag=0;
s16 TurnPeriodCount=0 ;
s32 MotorTurnPWM=0 ;
s16 TurnPWMOUT=0 ;
s16 LastTurnPWMOUT=0;
u16 discnt;
u8 workflag;
void run();//直立主函數
void qibu();//起步不能一下把速度加上去，速度要慢慢加
void GPIO_Init();
void dispage1();
void dispage2();
void dispage3();
void disdata1();
void disdata2();
void disdata3();
void disccdval();
extern void Pause(void);
extern void LCD_CLS(void);
void main()
{ int16 ii;
discnt=0;
paraadjflag=0;
workflag=0;
AmSpeed = SetSpeed;
turnsetmax = 650;
s32 xgtemp,ygtemp;
DisableInterrupts; //禁止總中斷
uart_init (UART0,115200);//初始化UART0，輸出腳PTA15，輸入腳PTA14
//AngleAcceleration_init() ;//AD初始化
FTM1_QUAD_Iint();//正交解碼測速 A相---PTA8 B相---PTA9
FTM2_QUAD_Iint();//正交解碼測速 A相---PTA10 B相---PTA11
oled_init();//oled初始化
CCD_init (); //CCD初始化
GPIO_Init(); //程序運行燈
pit_init_ms(PIT0, 1); //初始化PIT0，定時時間為： 1ms
pit_init_ms(PIT1, 100);//初始化PIT1，定時時間為： 100ms
FTM_init() ; //PWM初始化
delayms(100);
MPU6050_Init(); //MPU6050初始化 , PD8 , PD9
delayms(300);
xgtemp=0;
ygtemp=0;
TurnP=TurnP1;
TurnD=TurnD1;
for(ii=0;ii<100;ii++)
{
xgtemp += MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//讀X軸角速度,直立
Pause();
ygtemp += MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//讀Y軸角速度，轉向
Pause();
}
GYROSCOPE_OFFSET = xgtemp/100; //靜態X軸角速度值，直立，理論上為0，但實際可能不為0
GYROSCOPE_turn_OFFSET = ygtemp/100; //靜態Y軸角速度值，轉向，理論上為0，但實際可能不為0
EnableInterrupts;//開總中斷
uart_irq_EN(UART0);
uart_putchar(UART0,'O');
uart_putchar(UART0,'K');
A_P = ANGLE_CONTROL_P;
A_D = ANGLE_CONTROL_D*10;
V_P = SpeedKP;
V_I = SpeedKI;
T_P = TurnP;
T_D = TurnD;
G_RATIO = GYROSCOPE_ANGLE_RATIO*100;
LCD_CLS();
dispage1(); //正常顯示
while(1)
{
if(workflag==0) //正常狀態
{ if(gpio_get(PORTE,3)==0) //狀態切換
{ delayms(4);
if(gpio_get(PORTE,3)==0)
{
workflag=1;
LCD_CLS(); //清屏
dispage2();
while(gpio_get(PORTE,3)==0); //等待按鍵松開
}
}
discnt++;
if(discnt>=100)
{ discnt=0;
disdata1();
delayms(2);
}
delayms(1);
}
else if(workflag==1) //顯示和上傳CCD圖像
{ if(ccd_flag==1)//采完一副圖像
{
ccd_flag=0;
tuxiang(); //紅樹偉業上位機
discnt++;
if(discnt>=100)
{ discnt=0;
disdata2(); //顯示圖像
disccdval();
delayms(1);
}
}
if(gpio_get(PORTE,3)==0) //狀態切換
{ delayms(4);
if(gpio_get(PORTE,3)==0)
{
workflag=2;
LCD_CLS(); //清屏
dispage3();
while(gpio_get(PORTE,3)==0); //等待按鍵松開
}
}
if(gpio_get(PORTE,2)==0) //回正常狀態
{ delayms(4);
if(gpio_get(PORTE,2)==0)
{
workflag=0;
LCD_CLS(); //清屏
dispage1();
while(gpio_get(PORTE,5)==0); //等待按鍵松開
}
}
}
else if(workflag==2) //調節參數
{ if(gpio_get(PORTE,3)==0) //修改參數變換
{ delayms(4);
if(gpio_get(PORTE,3)==0)
{
paraadjflag++;
if(paraadjflag==5) paraadjflag=0; // 切換。如果有更多的參數組，自行修改
if(paraadjflag==0)
{ LCD_P6x8Str(0,1,"Speed Para: <--");
LCD_P6x8Str(0,3,"Turn Para: ");
LCD_P6x8Str(0,5,"Speed Set: ");
}
else if(paraadjflag==1)
{ LCD_P6x8Str(0,1,"Speed Para: -->");
LCD_P6x8Str(0,3,"Turn Para: ");
LCD_P6x8Str(0,5,"Speed Set: ");
}
else if(paraadjflag==2)
{ LCD_P6x8Str(0,1,"Speed Para: ");
LCD_P6x8Str(0,3,"Turn Para: <--");
LCD_P6x8Str(0,5,"Speed Set: ");
}
else if(paraadjflag==3)
{ LCD_P6x8Str(0,1,"Speed Para: ");
LCD_P6x8Str(0,3,"Turn Para: -->");
LCD_P6x8Str(0,5,"Speed Set: ");
}
else if(paraadjflag==4)
{ LCD_P6x8Str(0,1,"Speed Para: ");
LCD_P6x8Str(0,3,"Turn Para: ");
LCD_P6x8Str(0,5,"Speed Set: <--");
}
while(gpio_get(PORTE,3)==0); //等待按鍵松開
}
}
if(gpio_get(PORTE,2)==0) //回正常狀態
{ delayms(4);
if(gpio_get(PORTE,2)==0)
{
workflag=0;
LCD_CLS(); //清屏
dispage1();
while(gpio_get(PORTE,2)==0); //等待按鍵松開
}
}
if(gpio_get(PORTE,0)==0) //參數調節
{ delayms(4);
if(gpio_get(PORTE,0)==0)
{ if(paraadjflag==0) //速度KP 加減
{ SpeedKP++;
Dis_num(24,2,SpeedKP);
}
else if(paraadjflag==1)
{ SpeedKI++;
Dis_num(90,2,SpeedKI);
}
else if(paraadjflag==2)
{ turnsetmax=turnsetmax+5;
Dis_num(24,4,turnsetmax);
}
else if(paraadjflag==3)
{ TurnD++;
Dis_num(90,4,TurnD);
}
else if(paraadjflag==4) //速度
{ SetSpeed = SetSpeed - 5;
AmSpeed = SetSpeed;
Dis_num(24,6,SetSpeed);
}
while(gpio_get(PORTE,0)==0); //等待按鍵松開
}
}
if(gpio_get(PORTE,1)==0) //參數調節
{ delayms(4);
if(gpio_get(PORTE,1)==0)
{
if(paraadjflag==0) //速度KP 加減
{ SpeedKP--;
Dis_num(24,2,SpeedKP);
}
else if(paraadjflag==1)
{ SpeedKI--;
Dis_num(90,2,SpeedKI);
}
else if(paraadjflag==2)
{ turnsetmax=turnsetmax-5;
Dis_num(24,4,turnsetmax);
}
else if(paraadjflag==3)
{ TurnD--;
Dis_num(90,4,TurnD);
}
else if(paraadjflag==4) //速度
{ SetSpeed = SetSpeed + 5;
Dis_num(24,6,SetSpeed);
AmSpeed = SetSpeed;
}
while(gpio_get(PORTE,1)==0); //等待按鍵松開
}
}
delayms(5);
}
}
}
void GPIO_Init()
{
gpio_init (PORTA, 17, GPO, 1u); //初始化PTE0為高電平輸出---LED0
gpio_set (PORTA, 19, 0); //設置PTE0為高電平輸出，LED0滅
gpio_init (PORTE,0,GPI,0); //按鍵輸入
gpio_init (PORTE,1,GPI,0); //按鍵輸入
gpio_init (PORTE,2,GPI,0); //按鍵輸入
gpio_init (PORTE,3,GPI,0); //按鍵輸入
}
void run()//直立函數在isr.c中的1ms中斷中調用 PIT0_IRQHandler
{
TimeCount++ ;
SpeedPeriodCount++;
TurnPeriodCount ++ ;
AAngPeriodCount ++ ;
if(AAngPeriodCount>=4) AAngPeriodCount=4;
MotorTurnPWM = TurnPWMOut(TurnPWMOUT,LastTurnPWMOUT,TurnPeriodCount) ;
MotorSpeedPWM = SpeedPWMOut(g_SpeedControlOutNew ,g_SpeedControlOutOld,SpeedPeriodCount);
MotorAAngPWM = AAangPWMOut(AAngPWM ,LastAAngPWM ,AAngPeriodCount);
Checkcarstate();//開啟停止判斷
if(TimeCount>=5)//讀速度 5ms
{
TimeCount=0;
GetMotorPulse();//讀速度脈沖
}
else if(TimeCount == 1)//讀取MPU6050
{
xaout=MPU6050_GetDoubleData(MPU6050_ACCEL_XOUT);//讀X軸加速度
yaout=MPU6050_GetDoubleData(MPU6050_ACCEL_YOUT);//讀X軸加速度
zaout=MPU6050_GetDoubleData(MPU6050_ACCEL_ZOUT);//讀X軸加速度
xgout=MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//讀X軸角速度,直立角速度
ygout=MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//讀Y軸角速度，轉向角速度
angle6550=MPU6050_Get_Angle(xaout,yaout,zaout,0);
}
else if(TimeCount == 2)//5ms 直立濾波，控制
{
AAngPeriodCount = 0;
AngleCalculate();//計算加速值和角度值
LastAAngPWM = AAngPWM ;
AAngPWM = AngleControl() ; //計算平衡電機速度
PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//電機輸出
//PWMout=MotorSpeedOut(0,0,MotorTurnPWM);
//PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,0);//電機輸出剛開始用的時候可以把MotorSpeedPWM和MotorTurnPWM都設為0
//PWMout=MotorSpeedOut(AAngPWM,0,0);//電機輸出剛開始用的時候可以把MotorSpeedPWM和MotorTurnPWM都設為0
//先調直立，再加速度MotorSpeedPWM，最后加方向MotorTurnPWM
}
else if(TimeCount == 3)//5ms 速度PI調節
{
ATimeCount ++ ;
if(ATimeCount >= 20)//20*5=100ms進行一次速度PID調節
{
ATimeCount=0;
SpeedPID() ;
SpeedPeriodCount = 0 ;
qibu();//起步不可以一下就把速度加上，要緩慢加
}
}
else if(TimeCount == 4)// CCD圖像采集與處理
{
ccd_count++;
if(ccd_count>=4)//20ms進一次
{
ccd_count=0;
ccd_flag=1;//發送圖像到上位機標志
ImageCapture(Pixel) ;//將捕捉到的圖像放在Pixel數組中
//process();//圖像處理函數
FindLine();
dline = rightline - leftline;
if((dline>64)||(dline<12)) //十字路或橋上
{ //TurnP = TurnP1;
//TurnD = TurnD1;
turnpwmmax=turnsetmax;
}
else if((dline>(whitelength+16))||(dline<(whitelength-16))) //丟線
{ turnpwmmax=turnsetmax+50;
//TurnD = TurnD4;
}
else if((dline>(whitelength+9))||(dline<(whitelength-9))) //60度彎道
{ //TurnP = TurnP4;
//TurnD = TurnD4;
turnpwmmax=turnsetmax+30;
}
else if((dline>(whitelength+4))||(dline<(whitelength-4))) //50度彎道
{ //TurnP = TurnP3;
//TurnD = TurnD3;
turnpwmmax=turnsetmax+20;
}
else if((dline>(whitelength+2))||(dline<(whitelength-2))) //亞直道
{ //TurnP = TurnP2;
//TurnD = TurnD2;
turnpwmmax=turnsetmax;
}
else //直道
{ //TurnP = TurnP1;
//TurnD = TurnD1;
turnpwmmax=turnsetmax;
}
LastTurnPWMOUT = TurnPWMOUT ;
TurnPWM() ;
TurnPeriodCount = 0 ;
}
}
if(TIME1flag_1s == 1)//程序運行燈
{
TIME1flag_1s = 0 ;
PTA17_OUT = ~PTA17_OUT ;
}
}
void qibu()//這一部分認真看一下，也可以根據自己的想法改一下
{
if(AmSpeed!=0)//在目標速度不為0時才運行
{
start_flag++;
if(start_flag<30)//靜止2S才出發可根據規則修改
{ g_Speedgoal=0;
//SpeedKI=0;
}
else if(start_flag>=30)
{
if(g_Speedgoal>AmSpeed&&stop_jiasu==0)//然后逐漸加速
{
g_Speedgoal-=15; // 30 //單次加速值，值大加速能力強，比賽前可設兩組值，一組快速加速，用于應對起步時是長
//直道的情況，另一組為慢加速，用于應對起步時就是彎道的情況
}
else if(g_Speedgoal<=AmSpeed&&stop_jiasu==0) //內部速度高于設置速度，停止大幅度加速，進行小幅度調整加速
{
g_Speedgoal=AmSpeed;
stop_jiasu=1;
}
start_flag=3000; //終止靜止2s計數
//SpeedKI=0;
if(stop_jiasu==1) //車模之后的目標速度調整主要在此函數中
{
if(g_Speedgoal>AmSpeed)//然后逐漸加速
{
g_Speedgoal-=15;//
}
else if(g_Speedgoal<=AmSpeed)
{
g_Speedgoal=AmSpeed;
}
}
}
}
}
void dispage1()
{ Dis_num(0,0,GRAVITY_OFFSET); //設定值，垂直時加速度AD
Dis_num(42,0,GYROSCOPE_OFFSET);//陀螺儀零偏自檢平均值
LCD_P6x8Str(0,1,"Ap:");
Dis_num(20,1,A_P);//加計零偏定值
LCD_P6x8Str(60,1,"Ad:");
Dis_num(80,1,A_D);
LCD_P6x8Str(0,2,"Vp:");
Dis_num(20,2,V_P);
LCD_P6x8Str(60,2,"Vd:");
Dis_num(80,2,V_I);
LCD_P6x8Str(0,3,"Tp:");
Dis_num(20,3,T_P);
LCD_P6x8Str(60,3,"Td:");
Dis_num(80,3,T_D);
LCD_P6x8Str(0,4,"A S C");
LCD_P6x8Str(0,5,"Spd");
LCD_P6x8Str(1,7,"LMB:");
LCD_P6x8Str(64,7,"RMB:");
}
void disdata1()
{ Dis_num(6,4,angletmp);
Dis_num(48,4,-AmSpeed);
Dis_num(90,4,TurnPosition);
A_P = ANGLE_CONTROL_P;
A_D = ANGLE_CONTROL_D*10;
V_P = SpeedKP;
V_I = SpeedKI;
T_P = TurnP;
T_D = TurnD;
Dis_num(20,1,A_P);//加計零偏定值
//Dis_num(80,1,A_D);
Dis_num(80,1,TurnPWMOUT);
Dis_num(20,2,V_P);
Dis_num(80,2,V_I);
Dis_num(20,3,T_P);
Dis_num(80,3,T_D);
delayms(50);
Dis_num(10,6,leftlineold); //賽道左邊界
Dis_num(94,6,rightlineold); //賽道右邊界
Dis_num(52,6,dline); //賽道右-左
Dis_num(28,7,sumlspeed);
Dis_num(96,7,sumrspeed);
Dis_num(20,5,AmSpeed);
Dis_num(80,5,turnsetmax);
}
void dispage2()
{ LCD_P6x8Str(0,0,"CCD DATA UP&DIS");
LCD_P6x8Str(0,1,"L: R:");
Dis_num(20,1,leftlineold); //賽道左邊界
Dis_num(94,1,rightlineold); //賽道右邊界
}
void disdata2()
{ Dis_num(20,1,leftlineold); //賽道左邊界
Dis_num(94,1,rightlineold); //賽道右邊界
}
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

所有資料51hei提供下載:

K60_CCD直立.rar (13.35 MB, 下載次數: 55)

ID:1 · 發表于 2018-5-12 06:08

好資料，51黑有你更精彩!!!

帳號		自動登錄	找回密碼
密碼			立即注冊

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

K60_CCD直立小車源碼(二等獎程序)