基于路径记忆的最佳赛车控制算法程序.txt

飞思卡尔杯智能汽车竞赛清华大学三角洲队基于路径记忆的最佳赛车控制算法程序

#include "stdinc.h"
#include "utility.h"
#include "EEPROM.h"
#include "ADConv.h"
#include "PAC.h"
#include "adapter_C_CPP.h"
#include "PID.h"
#include "Sci.h"
#include "rule_table.h"
#include "trash.h"
#include "operation.h"
#include "course.h"
#include "widget.h"
#define Pixel_Num 6
#define Line_Num 50
xuint16 Line_Sample_Interval=5;
xuint16 Start_Of_Frame=30;
xuint16 End_Of_Frame=0;
#define PULSE_CNT 750
//xuint16 speedPulse[PULSE_CNT];
xuint16 Frame_Data[Line_Num][Pixel_Num];
xuint8 Frame_OddEven=0;
xint16 Frame_Data_Processed[Line_Num][2];
//xuint8 f[6];
xuint16 frame_num=PULSE_CNT;
xint16 position_temp[PULSE_CNT];
//xint16 rudder_temp[PULSE_CNT];
xint16 curve_temp[PULSE_CNT];
xuint8 line_num=0,pixel_num=0;
void system_init(void);
void auto_ctrl(void);
void frame_data_process(void);
void route_opti(xint16);
void Set_Rudder(xint16 Position);
xint16 get_curve(void);
void Set_Speed(xuint16 curve);
//PID
PID g_speedPID;
PID g_rudderPID;
//course
extern void timer_interrupt(void);
void system_init(void);
route_status Route_Status;
//--Program entry-------------------------------------------------------------
void main(void)
{
xuint16 delay;
xuint8 i,j;
system_init();
wait_until_button_up(0);
Frame_OddEven=PORTA_BIT0;
while(Frame_OddEven==PORTA_BIT0);
Frame_OddEven=PORTA_BIT0;
EnableInterrupts;
//wait_until_button_up(1);
while(frame_num||1)
{
}
DisableInterrupts;
g_motor_set_status(FAST_STOP);
//g_motor_set_status(REVERSE);
g_motor_set_speed(255);
wait_until_button_up(0);
while(1)
{
}
}
//--Function Implementation---------------------------------------------------
void actuate_speed_PID(xint16 speed)
{
//xint16 disableThresh = 70;
//xint16 highThresh = 25;
//xint16 lowThresh = 5;
xint16 PWMout = PIDCalc(&g_speedPID, speed);
if (PWMout>0)
{
g_motor_set_status(FORWARD);
g_motor_set_speed(PWMout);
}
else if (PWMout<=0) {
// g_motor_set_status(REVERSE);
if (PWMout>-50)
{
g_motor_set_speed(-PWMout);
g_motor_set_status(REVERSE);
}
else if(PWMout>-255)
{
g_motor_set_speed(-PWMout);
g_motor_set_status(FAST_STOP);
}
else
{
g_motor_set_speed(255);
g_motor_set_status(REVERSE);
// else g_motor_set_status(FREE_RUN);
}
}
else
g_motor_set_status(FAST_STOP);
}
//----------------------------------------------------------------------------
#define PWMCNT 7
#define ANGLECNT 6
xuint8 PWMSerial[PWMCNT] = {100, 130, 160, 190, 220, 255};
xint8 AngleSerial[ANGLECNT] = {10, 16, 22, 28, 34, 40};
xint8 PWMSelected = 0;
xint8 PWMIndex = 0;
xint8 AngleSelected = 0;
xint8 AngleIndex = 0;
xint8 stepResponseCnt = 0;
//--Timer interrupt service function------------------------------------------
//VECTOR ADDRESS 0xFFCA timer_interrupt
#pragma TRAP_PROC
void timer_interrupt(void) {}
//----------------------------------------------------------------------------
void system_init(void)
{
xuint16 delay;
//PLL settings
PLLCTL_ACQ = 1;
PLLCTL_PLLON = 1;
SYNR = 0x02;
// SYNR=0x01;
REFDV = 0x01;
//wait for a given time
for (delay=0;delay<0xffff;++delay){}
CLKSEL_PLLSEL = 1;
DDRT=0x00;
//PACA
init_PACA();
set_PACA(0);
init_PACB();
//motor
// timer_init();
//Timer's settings
//Sensor IO direction
DDRA=0x00;
DDRB=0xFF;
//
DDRH=0xFF;
init_SCI(BAUD_9600);
// PIDInit(&g_speedPID, 2.5, 0.1, 0, 255, 0);
PIDInit(&g_speedPID, 30, 0.5, 0, 255, 0);
// PIDInit(&g_speedPID,5, 0.1, 0, 255, 0);
g_speedPID.SetPoint = 50; //35
}
//----------------------------------------------------------------------------
void Set_Rudder(xint16 Position);
xint16 get_curve(void);
void Set_Speed(xuint16 curve);
xint16 Get_position(void);
void Set_Param(void);
xuint8 Get_Line_Selected(xuint16);
xuint8 Line_Selected = 25; //32 = 40cm
#define ROUTE_CENTER 670
xint16 route_center = ROUTE_CENTER ; // for adj
//#define Predict_Line_Num 6
#define Predict_Line_Begin 5 //5
#define Predict_Line_End 39 //40
xuint8 Predict_invalid_cnt = 0;
void auto_ctrl(void)
{
volatile xint16 position=0;
xuint8 i=0;
static xuint8 cnt = 0;
xuint8 speed = 0;
volatile xint16 curve=0;
curve = get_curve();
curve_temp[PULSE_CNT-frame_num]=curve;
// curve_tmp = curve;
// g_led_dec(abs(position)/10);
g_led_dec(abs(curve)/20);
// g_led_dec(Line_Selected);
// route_opti(curve);
//g_rudder_set_status(Route_Status);
// g_rudder_set_angle(30);
// Set_Param();
// g_rudder_set_status(Route_Status);
// g_speedPID.SetPoint=55;
// Line_Selected = 27;
// g_led_dec(g_rudder_get_d()/10);
// g_led_dec(Line_Selected);
// g_led_dec(Route_Status);
Set_Speed(curve);
/*
cnt++;
if(cnt==10)
{
g_led_dec(abs(curve)/10);
frame_num--;
position_temp[50-frame_num]=position;
rudder_temp[50-frame_num]=g_rudder_set_angle_p(position);
cnt = 0;
}
*/
}
#define Speed_Straight 90
#define Speed_Curve 60
#define Speed_S_Curve 65
#define Position_Dangerous 400
//#define Position_Devia_Trend
void Set_Param(void)
{
static xint16 last_position=0;
xint16 position_trend=0;
xint16 position;
xint16 angle;
switch (Route_Status)
{
case STRAIGHT:
if(g_speedPID.SetPoint<Speed_Straight)
g_speedPID.SetPoint+=10;
Line_Selected = Get_Line_Selected(get_PACA());
// dead1 = 10, dead2 =150, slope1 = 8, slope2 = 5, saturation = 100;
break;
case CURVE_L:
g_speedPID.SetPoint= Speed_Curve;
Line_Selected = Get_Line_Selected(g_speedPID.SetPoint);
break;
case CURVE_R:
g_speedPID.SetPoint= Speed_Curve;
Line_Selected = Get_Line_Selected(g_speedPID.SetPoint);
break;
case S_CURVE:
g_speedPID.SetPoint= Speed_S_Curve;
Line_Selected = 32;//Get_Line_Selected(g_speedPID.SetPoint);
break;
case DANGEROUS:
break;
}
position=Get_position();
angle=g_rudder_get_angle();
if(abs(position)>Position_Dangerous)
{
position_trend=position-last_position;
if((position>0&&position_trend>0)||(position<0&&position_trend<0))
g_speedPID.SetPoint-=0;
}
last_position=position;
/*
if(abs(angle)>=100)
{
position_trend=position-last_position;
if((angle>0&&position_trend>0)||(angle<0&&position_trend<0))
g_speedPID.SetPoint-=5;
}
*/
last_position=position;
}
#define Speed_Norm 55
#define Line_Selected_Norm 27
#define Line_Selected_Max 30
xuint8 Get_Line_Selected(xuint16 speed)
{
xuint16 line=0;
line=speed*Line_Selected_Norm/Speed_Norm;
if(line>Line_Selected_Max)
line=Line_Selected_Max;
return (xuint8)(line);
}
void Set_Rudder(xint16 Position)
{
}
//====================== realtime control area ========================
#define Line_Width_Norm 627 //30cm line19
#define Position_Comp_Slope 18
#define Position_Comp_Intercept 968
xint16 Get_position(void)
{
long int pos,pos_ori;
static xint16 pos_last = 4000;
#define DIFF_VAL 800
xint16 line_width=0;
pos_ori = Frame_Data_Processed[Line_Selected][0];
//根据前瞻距离补偿pos
line_width = - Line_Selected*Position_Comp_Slope + Position_Comp_Intercept;
pos = pos_ori * line_width / Line_Width_Norm;
//跳变处理，跳变过大则滤去
if (abs(pos - pos_last) < DIFF_VAL || pos_last == 4000)
pos_last = pos;
else
pos = pos_last;
return (xint16)(pos);
}
//====================== speed estimation ==============================
xint16 get_curve(void)
{
static xuint8 idx=0;
/*
xint16 average=0;
xuint8 i=0;
xuint16 curve=0;
for(i=0;i<Predict_Line_Num;i++)
average+=Frame_Data_Processed[Line_Selected-i-1][0];
for(i=0;i<Predict_Line_Num;i++)
curve+=abs(Frame_Data_Processed[Line_Selected-i-1][0]-average);
return curve;
*/
// 斜率的方差作为 曲率的估计
// Var[x]=E[x^2]-[E(x)]^2
volatile xint16 curve = 0;
long int curve_part1 = 0;
long int curve_part2 = 0;
xuint8 i, num = Predict_Line_End - Predict_Line_Begin;
/*
if (Predict_invalid_cnt > 4)
{
Predict_invalid_cnt=0;
return(0x0FF);
}
*/
for (i = Predict_Line_Begin + 1 ; i < Predict_Line_End+1 ; i++)
{
curve_part1 += (Frame_Data_Processed[i][0] - Frame_Data_Processed[i-1][0]) *
(Frame_Data_Processed[i][0] - Frame_Data_Processed[i-1][0]) ;
curve_part2 += (Frame_Data_Processed[i][0] - Frame_Data_Processed[i-1][0]) ;
}
curve = ( curve_part1 * num - curve_part2 * curve_part2) / (num*num) ;
return(curve);
}
#define PID_OR_NOT 0 // 0: PWM CONTROL 0 default
// 1: PID CONTROL
void Set_Speed(xuint16 curve)
{
volatile xuint16 speed=0;
static xuint8 cnt=0;
speed=get_PACA();
//speedPulse[PULSE_CNT-frame_num]=speed;
frame_num--;
set_PACA(0);
/*
if (curve < 100)
speed = 100;
else if (curve < 200)
speed = 60;
else if (curve < 300)
speed = 40;
*/
cnt++;
if(cnt<20)
{
//g_led_dec(speed/2);
cnt=0;
}
#if PID_OR_NOT
// if(frame_num<200)
actuate_speed_PID(speed);
// else
// g_motor_set_status(FAST_STOP);
#else
g_motor_set_status(FORWARD);
g_motor_set_speed(10);
#endif
}
//=========================== 数据处理每行处理一次 ================
void frame_data_process(void)
{
xuint8 p=0;
xint16 delta[Pixel_Num], pos = - route_center;
static xint16 pos_last = 0;
xuint8 j=0;
static xuint8 begin_valid = 0; //起始若干行有效性判断
static xint16 begin_pos_last = 0;
static xint16 slope_last = 0 , slope = 0; //斜率
static xuint8 slope_invalid = 2; //斜率有效性，0 时为有效
static xint16 width_last;
//==========s-curve identification use============
static xuint8 s_cnt = 0 ;
#define S_CNT_VALVE 10
static xuint8 s_iden1 = 0 , s_iden2 = 0;
static xint8 s_slope_now , s_slope_last;
static xint8 s_slope1; // slope of the first part of S-Curve
//===============================================
volatile xint16 position=0;
/*
if(line_num==7)
PORTB_BIT0=1;
*/
if (line_num == 0)
{
begin_valid = 0;
slope_invalid = 2;
slope_last = 0;
}
for(p=0;p < Pixel_Num-1;p++)
{
if ( (Frame_Data[line_num][p+1] == 0) && (begin_valid == 0) ) //起始无效行检测
{
begin_valid = 0;
break;
}
if(Frame_Data[line_num][p]<Frame_Data[line_num][p+1]) //计算脉冲宽度
delta[p]=Frame_Data[line_num][p+1]-Frame_Data[line_num][p];
else
delta[p]=65536-Frame_Data[line_num][p]+Frame_Data[line_num][p+1];
slope = pos - pos_last + delta[p]/2;
if ( p >= 1 &&
(delta[p] > 20) && (delta[p] < 200) &&
//线宽检验
((abs(slope) < 100) || !line_num || !begin_valid ) && //位置跳变检验
((abs(delta[p] - width_last) < 30) || !line_num || !begin_valid ) &&
//宽度不能跳变 30
(( abs(pos+delta[p]/2 - begin_pos_last) < 200 ) || begin_valid ||1)
//起始端位置不能跳变
)
{
if((abs(slope-slope_last) < 30) || (slope_invalid) ) //斜率不能调变 50
{
Frame_Data_Processed[line_num][0] = pos + delta[p]/2 ;
Frame_Data_Processed[line_num][1] = delta[p];
if(begin_valid == 0)
begin_pos_last = Frame_Data_Processed[line_num][0];
begin_valid = 1;
if(slope_invalid >0)
slope_invalid-- ;
slope_last = slope ;
break;
}
}
else if (p == (Pixel_Num - 2) && (line_num > 0)) //
无效处理
{
Frame_Data_Processed[line_num][0] = Frame_Data_Processed[line_num-1][0] +
sign(slope_last) * min(20,abs(slope_last)); //20
Frame_Data_Processed[line_num][1] = Frame_Data_Processed[line_num-1][1];
// if (line_num>Predict_Line_Begin && line_num<Predict_Line_End)
Predict_invalid_cnt++;
}
pos += delta[p];
}
/*
*/
//饱和区
if ( Frame_Data_Processed[line_num][0] > 700)
Frame_Data_Processed[line_num][0] = 700;
if ( Frame_Data_Processed[line_num][0] < -700)
Frame_Data_Processed[line_num][0] = -700;
pos_last = Frame_Data_Processed[line_num][0];
width_last = Frame_Data_Processed[line_num][1];
// clear
for(j=4;j<Pixel_Num;j++)
{
Frame_Data[line_num][j]=0;
}
//====================== rudder control =========================
if(line_num == 30)
{
position = Get_position(); // Frame_Data_Processed[Line_Selected][0];
position_temp[PULSE_CNT-frame_num]=position;
g_rudder_set_angle_p(position);
// g_led_dec(abs(position)/10);
}
//====================== s-curve identification
===========================
if(line_num == 0)
{
s_slope_last = 0;
s_cnt = 0;
s_iden1 = 0;
s_iden2 = 0;
Route_Status = STRAIGHT;
}
else
{
s_slope_now = (Frame_Data_Processed[line_num][0] -
Frame_Data_Processed[line_num-1][0]);
if (abs(s_slope_now)<5)
s_slope_now = 0;
else
s_slope_now = sign(s_slope_now);
if( s_slope_now != 0)
{
if (s_slope_now == s_slope_last)
s_cnt++;
else
s_cnt = 0;
if ( s_cnt > S_CNT_VALVE )
{
if(s_iden1 == 0)
{
s_iden1 = 1;
s_slope1 = s_slope_now;
}
else if (s_slope_now != s_slope1)
s_iden2 = 1;
}
s_slope_last = s_slope_now;
}
if (s_iden2 == 1)
Route_Status = S_CURVE;
}
//====================================================================
=======
}
//========================= route optimization =====================
#define Turn_Curve_H 1 //定义的认为前方为弯道的Curve 值
#define Turn_Curve_L 1
#define Route_Delta_Step 20 //入弯过程每次调整的偏移量
#define Route_Delta_Max 500 //最大偏移量，需标定与10cm 左右对应
//#define S_Turn_Curve 50
//#define S_Curve_Bound 300
#define Slope_Delta 50
#define S_Curve_ID_Num 5
void route_opti(xint16 curve)
{
static route_status Route_Status_Last=STRAIGHT;
//static xuint8 turn_flag=0;
//static xuint8 turn_exit_flag=0;
static xint16 route_delta=0;
xuint8 i=0;
xuint8 slope_increase_num=0;
xuint8 slope_decrease_num=0;
static xuint8 straight_cnt=0;
// status identification
/*
for(i=Predict_Line_Begin+1;i<Predict_Line_End;i++)
{
if(Frame_Data_Processed[i][2]>Slope_Delta)
slope_increase_num++;
else if(Frame_Data_Processed[i][2]<-Slope_Delta)
slope_decrease_num--;
}
if(slope_increase_num>S_Curve_ID_Num&&slope_decrase_num>S_Curve_ID_Num)
Route_Status = S_CURVE;
*/
if(Route_Status==S_CURVE)
{
if(curve<=Turn_Curve_L || curve > 140)
Route_Status==STRAIGHT;
}
if(Route_Status!=S_CURVE)
{
if(curve>Turn_Curve_H)
{
curve *= sign(Frame_Data_Processed[35][0]-Frame_Data_Processed[5][0]) ; //
给curve 符号
if(curve>0)
Route_Status = CURVE_L;
else
Route_Status = CURVE_R;
}
else
{
if(Route_Status_Last==CURVE_L||Route_Status_Last==CURVE_R)
{
if(curve<Turn_Curve_L)
straight_cnt++;
if(straight_cnt>=3)
{
Route_Status=STRAIGHT;
straight_cnt=0;
}
}
else
{
Route_Status = STRAIGHT;
straight_cnt=0;
}
}
}
Route_Status_Last=Route_Status;
}