📄 main.c
字号:
if(errWay1<0)
errWay1 = 0-errWay1;
errWay2=(GuideLine[top+17]+GuideLine[top+18])/2-(GuideLine[top+22]+GuideLine[top+23])/2;
if(errWay2<0)
errWay2 = 0-errWay2;
errWay = errWay1-errWay2;
}
//////////////////////////S Way
if(top==2) {
for(k=top;k<row_max-6;k++){
if(GuideLine[k]-GuideLine[k+6]<0)
s1=1;
if(GuideLine[k]-GuideLine[k+6]>0)
s2=1;
}
}
return 0;
/*for(y=0;y<row_max;y++) {
x=VIDEO_LEFT;
while((*(p+(y*col_max)+x)-*(p+(y*col_max)+x+3)<valve)&&x<(VIDEO_RIGHT-3)) x++;
line_left=x;
x=VIDEO_RIGHT;
while((*(p+(y*col_max)+x)-*(p+(y*col_max)+x-3)<valve)&&x>VIDEO_LEFT+3) x--;
line_right=x;
line_width=line_right-line_left;
if(line_width>1)
GuideLine[y]=(line_right+line_left)/2;
else{
GuideLine[y]=GuideLine[y-1];
err_line++;
}
}*/
//if(err_line>=5) abandon=1;
}
/*********************************************************/
void DealNearLine(byte LineNum){ //黑为0，白为1
byte *p;
byte good=0;
byte L1 = 0;
byte L2 = 0;
byte L3 = 0;
byte flag1 = 0; //第一次白-》黑
byte flag2 = 0; //已检测到一个黑线
byte flag3 = 0; //第二个白-》黑
byte flag6 = 0;
/////////////////
p = ImageReady+LineNum*col_max;
x=0;
for(x=VIDEO_LEFT;x<VIDEO_RIGHT;x++){
if(absl(*(p+x)-*(p+x+1))<8)
continue;
//没有沿就下一次循环
if(((*(p+x)-*(p+x+1))>30)&&(flag6==0)) { //白-》黑
L1 = x+1;
flag1 = 1;
continue;
}
if((flag1==1)&&(*(p+x)-*(p+x+1))<-30) { //黑-》白
if((x-L1)<25&&(x-L1)>7) { ////////////// //黑起跑线宽度
L2 = x+1;
flag2 = 1;
flag6=1;
}
else if((x-L1)<8&&(x-L1)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L1+x)/2 ;
L2 = x+1;
flag2 = 1;
flag6=1;
}
flag1 = 0;
continue;
}
///////////////////////////////////////////
if((flag2==1)&&(*(p+x)-*(p+x+1))>30) { //白-》黑
L3 = x+1;
flag3 = 1;
flag2 = 0;
continue;
}
/////////////////////////////////////////
if((flag3==1)&&(*(p+x)-*(p+x+1))<-30) { //黑-》白
if((x-L3)<8&&(x-L3)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L3+x)/2 ;
startflag=1; feng=LineNum;
break;
}
else if((x-L3)<25&&(x-L3)>7){
startflag=1; feng=LineNum;
break;
}
else{
flag2=1;
}
flag3 = 0;
}
}
//////////////////////////////////////////
if(good==0)
GuideLine[LineNum]=0;
}
/*********************************************************/
void DealFarLine(byte LineNum){ //黑为0，白为1
byte *p;
byte good=0;
byte L1 = 0;
byte L2 = 0;
byte L3 = 0;
byte flag1 = 0; //第一次白-》黑
byte flag2 = 0; //已检测到一个黑线
byte flag3 = 0; //第二个白-》黑
byte flag6 = 0;
/////////////////
p = ImageReady+LineNum*col_max;
x=0;
for(x=VIDEO_LEFT;x<VIDEO_RIGHT;x++){
if(absl(*(p+x)-*(p+x+1))<8)
continue;
//没有沿就下一次循环
if(((*(p+x)-*(p+x+1))>30)&&(flag6==0)) { //白-》黑
L1 = x+1;
flag1 = 1;
continue;
}
if((flag1==1)&&(*(p+x)-*(p+x+1))<-30) {
if((x-L1)<20&&(x-L1)>6) { ////////////// //黑起跑线宽度
L2 = x+1;
flag2 = 1;
flag6=1;
}
else if((x-L1)<7&&(x-L1)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L1+x)/2 ;
L2 = x+1;
flag2 = 1;
flag6=1;
}
flag1 = 0;
continue;
}
///////////////////////////////////////////
if((flag2==1)&&(*(p+x)-*(p+x+1))>30) { //白-》黑
L3 = x+1;
flag3 = 1;
flag2 = 0;
continue;
}
/////////////////////////////////////////
if((flag3==1)&&(*(p+x)-*(p+x+1))<-30) { //黑-》白
if((x-L3)<7&&(x-L3)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L3+x)/2 ;
startflag=1; feng=LineNum;
break;
}
else if((x-L3)<20&&(x-L3)>6){
startflag=1; feng=LineNum;
break;
}
else{
flag2=1;
}
flag3 = 0;
}
}
//////////////////////////////////////////
if(good==0)
GuideLine[LineNum]=0;
}
/*********************************************************/
void Sci_Send(byte onsend){
while(!(SCI0SR1&0X80));
SCI0DRL=onsend;
//Delay(10);
}
/*********************************************************/
void put_char(byte *p,byte n)
{
//register byte lie;
for (lie=0;lie<n;lie++)
{
Sci_Send(*p++);
}
}
/*********************************************************/
void PWM_PID_servo(void){
byte Potion;
abandon=Get_line();
if(abandon==0) {
if(top==2){
if(AngleWay<25)
servoOUT=SERVO_CENTER+curve*5+(curve-last_curve);
else
servoOUT=SERVO_CENTER+curve*7+(curve-last_curve)*4;
}
else if(top<12) {
Potion=KP_w+top;
if(Potion>18)
Potion=18;
servoOUT=SERVO_CENTER+curve*Potion+(curve-last_curve)*KD_w;
}
else {
if (servoOUT<SERVO_CENTER)
servoOUT=SERVO_RIGHT;
if(servoOUT>SERVO_CENTER )
servoOUT=SERVO_LEFT;
}
if (servoOUT>SERVO_LEFT) servoOUT=SERVO_LEFT;
if (servoOUT<SERVO_RIGHT) servoOUT=SERVO_RIGHT;
}
else { //abandan==1 ,图象放弃
if (servoOUT<SERVO_CENTER)
servoOUT=SERVO_RIGHT;
if (servoOUT>SERVO_CENTER)
servoOUT=SERVO_LEFT;
}
//servoOUT=SERVO_CENTER+KP_z*curve+KD_z*(curve-last_curve);
if(servoOUT<SERVO_RIGHT)
servoOUT=SERVO_RIGHT;
if(servoOUT>SERVO_LEFT )
servoOUT=SERVO_LEFT ;
PWMDTY01=servoOUT;
}
/*********************************************************/
void Speed_feedbacl_ctl (void) {
if(abandon==0) {
if(top==2) {
if((AngleWay<4)||(s1==1&&s2==1)){
if(cur_speed<110) //直，可加速
PWMDTY1=90;
if(cur_speed>110){ // /减速
PTP_PTP0=1;
PWMDTY1=50;
}
s1=0;
s2=0;
}
else if(AngleWay<6){ //弯，减速
if(cur_speed>105){
PWMDTY1=10;
}
if(cur_speed<105) //加速
PWMDTY1=90;
}
else if(AngleWay<9){ //弯，减速
if(cur_speed>105){
PTP_PTP0=1;
PWMDTY1=30;
}
if(cur_speed<105) //加速
PWMDTY1=90;
}
else if(AngleWay<13){ //弯，减速
if(cur_speed>90){
PTP_PTP0=1;
PWMDTY1=10 ;
}
if(cur_speed<90) //加速
PWMDTY1=80;
}
else if(AngleWay<18){ //弯，减速
if(cur_speed>85){
PTP_PTP0=1;
PWMDTY1=10 ;
}
if(cur_speed<85) //加速
PWMDTY1=90;
}
else if(AngleWay<25){ //弯，减速
if(cur_speed>80){
PTP_PTP0=1;
PWMDTY1=10 ;
}
if(cur_speed<80) //加速
PWMDTY1=90;
}
else if(AngleWay<30){ //弯，减速
if(cur_speed>80){
PTP_PTP0=1;
PWMDTY1=30;
}
if(cur_speed<80) //加速
PWMDTY1=80;
}
else if(AngleWay<38){ //弯，减速
if(cur_speed>75){
PTP_PTP0=1;
PWMDTY1=20 ;
}
if(cur_speed<75) //加速
PWMDTY1=80;
}
else if(AngleWay<45){ //弯，减速
if(cur_speed>73){
PTP_PTP0=1;
PWMDTY1=10 ;
}
if(cur_speed<73) //加速
PWMDTY1=80;
}
else{ //弯，减速
if(cur_speed>70){
PWMDTY1=10 ;
}
if(cur_speed<70) //加速
PWMDTY1=65;
}
}
else if(top<4){
if(AngleWay<26){ //弯，减速
if(cur_speed>85)
PWMDTY1=0;
if(cur_speed<85) //加速
PWMDTY1=80;
}
else if(AngleWay<50){ //弯，减速
if(cur_speed>80)
PWMDTY1=0;
if(cur_speed<80) //加速
PWMDTY1=80;
}
else{ //弯，减速
PWMDTY1=70;
}
}
else{ //top>=4
PWMDTY1=65;
}
}
else { //Imagegood=0
PWMDTY1=60;
}
}
/*********************************************************/
void send_image (void) {
//register byte hang;
if(PingOrPong==PING){
for(hang=0;hang<row_max;hang++){
put_char(&ImagePong[hang][0],col_max);
}
}
else if(PingOrPong==PONG){
for(hang=0;hang<row_max;hang++){
put_char(&ImagePing[hang][0],col_max);
}
}
}
/*********************************************************/
void Delay(dword num) {
dword int i;
for(i=0;i<num;i++) {asm("nop");}
}
/*********************************************************/
byte absl(signed char number){
if(number>=0) return(number);
else {
return(-number);
}
}
/*********************************************************/
//void Start_Switch(void){
//switch (PORTA&0x0f)
//case 0: break;
//case 1: break;
//}
/*********************************************************\
----------------------main function----------------------
\*********************************************************/
void main (void){
DisableInterrupts;
Overall_ini();
//Start_Switch();
/* put your own code here */
PORTB=0x00;
EnableInterrupts;
Delay(5000);
PORTB=0xFF;
TIE_C1I=1;
for(;;) {
CCD_get();
}/* wait forever */
/* please make sure that you never leave this function */
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -