?? freescale-smartcar.txt
字號:
附錄A 模型車控制主程序代碼
#include <hidef.h> /* common defines and macros */
#include <mc9s12dp512.h> /* derivative information */
#include "STDINC.h"
#include "stdlib.h"
#include "sci.h"
#pragma LINK_INFO DERIVATIVE "mc9s12dp512"
//////////////////////////參數設定//////////////////////
//舵機直行的PWM--2400
//馬達60%--1200
#define OK 1
#define WRONG 0
const int16 RUDKP=33;//50;
const int16 RUDKD=5;//5;
const float MOTKP=0.005263;
const float MOTKI=0.005263;
const float MOTKD=0.005263;
const float CONSTANT1=136;
const float CONSTANT2=5;
const float CONSTANT3=4;
const float CONSTANT4=3;
int16 RErrLast=0;
int16 RErrCurr=0;
int16 RErrPreLast=0;
int16 Ruddelta=0;
int16 RudPWM;
int16 SenFlg=0;
int16 SpeedCurr=0;
int16 SpeedLast=0;
int16 Speed=0;
double SpeedFreq=0;
int16 Motdelta=0;
int16 MotPWM=1000;
int16 SErrLast=0;
int16 SErrCurr=0;
int16 SErrPreLast=0;
int16 MotDD=0;
/////////////////速度參數///////////////////////////
float Pdelta[4]={0.94,0.82,0.52,0.65};
float Ddelta[4]={0.4,0.2,0.21,0};
int16 Vline=3040;
int16 Vscor=2565;
int16 Vlcor=2280;
int16 Vbrek=2375;
int16 Vsbrk=1330;
int16 Vstri=2600;
//Global variables
int16 SenCurBacW,SenLasBacW;
int16 TarSpeed,LastTarSpeed;
float Modulus,TarSpeedF;
int16 TarFlg=0;
uint8 sensorA=0,sensorB=0;
uint8 SensorA=0,SensorB=0;
int16 Sensor;
uint16 CounterCurr,CounterLast,Counter;
int16 tmpflg;
int16 look;
int16 LapFlg1=0;
int16 LapFlg2=0;
int16 Flag=0;
int16 temp=0;
int16 LapNum=0;
int16 LNum=0;
int16 FLAG=0;
int16 LFlg=0;
int16 delay1=0;
int16 StopFlg=0,StopNum=0;
int16 j=0;
int16 tmp[39]={0};
int16 num=39;
int16 s1,s2,s3;
int16 Tar,cnt1,cnt2,cnt3,cnt4,cnt5;
int16 flag4;
//////////////////////////函數聲明//////////////////////
void atd_init(void);
void motor_ctrl(void);
void readsensor(void);
void target_speed(void);
void readspeed(void);
void rudder_ctrl(void);
int8 path_recog(void);
void sort(void);
void startline(void)
////////////////////////中斷函數/////////////////
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt void readsensor(void){
DisableInterrupts;
sensorA=PTM;
sensorB=PORTB;
sensorA=sensorA&0x3F; //clean unwanted bit
sensorB=sensorB&0x3F;
Flag=0;
if(LFlg==1)
{
delay1++;
if(delay1>=5)
{
LFlg=0;
delay1=0;
}
}
else startline();
if(StopFlg==1)
{
StopNum++;
}
TarFlg++;
if(LapNum>2) {
PWMDTY45=1000;
}
PWMDTY01=1500;
MCCNT=20000;
MCFLG_MCZF=1;
EnableInterrupts;
}
uint16 plsnum=0;
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt void readspeed(void){
TFLG1_C3F=1;
//CounterLast=CounterCurr;
CounterCurr=TC3;
CounterLast=TC3H;
switch(TFLG2_TOF)
{
case 0:Counter=CounterCurr-CounterLast;
break;
case 1:Counter=65535-CounterLast+CounterCurr;
TFLG2_TOF=1;
break;
}
if(Counter<16)
{
Counter=16;
}
Speed = 1000000/Counter;//190;//((float)Speed*3/4+(float)SpeedFreq/4);
if(LapNum==1)
{
plsnum++;
}
if(LapNum>1)
{
StopFlg=1;
if(StopNum>100)
{
PWMDTY45=1000;
}
}
else
motor_ctrl();
//FLAG++;
}
////////////////////////函數定義//////////////////////////////////
void pwm_init(void){
PWMPOL = 0xFF; //polarity,high at the beginning;
PWMCLK = 0x2A; //select SA/SB as the pwmclk;SA clock for STEER;SB clock for MOTOR;
PWMPRCLK = 0x11; //A=bus clock/2=16MHz,B=bus clock/2=16MHz;
PWMCAE = 0x00; //PWM flush left
PWMSCLA = 0x01; //SA=A/2;PWM clock=bus clock/2/2/1=8MHz
PWMSCLB = 0x05; //SB=B/1;PWM clock=bus clock/2/2/5=1.6MHz
PWMCTL = 0xF0; //concatenate 0&1,2&3;
PWMPER45 = 2000; //output frequency=SA clock/2000=4KHz;for MOTOR using
PWMDTY45 = 1000; //PWM percent is 65%;
PWMPER23 = 16000; //output frequency=SB clock/16000=100Hz;for RUDDER using
PWMDTY23 = 2400; //PWM percent is 15% down--LEFT
PWME=0x2A;
}
void ect_init(void) {
MCCTL=0xC2; //MCZI MODMC RDMCL ICLAT FLMC MCEN MCPR1 MCPR0=11000010B clock=Fbus/8
MCCNT=20000; //5ms every interrupt
MCFLG=0x80; //set underflow flag
//////////////////////////////PACA_init//////////////////////////////////////
TIOS=0X00; //input capture selection,channal 6: output compare channal;
TCTL3=0X01;
TCTL4=0X40; //capture on the rasing edge;
TIE=0X18; //enable interrupt when captured;
TSCR2=0X05; //bit7:overflow interrupt disable,bit[2:0]:timer=bus clock/32=1MHz
ICOVW=0X00; //overwrite by the new data;
ICSYS=0X02; //queue mode;
TSCR1=0x80; //enable the timer;
MCCTL_MCEN=1;
}
//////////////////////////////sci_init/////////////////////////////////
void SciInt(void){
SCI0BDL=0xD0; //總線為32M，波特率為9600
SCI0CR2=0X2C; //允許中斷，允許發送，允許接受
}
void SCI_Trans(byte data) {
while (!SCI0SR1_TDRE);//SC0DR處于忙狀態，等待。
SCI0DRL=data;
}
void systemboot(void) {
SYNR = 7; //Fbus=Fosc*(SYNR+1)/(REFDV+1)=32MHz
REFDV = 3;
PLLCTL = 0x71;
while(CRGFLG_LOCK==0){}
CLKSEL = 0x80;
DDRA = 0xFF; //PortA is input
PORTA=0x00; //clean PortA
DDRB = 0x00; //PortB is input
PORTB=0x00; //clean PortB
PORTK= 0x00;
DDRK = 0x00;
PTM = 0x00;
DDRM = 0x00;
pwm_init();
ect_init();
SciInt();
//////////////////////////////////////////
if(PORTK_BIT0==1&&PORTK_BIT1==0)
{
Vline=2755;
Vscor=2375;
Vlcor=2280;
Vbrek=2185; //可嘗試變更2090-2185
Vsbrk=1330;
Vstri=2400;
Pdelta[0]=0.72;
Pdelta[1]=0.77;
Pdelta[2]=0.83;
Ddelta[0]=0.4;
Ddelta[1]=0.2;
Ddelta[2]=0.21;
}
else if(PORTK_BIT0==0&&PORTK_BIT1==1)
{
Vline=3135;
Vscor=2660;
Vlcor=2375;
Vbrek=2440; //可嘗試變更2090-2185
Vsbrk=1330;
Vstri=2700;
Pdelta[0]=0.67;
Pdelta[1]=0.72;
Pdelta[2]=0.77;
Ddelta[0]=0.4;
Ddelta[1]=0.2;
Ddelta[2]=0.21;
}
else if(PORTK_BIT0==1&&PORTK_BIT1==1)
{
Vline=2565;
Vscor=2280;
Vlcor=2185;
Vbrek=2185; //可嘗試變更2090-2185
Vsbrk=1330;
Vstri=2300;
Pdelta[0]=0.67;
Pdelta[1]=0.72;
Pdelta[2]=0.77;
Ddelta[0]=0.4;
Ddelta[1]=0.2;
Ddelta[2]=0.21;
}
if(PORTK_BIT2==1){Vscor=Vscor+95;Vstri=Vstri+95;}
if(PORTK_BIT3==1){Vscor=Vscor+190;Vstri=Vstri+190;}
if(PORTK_BIT4==1){Vlcor=Vlcor+95;}
if(PORTK_BIT5==1){Vlcor=Vlcor+190;}
if(PORTK_BIT7==1){Vline=Vline+95;}
if(PORTB_BIT7==1){Vline=Vline+190;}
}
#pragma CODE_SEG DEFAULT
void main(void) {
/* put your own code here */
DisableInterrupts;
systemboot();
asm{
nop;
}
EnableInterrupts;
for(;;) {
path_recog();
sort();
rudder_ctrl();
} /* wait forever */
/* please make sure that you never leave this function */
}
/////////////////MAIN END//////////////
/////////////////PATH RECOGNISE////////
int8 path_recog(void){
int16 i;
int16 sen_num=-6;
int16 sen_cnt=0;
int16 sen_pos[12];
uint8 mask;
tmpflg=0;
mask=0x20;
for(i=-10;i<=0;i+=2)
{
if((mask&SensorA)==0)
{
tmpflg+=i;
sen_pos[sen_cnt]=sen_num;
sen_cnt++;
}
sen_num++;
mask>>1;
}
mask=0x01;
for(i=0;i<=10;i+=2)
{
if((mask&SensorB)==0)
{
tmpflg+=i;//tmpflg=tmpflg+i-1;
sen_pos[sen_cnt]=sen_num;
sen_cnt++;
}
sen_num++;
mask<<1;
}
if(sen_cnt==0)
{
if(Sensor>=8)
{
Sensor=10; PWMDTY23=2160;
if(MotPWM<=1370) PWMDTY01=1370;
else PWMDTY01=MotPWM-30;
Flag=1;
}
else if(Sensor<=-8)
{
Sensor=-10; PWMDTY23=2640;
if(MotPWM<=1370) PWMDTY01=1370;
else PWMDTY01=MotPWM-30;
Flag=1;
} return 0;
}
if(sen_cnt==1);
else for(i=1;i<sen_cnt;i++)
{
if(sen_pos[i]-sen_pos[i-1]>=2)
{
return 0;
}
}
look=tmpflg;
Sensor=tmpflg/sen_cnt;
return 0;
}
void startline(void){
if( ((sensorA & 0x30)==0x00||(sensorA & 0x18)==0x00)
// ((sensorB & 0x30)==0x00||(sensorB & 0x18)==0x00)
&& ((sensorA & 0x03)==0x03||(sensorB & 0x03)==0x03)
&& ((sensorA & 0x06)!=0x00||(sensorB & 0x06)!=0x00))
{
//LapNum++;
LapFlg1=1;
FLAG=1;
LNum=0;
}
if( // ((sensorA & 0x30)==0x00||(sensorA & 0x18)==0x00)
((sensorB & 0x30)==0x00||(sensorB & 0x18)==0x00)
&& ((sensorA & 0x03)==0x03||(sensorB & 0x03)==0x03)
&& ((sensorA & 0x06)!=0x00||(sensorB & 0x06)!=0x00))
{
LapFlg2=1;
FLAG=1;
LNum=0;
}
if(LapFlg1==1&&FLAG==1)
{
if(LapFlg2==1)
{
LapNum++;
LapFlg1=0;
LapFlg2=0;
FLAG=0;
LNum=0;
LFlg=1;
}
}
if(LapFlg2==1&&FLAG==1)
{
if(LapFlg1==1)
{
LapNum++;
LapFlg2=0;
LapFlg1=0;
FLAG=0;
LNum=0;
}
}
}
////////////////Delta Kp///////////////
////////////////RUDDER CONTRAL/////////
void rudder_ctrl(void){
float Kp,Kd;
if(Sensor>=8||Sensor<=-8) temp=0;
else if(Sensor>4||Sensor<-4) temp=1;
else temp=2;
RErrLast=RErrCurr;
RErrCurr=Sensor;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -