#include "derivative.h" /* derivative-specific definitions */
#include <mc9s12xs128.h>
#include "KEY.H"
#include "PLL.H"
#include "PWM.H"
#include "ATD.H"
#include "LCD5110.H"
#include "PIT.H"
#include "MATH.H"
#include "ECT.H"
#define AD_NUM 5
/***************************************************************/
signed int ad_value[AD_NUM]={0,0,0,0,0}
/**********************卡尔曼滤波*****************************************/
float gyro=0,acceler=0,Vref=2.048,jiaodu_jifen=0,jiaodu_error=0//2.048
float angle=0, angle_dot=0
long Tg=3
unsigned int fangdabeishu =9//15
float C_0 = 1
float Q_angle=0.001
float Q_gyro=0.003
float R_angle=0.5
float dt=0.006
float P[2][2] = {{ 1, 0 },
{ 0, 1 }
}
float Pdot[4] ={0,0,0,0}
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1
/*********************中断变量**************************************/
unsigned char
EventCount=0,SpeedControlPeriod=0,DirectionControlPeriod=0,InputVoltageCount=0,SpeedCont
rolCount=0,DirectionControlCount=0,BMQ_count=0
long MotorOut=0
long LeftMotorPulseSigma=0,RightMotorPulseSigma=0,InputVoltageSigma[5]=0
/************************直立PID************************************/
unsigned int JIAODU_CENTER=820,JIAO_SPEED_CENTER=580
unsigned int ZHILI_PID_P=460,ZHILI_PID_D=9,ZHILI_PID_I=25
全国大学生智能汽车邀请赛技术报告
38
float ZHILI_PD=0,ZHILI_I=0,ZHILI=0,PIANJIAO=0
/*************************速度PID*************************************/
unsigned int Sudu_PID_P=0,Sudu_PID_I=0,Sudu_PID_D=0
signed int
SpeedLeft_now=0,SpeedRight_now=0,Speed_want=0,speed[6]={140,20,140,20,140,18}
int Speed_Break_Flag1=0,Speed_Break_Flag2=0,Speed_Break_Count=0
long Speed_P=0,Speed_I=0,Speed_D=0,Speed_error=0,Speed_error_old=0,Speed_error_error=0
float Speed=0,SpeedControlOutOld=0,fValue=0,CarSpeed=0,CarSpeed1=0,CarSpeed2=0
float SpeedControlOut=0
/*************************方向PID******************************/
signed int Turn_PID_P=0,PP[3]={15,30,0},Turn_PID_D=50
signed int LeftVoltageSigma=0,RightVoltageSigma =0
long DirectionControlOutOld=0,DirectionControlOutNew=0
long DirectionControlOut=0,DirectionControlOut1=0
long LeftMotorOut=0,RightMotorOut=0
float fLeftRightSub_old=0, fLeftRightSub=0
int nLeft=0, nRight=0
float DValue=0
/*************************信号PID********************************/
#include "LCDKEY.H" //液晶控制程序
/***********************读AD******************************************/
void Read_Atd_Several()
{
ATD0CTL5=(atd0_scan<<5) + (atd0_mult<<4) + atd0_cd_cc_cb_ca
while(!ATD0STAT0_SCF)
ad_value[0]=ATD0DR0
ad_value[1]=ATD0DR1
ad_value[2]=ATD0DR2
ad_value[3]=ATD0DR3
ad_value[4]=ATD0DR4
}
/************************* 卡尔曼滤波
附录 A
39
*********************************************/
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt//先验估计
angle_err = angle_m - angle//zk-先验估计
Pdot[0]=Q_angle - P[0][1] - P[1][0]// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1]
Pdot[2]=- P[1][1]
Pdot[3]=Q_gyro
P[0][0] += Pdot[0] * dt// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += Pdot[1] * dt
P[1][0] += Pdot[2] * dt
P[1][1] += Pdot[3] * dt
PCt_0 = C_0 * P[0][0]
PCt_1 = C_0 * P[1][0]
E = R_angle + C_0 * PCt_0
K_0 = PCt_0 / E
K_1 = PCt_1 / E
t_0 = PCt_0
t_1 = C_0 * P[0][1]
P[0][0] -= K_0 * t_0//后验估计误差协方差
P[0][1] -= K_0 * t_1
P[1][0] -= K_1 * t_0
P[1][1] -= K_1 * t_1
angle += K_0 * angle_err//后验估计
q_bias += K_1 * angle_err//后验估计
angle_dot = gyro_m-q_bias
}
/****************************************************************************/
void ZHILI_lvbo(void)
{
angle+=(gyro+ jiaodu_error)/200
全国大学生智能汽车邀请赛技术报告
40
jiaodu_error= (acceler-angle)/Tg
if(angle>5) angle=5
else if(angle<-5) angle=-5
}
/***************************AD计算********************************************/
void AD_calculate(void)
{
acceler=ad_value[0]
gyro=ad_value[1]
gyro=(JIAO_SPEED_CENTER-gyro)*Vref/(1024*0.00067*fangdabeishu)
acceler=(acceler-JIAODU_CENTER )*Vref/(1024*0.8)*60
Kalman_Filter(acceler,gyro)//卡尔曼滤波
//angle_dot=gyro
//ZHILI_lvbo()
}
/************************* 直立PID 控制
*********************************************/
void zhili_kongzhi(void)
{
ZHILI_PD=(0-angle-(PIANJIAO/100))*ZHILI_PID_P+(0-angle_dot)*ZHILI_PID_D
ZHILI_I+=(0-angle-(PIANJIAO/100))*ZHILI_PID_I
if(ZHILI_I>1000) ZHILI_I=1000
if(ZHILI_I<-1000) ZHILI_I=-1000
ZHILI=ZHILI_PD/10+ZHILI_I/10
}
/***************************** 速度PID 控制
*****************************************************/
void SampleInputVoltage(void)
{
Read_Atd_Several()
InputVoltageSigma[0] += ad_value[0]
InputVoltageSigma[1] += ad_value[1]
InputVoltageSigma[2] += ad_value[2]
附录 A
41
InputVoltageSigma[3] += ad_value[3]
InputVoltageSigma[4] += ad_value[4]
InputVoltageCount ++
}
void GetInputVoltageAverage(void)
{
int i
if(InputVoltageCount == 0)
{
for(i = 0i <5i++)
InputVoltageSigma[i] = 0
return
}
// InputVoltageCount <<= 4
for(i = 0i <5i ++)
{
ad_value[i] = (int)(InputVoltageSigma[i] / InputVoltageCount)
InputVoltageSigma[i] = 0
}
InputVoltageCount = 0
}
void DirectionVoltageSigma(void)
{
LeftVoltageSigma += ad_value[2]
RightVoltageSigma += ad_value[3]
}
/*************************方向PID****************/
void DirectionControl(void)
{
nLeft = (int)(LeftVoltageSigma /= 2)
nRight = (int)(RightVoltageSigma /= 2)
LeftVoltageSigma = 0
RightVoltageSigma = 0
全国大学生智能汽车邀请赛技术报告
42
fLeftRightSub_old=fLeftRightSub
fLeftRightSub = nLeft - nRight
DirectionControlOutOld = DirectionControlOutNew
Turn_PID_P=PP[0]
if(ad_value[2]+ad_value[3]<300) Turn_PID_P=PP[1]
DValue = (fLeftRightSub *
Turn_PID_P/100)+((fLeftRightSub-fLeftRightSub_old)*Turn_PID_D/100)
DirectionControlOutNew = DValue
}
void DirectionControlOutput(void)
{
float Value
Value = DirectionControlOutNew - DirectionControlOutOld
DirectionControlOut = Value * (DirectionControlPeriod + 1) / 10 + DirectionControlOutOld
}
/*void DirectionControlOutput(void)
{
DirectionControlOutOld = DirectionControlOut
DirectionControlOut = (float)((ad_value[2] - ad_value[3]) * Turn_PID_P/100)
} */
void GetLeftMotorPulse(void)
{
SpeedLeft_now=PACNT
PACNT=0
附录 A
43
if(LeftMotorOut<0) SpeedLeft_now=-SpeedLeft_now
LeftMotorPulseSigma+=SpeedLeft_now
}
void GetRightMotorPulse(void)
{
SpeedRight_now=PACNT
PACNT=0
if(RightMotorOut<0) SpeedRight_now=-SpeedRight_now
RightMotorPulseSigma+=SpeedRight_now
}
void SpeedControl(void)
{
CarSpeed=(LeftMotorPulseSigma+RightMotorPulseSigma)/10
LeftMotorPulseSigma=0
RightMotorPulseSigma=0
Speed_error_old=Speed_error
Speed_want=speed[0]
Speed_error=Speed_want-CarSpeed
Speed_error_error=Speed_error-Speed_error_old
Speed_P=Speed_error*Sudu_PID_P
Speed_D=Speed_error_error*Sudu_PID_D
Speed_I+=Speed_error*Sudu_PID_I/10
/*if(Speed_error>5||Speed_error<-5)
{
Speed_I=0
} */
SpeedControlOutOld=Speed
Speed=Speed_P+Speed_I+Speed_D
}
void SpeedControlOutput(void)
{
fValue = Speed - SpeedControlOutOld
SpeedControlOut = fValue * (SpeedControlPeriod + 1) / 100 + SpeedControlOutOld
全国大学生智能汽车邀请赛技术报告
44
}
void PWM_out(void)
{
if(LeftMotorOut>0)
{
PWMDTY7=LeftMotorOut+5
PWMDTY5=0//左电机
}
else
{
PWMDTY5=-LeftMotorOut+5
PWMDTY7=0//左电机
}
if(RightMotorOut>0)
{
PWMDTY1=RightMotorOut+5//右电机
PWMDTY3=0
}
else
{
PWMDTY1=0
PWMDTY3=-RightMotorOut+5
}
if(angle>=10||angle<=-10)
{
PWMDTY1=PWMDTY3=PWMDTY5=PWMDTY7=0
}
}
void MotorOutput(void)
{
int Speed_left=0,Speed_right=0
Speed_left=(int)(ZHILI-SpeedControlOut+DirectionControlOut)
Speed_right=(int)(ZHILI -SpeedControlOut-DirectionControlOut)
if(Speed_left >250)Speed_left =250
else if(Speed_left <-250)Speed_left = -250
if(Speed_right >250)Speed_right =250
else if(Speed_right <-250)Speed_right = -250
附录 A
45
LeftMotorOut = Speed_left
RightMotorOut = Speed_right
PWM_out()
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 Pit0_interrupt(void)
{
DisableInterrupts
EventCount++
SpeedControlPeriod++
SpeedControlOutput()
DirectionControlPeriod++
DirectionControlOutput()
if(EventCount >=5)
{ // Motor speed adjust
EventCount = 0// Clear the event counter
BMQ_count++// Motor speed adjust
if(BMQ_count==1)
{
PWMDTY2=255 // Clear the event counter
GetLeftMotorPulse()
PWMDTY0=0
}
else if(BMQ_count==2)
{
BMQ_count=0
PWMDTY0=255
GetRightMotorPulse()
PWMDTY2=0
}
}
else if(EventCount == 1)
{
int i=0
for(i = 0i <20i ++)
SampleInputVoltage()
}
else if(EventCount == 2)
全国大学生智能汽车邀请赛技术报告
46
{
GetInputVoltageAverage()
AD_calculate()
zhili_kongzhi()
MotorOutput()
}
else if(EventCount == 3)
{ // Car speed adjust
SpeedControlCount ++
if(SpeedControlCount >= 20)
{
SpeedControl()
SpeedControlCount = 0
SpeedControlPeriod = 0
}
}
else if(EventCount == 4)
{ // Car direction control
DirectionControlCount ++
DirectionVoltageSigma()
if(DirectionControlCount >= 2)
{
DirectionControl()
DirectionControlCount = 0
DirectionControlPeriod = 0
}
// DirectionControlOutput()
}
PITTF_PTF0=1
EnableInterrupts
}
#pragma CODE_SEG DEFAULT
/********************************************************/
/*************************主函数*************************/
void Init_ALL()
{
Pll_Init()
附录 A
47
Pwm0_Init()
Pwm1_Init()
Pwm2_Init()
Pwm3_Init()
Pwm5_Init()
Pwm7_Init()
ATD0_Init()
Pit_Init()
Ect7_Init()
//Pulse_Add_Init()
init_key()
DDRT=0X7F//液晶使能
LCD_init()
LCD_clear()
PTT_PTT5=1//液晶背光
angle=(JIAODU_CENTER-ad_value[0] )*Vref/(1024*0.8)*90
}
void main(void)
{
DisableInterrupts
Init_ALL()
EnableInterrupts
for()
{
LCD_work()
}
/* please make sure that you never leave main */
我是刚参加过第七届飞思卡尔智能车竞赛的，我是光电组的，一般说来，摄像头是软件比较难，光电是硬件比较难，而电磁组的规则改成了直立了之后就是软硬件都有难度。其实关于那个组更有难度的话也因人而异，我觉得的话，无论什么组别，做车的前期靠的是硬件，但是到了后期，靠的就是软件的控制，当然也有例外的咯，后期的时候一些机械结构的调整和硬件的优化也是很有必要的。
关于那个组更好做，只要你掌握了解决问题的思路和方法，哪个组都不会很难，不过值得注意的是智能车是一个团队的竞赛，队伍成绩好不好也很大程度取决于队伍最弱的那一部分，最弱的这部分有可能出在硬件上也有可能出在软件上
其实选哪个组也得要综合考虑，比如说你们学校摄像头强你又希望取得好成绩，那你就可以首选摄像头，你对电磁组的直立感兴趣的话也可以选电磁组，你对一些机械手工之类感兴趣的话光电也是不错的选择，希望对你有所帮助
一、 参与范围（1）在8月24日预赛时，报名登记的参赛队如果在预赛中没有成绩或未进入决赛，可以凭报名时抽取的号码牌在2月25日决赛当天上午7点半（创意组时间为7点）领取车模准备参加户外挑战赛。
（2）进入8月25日决赛的队伍，若也想参加户外挑战赛，可在10点30前在完成本组比赛的前提下领取车模并到达挑战赛起点进行户外挑战赛。若上一条所述参赛队在10点30前仍未完成挑战赛，则不接受决赛队伍的报名。
二、 参与规则
（1）领取车模后，直接等候比赛，不安排实地测试时间。
（2）比赛以折返跑形式完成，到达终点后，由另一位队员调转车头回到起点。仅发车一次。
（3）比赛结束后必须交还车模，在决赛结束后进行车模展示。14：00安排车模领取。未交还车模的队伍，成绩取消。
三、 奖项设置
跑完比赛规定路段的队伍取三分之一获得二等奖，其余跑完的队伍获得三等奖。
四、参赛队伍名单
摄像头类抽签号码 学校名称 队伍名称 组别
1韶关学院韶魂-十里光电四轮组
7太原理工大学太原理工大学信标对抗队信标组
11重庆邮电大学永流渊光电四轮组
13齐鲁工业大学探索者光电四轮组
15中央民族大学MUC创意车队创意组
18枣庄学院Superman光电四轮组
23安徽信息工程学院追标组信标组
32合肥工业大学云梦之巅队光电四轮组
40西安邮电大学Zorro创意组
42厦门大学嘉庚学院PRT-笃行队光电四轮组
48太原理工大学太原理工大学创意队创意组
50华北电力大学（保定）华北电力大学光电四轮一队光电四轮组
51太原理工大学太原理工大学摄像头双车汇车队双车会车组
53大连理工大学Ameadeus创意组
55北京邮电大学皮卡丘快跑创意组
56厦门大学嘉庚学院PRT-自强队信标组
63黄山学院HY-作死的佩奇双车会车组
64湘潭大学兴湘学院羊牯塘车神2018光电四轮组
67燕山大学燕凌队光电四轮组
74阜阳师范学院光电四轮组
81青岛理工大学青岛理工大学创意一队创意组
82东华大学川夏了夏天双车会车组
87长春理工大学追风二队创意组
91南昌工程学院0 error 0 warning光电四轮组
92郑州轻工业大学封装不队双车会车组
95乐山师范学院乐师逐飞科技会车队双车会车组
97上海工程技术大学功诚五队信标组
99贵州大学佩奇快跑光电四轮组
100盐城工学院飞跃双车会车组
102常熟理工学院闪电五队光电四轮组
103大连海事大学同舟二队无线节能组
105中南大学比亚迪卯兔2018光电四轮组
108河海大学常州校区VNX光电四轮组
109山东师范大学开车开派对光电四轮组
110北华大学北华翔龙创意组
111江西科技学院阳晨科技信标组
115常州大学都是假象光电四轮组
119黑龙江工业学院蹑影追风光电四轮组
125天津中德应用技术大学AFZ队光电四轮组
127东北林业大学东林御风光电四轮组
129洛阳理工学院洛理碰碰车队双车会车组
131曲阜师范大学YC光电四轮组
134浙江科技学院费米队创意组
138重庆邮电大学素米线信标组
139江苏理工学院江理工旋风冲锋光电四轮组
144东北大学秦皇岛分校东秦四轮光电一队光电四轮组
145燕山大学里仁学院逸风车队双车会车组
146黄山学院HY-复兴三号光电四轮组
147中国石油大学（华东）石大光电一队光电四轮组
149成都工业学院成工光电队光电四轮组
电磁类抽签号码 学校名称 队伍名称 组别
1安徽新华学院虎纹鲨鱼电磁三轮组
2长春工业大学闪电极速无线节能组
3贵州大学楠泊湾无线节能组
5厦门大学嘉庚学院PRT-凌云队电磁直立组
6东南大学成贤学院驾长车踏破贺兰赛道无线节能组
7厦门大学嘉庚学院PRT-若谷无线节能组
8厦门大学南墙至臻电磁直立组
9哈尔滨工程大学极品飞车三队电磁三轮组
11西南科技大学西科五队无线节能组
12海南大学北极狼电磁直立组
13三江学院猫老师的喵无线节能组
15太原理工大学太原理工大学直立电磁队电磁直立组
16西华大学西华FC电磁三轮组
17安徽工程大学优秀青年队电磁直立组
18华东交通大学花椒先锋队电磁三轮组
19上海工程技术大学功诚0队无线节能组
20南昌大学南昌瓦罐汤双车会车组
23浙江万里学院万里节能1队无线节能组
26皖西学院电协胖虎二队电磁直立组
27青岛理工大学青岛理工大学电磁直立二队电磁直立组
28集美大学集大·高斯洛比达队双车会车组
29常熟理工学院物电电磁一队电磁三轮组
32陕西理工大学陕理工探索队双车会车组
33上海海事大学Omelette创意组
38安徽信息工程学院火箭一队电磁三轮组
39常熟理工学院闪电一队电磁直立组
41上海工程技术大学功诚三队电磁直立组
42厦门大学嘉庚学院PRT-长风队电磁三轮组
43贵州理工学院知行至善无线节能组
46西北工业大学承影2018双车会车组
47山东大学再见，其他队电磁三轮组
50南昌航空大学拖拉机电磁三轮组
51大连海事大学叁点叁叁电磁三轮组
52江苏科技大学奔跑的小烧杯无线节能组
56合肥工业大学东风神娃队无线节能组
57山东建筑大学电饭锅电磁直立组
58天津大学天津大学创想仪器电磁一队电磁三轮组
59辽宁工程技术大学FireFly一队电磁直立组
60西南科技大学西科三队电磁直立组
61临沂大学一米六八电磁直立组
62皖西学院电协佩琪零队双车会车组
65燕山大学里仁学院愣子三轮组电磁三轮组
68浙江万里学院万里三轮1队电磁三轮组
69燕山大学燕飞队电磁直立组
71杭州电子科技大学杭电直立一队电磁直立组
72阜阳师范学院皖K-EA128无线节能组
73天津中德应用技术大学跃起动力电磁三轮组
74浙江工业大学浙工大银江电磁一队电磁三轮组
75天津大学天津大学电磁平衡队电磁直立组
76西南科技大学西科九队双车会车组
77集美大学集大·特斯拉队无线节能组
78南昌工程学院队名是浮云双车会车组
81南昌大学黑子曙光无线节能组
82皖西学院电协胖虎一队电磁三轮组
84巢湖学院沐芝队双车会车组
86湖北汽车工业学院寸芒电磁直立组
88安徽三联学院学无止境无线节能组
90湖北汽车工业学院科技学院绝地求生三人帮电磁三轮组
91河南理工大学HPU电磁三轮一队电磁三轮组
92辽宁工程技术大学清风一队无线节能组
93乐山师范学院乐师逐飞科技直立队电磁直立组
96电子科技大学成电沉淀电磁直立组
97东北大学秦皇岛分校东秦三轮电磁一队电磁三轮组
98西安邮电大学听风忆帆双车会车组
100辽宁工程技术大学LNTU一队电磁三轮组
101东北大学秦皇岛分校东秦两轮直立一队电磁直立组
102金华职业技术学院金色年华电磁三轮组
105湖北文理学院湖文明志一队电磁三轮组
108青岛理工大学青岛理工大学无线节能一队无线节能组
112贵州理工学院雪后初晴电磁直立组
113山东大学（威海）武东中华无线节能组
114临沂大学Macro电磁三轮组
115中国计量大学仰仪9队电磁直立组
116齐鲁工业大学胖虎七队电磁三轮组
117江苏理工学院江理工FX队双车会车组
118集美大学集大·卡尔曼队电磁直立组
120黑龙江科技大学速墨四队无线节能组
122杭州电子科技大学信息工程学院杭电信工普源电磁三轮二队电磁三轮组
124南昌航空大学煎鸡蛋队电磁直立组
125攀枝花学院芒果mxz电磁三轮组
128安徽财经大学安财直立零队电磁直立组
129南昌航空大学秋名山双车会车组
131长江大学长大三号电磁直立组
132华北理工大学听力全队无线节能组
133江西科技学院没有这个队电磁直立组
134安徽三联学院心有灵犀电磁直立组
138石家庄学院三蹦子队电磁三轮组
139临沂大学节能二队无线节能组
142浙江大学浙大直立一队电磁直立组
143嘉兴学院文杰双车一队双车会车组
145重庆邮电大学Auto-e电磁直立组
146曲阜师范大学曲师大会车二队双车会车组
148盐城工学院三蹦子队电磁三轮组
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）