📄 main.c
字号:
TIOS=(TIOS|0x03);
TCTL2=(TCTL2&~0x0f)|(TCTL2|0x05);
TIE=(TIE|0x03); // TC10 Interrupt Enable
TFLG1=(TFLG1|0x03);
PACTL=0x40; //16-bit,下降沿触发，use prescaler timer clock
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt 8 void OC0Isr(void){ //定时0.5s
TC0=0;
test_speed=PACN32;
test_speed=test_speed*2+19;
PORTB=~0x01;
//if(kp>=2&&kp<=4)test_speed=test_speed+PACN32;
PACN32=0;
TFLG1=(TFLG1|0x01);
}
#pragma CODE_SEG DEFAULT
/*===============PID========================
unsigned int PID(unsigned int speed1,unsigned int test_speed,unsigned char s1,unsigned char s2)
{
unsigned int x;
unsigned int y;
unsigned int cspeed;
if ( s1==0||s1==11&&s2==0xff) s1= 3;//3;
else if ((s1==0&&s2== 1)||(s1==10&&s2==11)) s1= 4;//4;
else if ( s1==1||s1==10&&s2==0xff) s1= 5;//5;
else if ((s1==1&&s2== 2)||(s1== 9&&s2==10)) s1= 6;//6;
else if ( s1==2||s1== 9&&s2==0xff) s1= 7;//7;
else if ((s1==2&&s2== 3)||(s1== 8&&s2== 9)) s1= 8;//8;
else if ( s1==3||s1== 8&&s2==0xff) s1= 9;//9;
else if ((s1==3&&s2== 4)||(s1== 7&&s2== 8)) s1= 9;//9;
else if ( s1==4||s1== 7&&s2==0xff) s1= 10;//10;
else if ((s1==4&&s2== 5)||(s1== 6&&s2== 7)) s1= 11;//11;
else if ( s1==5||s1== 6 &&s2==0xff) s1= 12;//12;
else if ( s1==5&&s2== 6) s1= 13;//13;
speed1=Set_Speed[s1]*327; //让满值200的pmw和65535对应
cspeed=speed1; //用于判断用
ebb=eb;
eb=e;
e=speed1-test_speed;
x=e-eb; //kp=0.11
x=x>>7; //先整除是为了不溢出，先乘的话，可能溢出(也就是话值大于65535）
x=x*14;
y=e-2*eb+ebb; //kd=0.009
y=y>>10;
y=y*9;
speed1=speed1+x+y;
if((speed1>cspeed&&speed1-cspeed<30) || (speed1<cspeed&&cspeed-speed1<30)){
speed1=cspeed;
}
test_speed=speed1;
speed1=speed1/327;
return speed1;
} */
/*================PID==========================*/
unsigned int PID(unsigned int speed1,unsigned int test_speed,unsigned char s1,unsigned char s2)
{ //kp=0.01,kd=0.005
//kp=0.01,kd=0.01
//kp=0.02,kd=0.005
unsigned int x;
unsigned int y;
/* if ( s1==0||s1==11&&s2==0xff) s1= 3;
else if ((s1==0&&s2== 1)||(s1==10&&s2==11)) s1= 4;
else if ( s1==1||s1==10&&s2==0xff) s1= 5;
else if ((s1==1&&s2== 2)||(s1== 9&&s2==10)) s1= 6;
else if ( s1==2||s1== 9&&s2==0xff) s1= 7;
else if ((s1==2&&s2== 3)||(s1== 8&&s2== 9)) s1= 7;
else if ( s1==3||s1== 8&&s2==0xff) s1= 8;
else if ((s1==3&&s2== 4)||(s1== 7&&s2== 8)) s1= 8;
else if ( s1==4||s1== 7&&s2==0xff) s1= 9;
else if ((s1==4&&s2== 5)||(s1== 6&&s2== 7)) s1= 9;
else if ( s1==5||s1== 6 &&s2==0xff) s1= 10;
else if ( s1==5&&s2== 6) s1= 11;
*/
if ( s1==0||s1==11&&s2==0xff) s1= 5;
else if ((s1==0&&s2== 1)||(s1==10&&s2==11)) s1= 5;
else if ( s1==1||s1==10&&s2==0xff) s1= 6;
else if ((s1==1&&s2== 2)||(s1== 9&&s2==10)) s1= 6;
else if ( s1==2||s1== 9&&s2==0xff) s1= 7;
else if ((s1==2&&s2== 3)||(s1== 8&&s2== 9)) s1= 7;
else if ( s1==3||s1== 8&&s2==0xff) s1= 8;
else if ((s1==3&&s2== 4)||(s1== 7&&s2== 8)) s1= 8;
else if ( s1==4||s1== 7&&s2==0xff) s1= 9;
else if ((s1==4&&s2== 5)||(s1== 6&&s2== 7)) s1= 9;
else if ( s1==5||s1== 6 &&s2==0xff) s1= 10;
else if ( s1==5&&s2== 6) s1= 11;
speed1=Set_Speed[s1]*10;
ebb=eb;
eb=e;
e=speed1-test_speed;
x=(e-eb)*10; //kp=0.01 0.02 0.01
x=x>>10;
y=(e-2*eb+ebb)*5; //kd=0.01 0.005 0.0005
y=y>>10;
speed1=speed1+x+y;
test_speed=speed1;
speed1=speed1/10;
return speed1;
}
/*================PID==========================
unsigned int PID(unsigned int speed1,unsigned int test_speed,unsigned char s1,unsigned char s2)
{ //只用PD调节
//kp= 0.01;
//kd= 0.13;
unsigned int x;
unsigned int y;
//unsigned int cspeed;
if ( s1==0||s1==11&&s2==0xff) s1= 3;
else if ((s1==0&&s2== 1)||(s1==10&&s2==11)) s1= 4;
else if ( s1==1||s1==10&&s2==0xff) s1= 5;
else if ((s1==1&&s2== 2)||(s1== 9&&s2==10)) s1= 6;
else if ( s1==2||s1== 9&&s2==0xff) s1= 6;
else if ((s1==2&&s2== 3)||(s1== 8&&s2== 9)) s1= 7;
else if ( s1==3||s1== 8&&s2==0xff) s1= 7;
else if ((s1==3&&s2== 4)||(s1== 7&&s2== 8)) s1= 8;
else if ( s1==4||s1== 7&&s2==0xff) s1= 8;
else if ((s1==4&&s2== 5)||(s1== 6&&s2== 7)) s1= 9;
else if ( s1==5||s1== 6 &&s2==0xff) s1= 9;
else if ( s1==5&&s2== 6) s1=10;
ebb=eb;
eb=e;
e=Set_Speed[s1]-test_speed;
x=e-eb;
x=x*3;
x=x/256;
y=e-2*eb+ebb;
y=y*17;
y=y/128;
speed1=Set_Speed[s1]+y+x;
//test_speed=speed1;
return speed1;
}
*/
/*=================Steer=======================*/
unsigned int rotate(unsigned char s1,unsigned s2) //有待于改进，只是粗略的处理
{
/*if (s1==11&&s2==0xff) return 1835;//Right_5;}; 142 144
else if(s1==10&&s2==11) return 1830;//Right_5;}; 140 143
else if(s1==10&&s2==0xff) return 1786;//Right_4;} 139 142
else if(s1==9&&s2==10) return 1744;//Right_4;} 137 141
else if(s1==9&&s2==0xff) return 1703;//Right_3; 136 140
else if(s1==8&&s2==9) return 1662;//Right_3; 134 130
else if(s1==8&&s2==0xff) return 1623;//Right_2;}; 133 125
else if(s1==7&&s2==8) return 1585;//Right_2;}; 131 121
else if(s1==7&&s2==0xff) return 1547;//Right_1;}; 130 119
else if(s1==6&&s2==7) return 1511;//Right_1;}; 128 117
else if(s1==6&&s2==0xff) return 1503;
*/if (s1==11&&s2==0xff) return 1835;//Right_5;}; 142 144
else if(s1==10&&s2==11) return 1830;//Right_5;}; 140 143
else if(s1==10&&s2==0xff) return 1793;//Right_4;} 139 142
else if(s1==9&&s2==10) return 1758;//Right_4;} 137 141
else if(s1==9&&s2==0xff) return 1723;//Right_3; 136 140
else if(s1==8&&s2==9) return 1685;//Right_3; 134 130
else if(s1==8&&s2==0xff) return 1640;//Right_2;}; 133 125
else if(s1==7&&s2==8) return 1595;//Right_2;}; 131 121
else if(s1==7&&s2==0xff) return 1552;//Right_1;}; 130 119
else if(s1==6&&s2==7) return 1511;//Right_1;}; 128 117
else if(s1==6&&s2==0xff) return 1503;
else if(s1==5&&s2==6) return 1500;//Middle;}; 127 116
else if(s1==5&&s2==0xff) return 1496;//Middle;}; 127 115
else if(s1==4&&s2==5) return 1487;//Left_1;}; 123 114
else if(s1==4&&s2==0xff) return 1446;//Left_1;}; 121 112
else if(s1==3&&s2==4) return 1406;//Left_2;}; 117 110
else if(s1==3&&s2==0xff) return 1367;//Left_2;}; 115 106
else if(s1==2&&s2==3) return 1330;//Left_3;}; 111 104
else if(s1==2&&s2==0xff) return 1293;//Left_3;}; 109 102
else if(s1==1&&s2==2) return 1258;//Left_4;}; 105 100
else if(s1==1&&s2==0xff) return 1223;//Left_4;}; 103 99
else if(s1==0&&s2==1) return 1190;//Left_5;}; 99 98
else if(s1==0&&s2==0xff) return 1185;//Left_5;}; 97
return Middle;
/*else if(s1==5&&s2==0xff) return 1497;//Middle;}; 127 115
else if(s1==4&&s2==5) return 1489;//Left_1;}; 123 114
else if(s1==4&&s2==0xff) return 1453;//Left_1;}; 121 112
else if(s1==3&&s2==4) return 1419;//Left_2;}; 117 110
else if(s1==3&&s2==0xff) return 1386;//Left_2;}; 115 106
else if(s1==2&&s2==3) return 1353;//Left_3;}; 111 104
else if(s1==2&&s2==0xff) return 1321;//Left_3;}; 109 102
else if(s1==1&&s2==2) return 1290;//Left_4;}; 105 100
else if(s1==1&&s2==0xff) return 1259;//Left_4;}; 103 99
else if(s1==0&&s2==1) return 1230;//Left_5;}; 99 98
else if(s1==0&&s2==0xff) return 1225;//Left_5;}; 97
return Middle;
*/
/* else if(s1==5&&s2==0xff) return 1497;//Middle;}; 127 115
else if(s1==4&&s2==5) return 1489;//Left_1;}; 123 114
else if(s1==4&&s2==0xff) return 1454;//Left_1;}; 121 112
else if(s1==3&&s2==4) return 1420;//Left_2;}; 117 110
else if(s1==3&&s2==0xff) return 1388;//Left_2;}; 115 106
else if(s1==2&&s2==3) return 1356;//Left_3;}; 111 104
else if(s1==2&&s2==0xff) return 1324;//Left_3;}; 109 102
else if(s1==1&&s2==2) return 1294;//Left_4;}; 105 100
else if(s1==1&&s2==0xff) return 1264;//Left_4;}; 103 99
else if(s1==0&&s2==1) return 1235;//Left_5;}; 99 98
else if(s1==0&&s2==0xff) return 1230;//Left_5;}; 97
return Middle; */
}
void main(void) {
interrupt_init();
ADC_init();
PWM_init();
num1=0;
e=0;
eb=50;
ebb=50;
speed=50;
test_speed=50;
state[0]=0xff;
state[1]=0xff;
state[2]=0xff;
//kp=0;
//EnableInterrupts;
PWM_01(70,200);
PWM_23(70,200);
//PWM_45(1190,2000); //middle
for(;;) {
//if (kp==4) {
//PORTB=~((unsigned char)test_speed); //测速度程序
//PWM_01(0,200);
//PWM_23(0,200);
//}
ADC0_12();
num1++;
/*=======================judge================== */
if(num1==1) {
former_sensor=now_sensor;
now_sensor=LED;
if(former_sensor==now_sensor&&(state[1]==5||state[1]==6)) //直线加速
{
steer=Middle;
directcount++;
if(directcount==0xff)directcount=0xfe;
if(directcount>=0x08)
speed=Set_Speed[12];
PWM_01(speed,200);
PWM_23(speed,200);
PWM_45(steer,2000);
outcount=0;
}
else if(now_sensor==0x0fff) //十字交叉路线
{
steer=Middle;
directcount++;
if(directcount==0xff)directcount=0xfe;
if(directcount>=0x08)speed=Set_Speed[12]; //直线加速
PWM_01(speed,200);
PWM_23(speed,200);
PWM_45(steer,2000);
outcount=0;
}
else if(state[0]==0&&state[1]==0xff&&state[2]==0xff) //跑出跑道
{
outcount++;
if(outcount>=0x0a) //冲出去情况还要再考虑
{ //万一有干扰，出问题，回不去
if(steer>=Big_Right) {
steer=Big_Right+5;
}
if(steer<=Big_Left) {
steer=Big_Left-5;
}
speed=Set_Speed[4];//
PWM_01(speed,200);
PWM_23(speed,200);
PWM_45(steer,2000);
}
if(outcount==0xff)outcount=0xfe;
directcount=0x00;
}
else if((state[1]!=0xff&&state[2]!=0xff)||(state[1]!=0xff&&state[2]==0xff))
{ //弯道
steer=rotate(state[1],state[2]);
speed=PID(speed,test_speed,state[1],state[2]);
PWM_01(speed,200);
PWM_23(speed,200);
PWM_45(steer,2000);
outcount=0;
directcount=0x00;
}
if(former_sensor!=now_sensor) state[0]=0;
if(state[0]==0xff) state[0]=state[0]-1;
state[1]=0xff;
state[2]=0xff;
}
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -