chengxu.rar_smart car freescale_路径记忆_飞思卡尔_飞思卡尔 电磁组_飞思卡尔赛车

#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_Dat