📄 freescale-smartcar.txt
字号:
Kp=Pdelta[temp]*(float)RUDKP;
Kd=Ddelta[temp]*(float)RUDKD;
Ruddelta=(int16)Kp*RErrCurr+(int16)Kd*(RErrCurr-RErrLast);
RudPWM=2400-Ruddelta;
if(RudPWM<2170)
RudPWM=2170;
if(RudPWM>2630)
RudPWM=2630;
PWMDTY23=RudPWM;
}
////////////////PATH CLASSI///////////////
void sort(void){
if(TarFlg>1)
{
TarFlg=0;
for(j=0;j<num-1;j++)
{
tmp[j]=tmp[j+1];
}
tmp[num-1]=Tar;
for(j=0;j<num;j++)
{
if(tmp[j]<-2)
{
s1=1;
break;
}
else
{
s1=0;
}
}
for(j=0;j<num;j++)
{
if(tmp[j]>=-2&&tmp[j]<=2)
{
s2=1;
break;
}
else
{
s2=0;
}
}
for(j=0;j<num;j++)
{
if(tmp[j]>2)
{
s3=1;
break;
}
else
{
s3=0;
}
}
if(Tar<=-7)
{
cnt1++;
cnt2++;
cnt3=0;
cnt4=0;
cnt5=0;
if(cnt1>num-10)
{
flag4=1;
TarSpeed=Vlcor;//12*190;
PORTA=0xC0;
}
else
{
TarSpeed=Vlcor;//12*190;
PORTA=0xC0;
}
}
if(Tar<=-4&&Tar>-7)
{
cnt2++;
cnt1++;
cnt3=0;
cnt4=0;
cnt5=0;
if(cnt2>num-10)
{
flag4=1;
TarSpeed=Vscor;//12*190;
PORTA=0xF0;
}
else
{
TarSpeed=Vscor;//12*190;
PORTA=0xF0;
}
}
if(Tar>-4&&Tar<4)
{
cnt3++;
cnt1=0;
cnt2=0;
cnt4=0;
cnt5=0;
if(cnt3>num-10)
{
flag4=3;
TarSpeed=Vline;
PORTA=0xFF;
}
else
{
TarSpeed=Vstri;
PORTA=0xFC;
}
}
if(Tar>=4&&Tar<7)
{
cnt4++;
cnt1=0;
cnt2=0;
cnt3=0;
cnt5++;
if(cnt4>num-10)
{
flag4=5;
TarSpeed=Vscor;
PORTA=0xF0;
}
else
{
TarSpeed=Vscor;
PORTA=0xF0;
}
}
if(Tar>=7)
{
cnt5++;
cnt1=0;
cnt2=0;
cnt3=0;
cnt4++;
if(cnt5>num-10)
{
flag4=5;
TarSpeed=Vlcor;
PORTA=0xC0;
}
else
{
TarSpeed=Vlcor;
PORTA=0xC0;
}
}
if(flag4==1)
{
t1++;
if(Tar>=7||Tar<=-7)
{
TarSpeed=Vbrek;
PORTA=0x81;
}
}
}
}
////////////////MOTOR CONTRAL///////////////
void motor_ctrl(void){
int16 MotDD=0;
SErrPreLast=SErrLast;
SErrLast=SErrCurr;
SErrCurr=TarSpeed-Speed;
MotDD=(800*MotDD+200*(int16)(MOTKD*(float)(SErrCurr+SErrPreLast-2*SErrLast)))/1000;
Motdelta=(int16)(MOTKP*(float)(SErrCurr-SErrLast)+MOTKI*(float)SErrCurr+MotDD);//(SErrCurr-SErrLast);
if(Motdelta>25)
{
Motdelta=25;
}
else if(Motdelta<-25)
{
Motdelta=-25;
}
MotPWM=MotPWM+Motdelta;
if(MotPWM>1900)
{
MotPWM=1900;
}
else if(MotPWM<200)
{
MotPWM=200;
}
PWMDTY45=MotPWM;
}
////////////////////////////////////sci receive isr////////////////////////////////////////////////
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt void sci0(void){ //receive interruption
int8 data_rec,digi[7];
int8 seq=0;//flagSci;
int16 timerSCI,iCoeRec;
int16 dispflag;
//byte iDis,iCol;
//byte ledstate[16],leddata,i ;
//int temp;
//pointPos=0;
dispflag=WRONG;
DisableInterrupts;
for(seq=0;seq<7;seq++){ //receive sequency;
while(SCI0SR1_RDRF==0){
timerSCI++;
if(timerSCI>4000) {
display('F','E',0);
// EnableInterrupts;
return;
}
} //receive buffer full flag;
timerSCI=0;
data_rec=SCI0DRL;
digi[seq]=data_rec;
switch(digi[seq]){ //change the ASCII to decimal;
case 48 : digi[seq]=0; break;
case 49 : digi[seq]=1; break;
case 50 : digi[seq]=2; break;
case 51 : digi[seq]=3; break;
case 52 : digi[seq]=4; break;
case 53 : digi[seq]=5; break;
case 54 : digi[seq]=6; break;
case 55 : digi[seq]=7; break;
case 56 : digi[seq]=8; break;
case 57 : digi[seq]=9; break;
//case '.': digi[seq]=0; pointPos=seq; break;
}
}
iCoeRec=digi[2]*1000+digi[3]*100+digi[4]*10+digi[5];
if(digi[0]=='r'&&digi[1]=='p') { //steer_kp
if(digi[6]=='w'){
//RUDKP=iCoeRec;
}
display('r','p',RUDKP);
dispflag=OK;
}
if(digi[0]=='r'&&digi[1]=='d') { //steer_kd
if(digi[6]=='w'){
//RUDKD=iCoeRec;
}
display('r','d',RUDKD);
dispflag=OK;
}
if(digi[0]=='v'&&digi[1]=='s') { //speed_kp
if(digi[6]=='w'){
TarSpeed=iCoeRec;
}
display('v','t',TarSpeed);
display('v','n',Speed);
dispflag=OK;
}
if(digi[0]=='r'&&digi[1]=='n') { //speed_kd
if(digi[6]=='w'){
Speed=iCoeRec;
}
display('r','n',RudPWM);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='e') { //speed_fast
if(digi[6]=='w'){
;//speed_fast=iCoeRec;
}
display('s','a',sensorA);
display('s','b',sensorB);
dispflag=OK;
}
if(digi[0]=='c'&&digi[1]=='w') { //speed_middle
if(digi[6]=='w'){
t1=iCoeRec;
}
// for(temp=0;temp<6;temp++)
display('c','1',t1);
display('c','2',t2);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='l') { //speed_slow
if(digi[6]=='w'){
//speed_slow=iCoeRec;
}
display('s','l',LapNum);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='p') { //speed_max
if(digi[6]=='w'){
plsnum=iCoeRec;
}
display('s','p',plsnum);
dispflag=OK;
}
/*if(digi[0]=='v'&&digi[1]=='n') { //speed_min
if(digi[6]=='w'){
speed_min=iCoeRec;
}
display('v','n',speed_min);
dispflag=OK;
}
if(digi[0]=='v'&&digi[1]=='c') { //speed_chg_max
if(digi[6]=='w'){
speed_chg_max=iCoeRec;
}
display('v','c',speed_chg_max);
dispflag=OK;
}
if(digi[0]=='l'&&digi[1]=='s') { //led_state
temp=LedState;
for(i=0;i<16;i++){
ledstate[15-i]=temp%2;
temp=temp/2;
}
for(i=0;i<16;i++){
switch(ledstate[i]){
case 1: leddata=49;break;
case 0: leddata=48;break;}
while(SCI0SR1_TDRE==0);
SCI0DRL=leddata;
}
while(SCI0SR1_TDRE==0); //tab
SCI0DRL=0x0A ;
//display('s','1',steer_kp1);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='1') { //speed_max
if(digi[6]=='w'){
steer_kp1=iCoeRec;
}
display('s','1',steer_kp1);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='2') { //speed_max
if(digi[6]=='w'){
steer_kp2=iCoeRec;
}
display('s','2',steer_kp2);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='g') { //led_state
display('s','g',speed_goal);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='n') { //led_state
display('s','n',speed_now);
dispflag=OK;
}
if(digi[0]=='s'&&digi[1]=='a') { //led_state
display('s','a',steer_angle);
dispflag=OK;
} */
if(digi[0]=='a'&&digi[1]=='l') { //speed_slow
display('r','p',RUDKP);
display('r','d',RUDKD);
display('v','t',TarSpeed);
display('v','n',Speed);
display('s','a',sensorA);
display('s','b',sensorB);
display('s','l',LapNum);
display('s','p',plsnum);
//display('v','s',speed_slow);
//display('v','x',speed_max);
//display('v','n',speed_min);
//display('v','c',speed_chg_max);
dispflag=OK;
}
/* if(digi[0]=='l'&&digi[1]=='n') { //led_state
display('l','n',lapcnt);
dispflag=OK;
} */
if(!dispflag)
{
display('*','*',0);
}
EnableInterrupts;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -