con_vec Ralph2(situation &s) { static int firstcall = 1; con_vec result = CON_VEC_EMPTY; // This is what is returned. double ideal_alpha; int segnum, nxtseg, nxtnxtseg; if (firstcall) { // this is the very first call my_name_is("Ralph2"); // this lets everyone know who // we are. firstcall = 0; // theres only one first call return result; // must return an answer } if (s.starting) { track = get_track_description(); } if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc)) return result; getabsxy(s.seg_ID, s.to_lft, s.to_rgt, s.to_end); // point A segnum = s.seg_ID; nxtseg = (segnum+1)%track.NSEG; nxtnxtseg = (nxtseg+1)%track.NSEG; if (s.cur_rad == 0.0) ideal_alpha = angleforcurve(nxtseg, s.v); else { // look for either an s curve or a situation where we have // an s curve with a straight in the middle ideal_alpha = angleforcurve(segnum, s.v); if (curveradiuslow(nxtseg) != 0) { // one curve following another. // now we look to see if we have an s-curve situation if (curveradiuslow(segnum) * curveradiuslow(nxtseg) < 0) { double nextcurvealpha = angleforcurve(nxtseg, s.v); // can we see next curve if ( (curveradiuslow(segnum) > 0 && nextcurvealpha < ideal_alpha) || (curveradiuslow(segnum) < 0 && nextcurvealpha > ideal_alpha) ) ideal_alpha = nextcurvealpha; } } else { // not an s curve, check for opposite direction curves with // an intervening straight if (curveradiuslow(segnum) * curveradiuslow(nxtnxtseg) < 0) { double nextcurvealpha = angleforcurve(nxtnxtseg, s.v); // can we see next curve if ( (curveradiuslow(segnum) > 0 && nextcurvealpha < ideal_alpha) || (curveradiuslow(segnum) < 0 && nextcurvealpha > ideal_alpha) ) ideal_alpha = nextcurvealpha; } } } result.alpha = ideal_alpha - gc_dot; // point B getspeed(s, result); link_alpha_and_vc(s, result); avoidcars2(s, result); if(s.starting) result.fuel_amount = MAX_FUEL; result.request_pit = 0; if (s.stage != QUALIFYING && (s.damage > 25000 || s.fuel < 10)) { result.request_pit = 1; result.repair_amount=s.damage; result.fuel_amount = MAX_FUEL; } return result; }
/* * 函数名:main * 描述 :主函数 * 输入 :无 * 输出 :无 */ int main(void) { uint32_t i, u32RxLen; u16 led0pwmval1=59999; u16 led0pwmval2=59999; u16 led0pwmval3=59999; u16 led0pwmval4=59999; u16 adcx; float temp; double x1,y1,x2,y2,fai; u8 LAT1,LNG1;//定义两个指针指向存放经纬度的内存地址 u8 processok=0;//所有数据每20ms处理一次 double lat,lng; double speed2; // double seitatest=0; static u8 heartbeat=0; const u8 at[] = {"receiveok"}; u8 reply[6]={0x01,0x04,0x01}; u8 gpsdata[100]={0};//存放待处理的GPS数组 u8 reply1[3];//准备存放回应的数据 u8 reply2[3];//存放左右倾角,滚转角 u8 reply3[3];//存放前后倾角,俯仰角 u8 package1[10];//存放纬度 u8 package2[10];//存放经度 u8 package3[10];//存放方位角 u8 package4[10];//存放滚转角 u8 package5[10];//存放前后倾角 u8 package6[4];//存放速度 u8 package7[4];//存放剩余电量 uint8_t source=0x01;//源地址 uint8_t destination=0x02;//目的地址 tsPacketOfAppInfo truesendout;//给这个类型定义一个名字 uint8_t len=61; // u8 gpgga[]={"$GPGGA,061021.05,3102.02726033,N,12126.86794498,E,2,12,0.9,25.022,M,8.127,M,1.0,0000*46\r\n"}; uint8_t sendbuff[]="receiveok"; u32RxLen=sizeof(at); ptuoqiu->latitude=&latitude3; ptuoqiu->longitude=&longitude3; gaosi->gaosix =&gaosix3; gaosi->gaosiy =&gaosiy3; gaosi->gaosix1 =&gaosix4; gaosi->gaosiy1 =&gaosiy4; DK_NVIC_Init(4); SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);//设置systick的时钟是AHB不分频的 SysTick_Config(7200000);//设置两次中断的时间间隔为100ms /* USART1,2,3 ,UART4 config 115200 8-N-1 */ USART1_Config(); USART2_Config(); USART3_Config(); UART4_Config(); Usart_DMA_Config(); LED_GPIO_Config(); delay_ms(1000); PWM_Initch1(60000,0); //不分频。PWM频率=72000000/60000=1.2Khz PWM_Initch2(60000,0); PWM_Initch3(60000,0); PWM_Initch4(60000,0); TIM3->CR1|=0x01; //使能定时器3 LED0_PWM_VAL1=led0pwmval1; LED0_PWM_VAL2=led0pwmval2; LED0_PWM_VAL3=led0pwmval3; LED0_PWM_VAL4=led0pwmval4; //这个是测试用的数据 /*填充将要发送的数据*/ // for (i = 0; i < sizeof(at); i++) // { // SendBuff[i] = at[i]; // } // USART_SEND(SendBuff, sizeof(at)-1); // delay_ms(1000); /*填充将要发送的数据*/ // for (i = 0; i < SENDBUFF_SIZE; i++) // { // SendBuff[i] = 0x31; // } // USART_SEND(SendBuff, 30); // //测试数据结束 // /*串口向 DMA发出请求 */ // USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE); /*在DMA尚未传送完成时,cpu继续执行main函数中的代码*/ /*点亮了LED灯*/ /*而同时DMA在向串口运送数据,当DMA发送完成时,在中断函数关闭LED灯 */ Adc_Init();//ADC转换是分频为12MHz //order_sendinit(); while(1) { // gpggaresolve(gpgga); //Usart1_Send(command1, sizeof(comd1)); //delay_ms(500); // static u8 pathen=0; if (sUsart2_entity.u8RxFlag == 1) {//如果收到罗盘返回的数据 sUsart2_entity.u8RxFlag = 0; u32RxLen = sUsart2_entity.u16RxTotalNum; sUsart2_entity.u16RxTotalNum = 0; processok=1;//50ms到,可以开始处理数据 if(sUsart2_entity.u8RX_Buffer[1]==0x0d)//如果传回的数据是角度 { for(i=0;i<3;i++) { reply1[i]=sUsart2_entity.u8RX_Buffer[10+i];//取出方向角 reply2[i]=sUsart2_entity.u8RX_Buffer[4+i];//取出左右倾角,滚转角 reply3[i]=sUsart2_entity.u8RX_Buffer[7+i];//取出前后倾角,俯仰角 } } memset(sUsart2_entity.u8RX_Buffer,0,15); } if(sUsart3_entity.u8RxFlag==1) {//如果收到GPS传回的数据 sUsart3_entity.u8RxFlag = 0; u32RxLen = sUsart3_entity.u16RxTotalNum; sUsart3_entity.u16RxTotalNum = 0; // processok=1;//20ms到,可以开始处理数据 for(i=0;i<sizeof(sUsart3_entity.u8RX_Buffer);i++) { sUsart3_entity.u8RX_Buffer[i]=gpsdata[i]; } memset(sUsart3_entity.u8RX_Buffer,0,150); // gpggaresolve(sUsart3_entity.u8RX_Buffer); } if (sUart4_entity.u8RxFlag == 1) {//如果收到指令 sUart4_entity.u8RxFlag = 0; u32RxLen = sUart4_entity.u16RxTotalNum; sUart4_entity.u16RxTotalNum = 0; } if(speeden==1)//如果2秒到了的话,测速一次,检测剩余电量一次 { speeden=0; heartbeat++; memset(package6,0,4);//速度和电量的数组先清掉再赋新的值 memset(package7,0,4); speed2=getspeed((*(gaosi->gaosix1)),(*(gaosi->gaosiy1)));//1秒测速一次 // speed2=10.6; // seita=getheading(0.5,-1,1,-2);//检测方位角函数是否正常 // gettoorbit(200,330);//检测大调整函数 // changedirections(1,4,0.5,-1,1,-2);//检测微调是否正常 if(heartbeat==7)//每隔14s检测一次心跳是否正常 { heartbeat=0; hearttest();//检测心跳是否正常函数 } // (*(gaosi->gaosix1))+=0.05; // (*(gaosi->gaosiy1))+=0.05;//测试速度 // if((*(gaosi->gaosix1))>=5) // *(gaosi->gaosix1)=0; // if((*(gaosi->gaosiy1))>=5) // *(gaosi->gaosiy1)=0; // GaussProjCal(*(ptuoqiu->longitude),*(ptuoqiu->latitude),gaosi->gaosix,gaosi->gaosiy); // speed2=getspeed((*(gaosi->gaosix1)),(*(gaosi->gaosiy1)));//1秒测速一次 sprintf(package6,"%4.1f",speed2); for(i=50;i<=53;i++) { package[i]=package6[i-50];//速度数组赋值,准备打包 } temp=powadc();//1秒转换电压一次 sprintf(package7,"%4.1lf",temp); for (i=54;i<=57;i++) { package[i]=package7[i-54];//将电压值赋值到数组的后面 } } if(processok==1)//如果50ms到了的话,那么所有数据进行处理 { memset(package1,0,10);//数据域经纬度,方位角,前后倾角,左右滚转角数组清零 memset(package2,0,10); memset(package3,0,10); memset(package4,0,10); memset(package5,0,10); // memset(package6,0,4);//速度和电量的数组先清掉再赋新的值 // memset(package7,0,4); processok=0;//清零50ms标记 head2=hextodex(reply1);//将角度转换为十进制 roll2=rolltodec(reply2); pitch2=pitchtodec(reply3); gpggaresolve(sUsart3_entity.u8RX_Buffer,&LAT1,&LNG1);//处理GPS的数据 lat=longtodouble(ptuoqiu->LAT); lng=longtodouble(ptuoqiu->LNG); changeformat(lng,lat,ptuoqiu->latitude,ptuoqiu->longitude);//将GPS数据解析成经纬度 order();//判断串口4是否接收到什么指令了 if((pathok==1)&&(heartok==1))//如果允许路径转换的话 { move();//路径转换,大约耗时4ms左右 } sprintf(package1,"%10.6lf",*(ptuoqiu->latitude)); sprintf(package2,"%10.6lf",*(ptuoqiu->longitude)); sprintf(package3,"%10.6lf",head2); sprintf(package4,"%10.6lf",roll2); sprintf(package5,"%10.6lf",pitch2); // GaussProjCal(*(ptuoqiu->longitude),*(ptuoqiu->latitude),gaosi->gaosix,gaosi->gaosiy);//将小车当前的坐标转换为平面坐标 for(i=0;i<=9;i++)//纬度赋值到数据域 { package[i]=package1[i]; } for(i=10;i<=19;i++)//经度赋值到数据域 { package[i]=package2[i-10]; } for(i=20;i<=29;i++)//将方位角赋值到数据域中 { package[i]=package3[i-20]; } for(i=30;i<=39;i++)//将左右倾角赋值到数据域中 { package[i]=package4[i-30]; } for(i=40;i<=49;i++)//将前后倾角赋值到数据域中 { package[i]=package5[i-40]; } memset(sUart4_entity.u8RX_Buffer,0,150); count=SendData_thisprotocol(truesendout,source,destination,len);//所有要打包的数据准备到一个数组里 //返回打包后的数据个数,函数很好,但是用于VB非常麻烦,太麻烦了简直,所以不用了,解析的时候实在是崩溃了 head2=0; roll2=0; pitch2=0; memset(reply1,0,10);//三个角度数组都清零 memset(reply2,0,10); memset(reply3,0,10); // memset(package1,0,10); // memset(package2,0,10); // memset(package3,0,10); // memset(package4,0,10); // memset(package5,0,10); // memset(package6,0,4); // memset(package7,0,4); } } }
/* * mode(n) * 3: save old tty stat, set raw mode with flow control * 2: set XON/XOFF for sb/sz with ZMODEM or YMODEM-g * 1: save old tty stat, set raw mode * 0: restore original tty mode */ int io_mode(int fd, int n) { static int did0 = FALSE; vfile("Setting iode:%d", n); /* fprintf(stderr,"TEST BUILD: ignoring all terminal mode changes"); return 0 ; */ switch(n) { #ifdef USE_TERMIOS case 2: /* Un-raw mode used by sz, sb when -g detected */ if(!did0) { did0 = TRUE; tcgetattr(fd,&oldtty); } tty = oldtty; tty.c_iflag = BRKINT|IXON; tty.c_oflag = 0; /* Transparent output */ tty.c_cflag &= ~PARENB; /* Disable parity */ tty.c_cflag |= CS8; /* Set character size = 8 */ if (Twostop) tty.c_cflag |= CSTOPB; /* Set two stop bits */ #ifdef READCHECK tty.c_lflag = protocol==ZM_ZMODEM ? 0 : ISIG; tty.c_cc[VINTR] = protocol==ZM_ZMODEM ? -1 : 030; /* Interrupt char */ #else tty.c_lflag = 0; tty.c_cc[VINTR] = protocol==ZM_ZMODEM ? 03 : 030; /* Interrupt char */ #endif #ifdef _POSIX_VDISABLE if (((int) _POSIX_VDISABLE)!=(-1)) { tty.c_cc[VQUIT] = _POSIX_VDISABLE; /* Quit char */ } else { tty.c_cc[VQUIT] = -1; /* Quit char */ } #else tty.c_cc[VQUIT] = -1; /* Quit char */ #endif #ifdef NFGVMIN tty.c_cc[VMIN] = 1; #else tty.c_cc[VMIN] = 3; /* This many chars satisfies reads */ #endif tty.c_cc[VTIME] = 1; /* or in this many tenths of seconds */ tcsetattr(fd,TCSADRAIN,&tty); return OK; case 1: case 3: if(!did0) { did0 = TRUE; tcgetattr(fd,&oldtty); } tty = oldtty; tty.c_iflag = IGNBRK; if (n==3) /* with flow control */ tty.c_iflag |= IXOFF; /* No echo, crlf mapping, INTR, QUIT, delays, no erase/kill */ tty.c_lflag &= ~(ECHO | ICANON | ISIG); tty.c_oflag = 0; /* Transparent output */ tty.c_cflag &= ~(PARENB); /* Same baud rate, disable parity */ /* Set character size = 8 */ tty.c_cflag &= ~(CSIZE); tty.c_cflag |= CS8; if (Twostop) tty.c_cflag |= CSTOPB; /* Set two stop bits */ #ifdef NFGVMIN tty.c_cc[VMIN] = 1; /* This many chars satisfies reads */ #else tty.c_cc[VMIN] = HOWMANY; /* This many chars satisfies reads */ #endif tty.c_cc[VTIME] = 1; /* or in this many tenths of seconds */ tcsetattr(fd,TCSADRAIN,&tty); Baudrate = getspeed(cfgetospeed(&tty)); return OK; case 0: if(!did0) return ERROR; tcdrain (fd); /* wait until everything is sent */ tcflush (fd,TCIOFLUSH); /* flush input queue */ tcsetattr (fd,TCSADRAIN,&oldtty); tcflow (fd,TCOON); /* restart output */ return OK; #endif #ifdef USE_TERMIO case 2: /* Un-raw mode used by sz, sb when -g detected */ if(!did0) (void) ioctl(fd, TCGETA, &oldtty); tty = oldtty; tty.c_iflag = BRKINT|IXON; tty.c_oflag = 0; /* Transparent output */ tty.c_cflag &= ~PARENB; /* Disable parity */ tty.c_cflag |= CS8; /* Set character size = 8 */ if (Twostop) tty.c_cflag |= CSTOPB; /* Set two stop bits */ #ifdef READCHECK tty.c_lflag = protocol==ZM_ZMODEM ? 0 : ISIG; tty.c_cc[VINTR] = protocol==ZM_ZMODEM ? -1 : 030; /* Interrupt char */ #else tty.c_lflag = 0; tty.c_cc[VINTR] = protocol==ZM_ZMODEM ? 03 : 030; /* Interrupt char */ #endif tty.c_cc[VQUIT] = -1; /* Quit char */ #ifdef NFGVMIN tty.c_cc[VMIN] = 1; #else tty.c_cc[VMIN] = 3; /* This many chars satisfies reads */ #endif tty.c_cc[VTIME] = 1; /* or in this many tenths of seconds */ (void) ioctl(fd, TCSETAW, &tty); did0 = TRUE; return OK; case 1: case 3: if(!did0) (void) ioctl(fd, TCGETA, &oldtty); tty = oldtty; tty.c_iflag = n==3 ? (IGNBRK|IXOFF) : IGNBRK; /* No echo, crlf mapping, delays, no erase/kill */ tty.c_lflag &= ~(ECHO | ICANON | ISIG); tty.c_oflag = 0; /* Transparent output */ tty.c_cflag &= ~PARENB; /* Same baud rate, disable parity */ tty.c_cflag |= CS8; /* Set character size = 8 */ if (Twostop) tty.c_cflag |= CSTOPB; /* Set two stop bits */ #ifdef NFGVMIN tty.c_cc[VMIN] = 1; /* This many chars satisfies reads */ #else tty.c_cc[VMIN] = HOWMANY; /* This many chars satisfies reads */ #endif tty.c_cc[VTIME] = 1; /* or in this many tenths of seconds */ (void) ioctl(fd, TCSETAW, &tty); did0 = TRUE; Baudrate = getspeed(tty.c_cflag & CBAUD); return OK; case 0: if(!did0) return ERROR; (void) ioctl(fd, TCSBRK, 1); /* Wait for output to drain */ (void) ioctl(fd, TCFLSH, 0); /* Flush input queue */ (void) ioctl(fd, TCSETAW, &oldtty); /* Restore modes */ (void) ioctl(fd, TCXONC,1); /* Restart output */ return OK; #endif #ifdef USE_SGTTY /* * NOTE: this should transmit all 8 bits and at the same time * respond to XOFF/XON flow control. If no FIONREAD or other * READCHECK alternative, also must respond to INTRRUPT char * This doesn't work with V7. It should work with LLITOUT, * but LLITOUT was broken on the machine I tried it on. */ case 2: /* Un-raw mode used by sz, sb when -g detected */ if(!did0) { ioctl(fd, TIOCEXCL, 0); ioctl(fd, TIOCGETP, &oldtty); ioctl(fd, TIOCGETC, &oldtch); #ifdef LLITOUT ioctl(fd, TIOCLGET, &Locmode); #endif } tty = oldtty; tch = oldtch; #ifdef READCHECK tch.t_intrc = Zmodem ? -1:030; /* Interrupt char */ #else tch.t_intrc = Zmodem ? 03:030; /* Interrupt char */ #endif tty.sg_flags |= (ODDP|EVENP|CBREAK); tty.sg_flags &= ~(ALLDELAY|CRMOD|ECHO|LCASE); ioctl(fd, TIOCSETP, &tty); ioctl(fd, TIOCSETC, &tch); #ifdef LLITOUT ioctl(fd, TIOCLBIS, &Locbit); #else bibi(99); /* un-raw doesn't work w/o lit out */ #endif did0 = TRUE; return OK; case 1: case 3: if(!did0) { ioctl(fd, TIOCEXCL, 0); ioctl(fd, TIOCGETP, &oldtty); ioctl(fd, TIOCGETC, &oldtch); #ifdef LLITOUT ioctl(fd, TIOCLGET, &Locmode); #endif } tty = oldtty; tty.sg_flags |= RAW; tty.sg_flags &= ~ECHO; ioctl(fd, TIOCSETP, &tty); did0 = TRUE; Baudrate = getspeed(tty.sg_ospeed); return OK; case 0: if(!did0) return ERROR; ioctl(fd, TIOCSETP, &oldtty); ioctl(fd, TIOCSETC, &oldtch); ioctl(fd, TIOCNXCL, 0); #ifdef LLITOUT ioctl(fd, TIOCLSET, &Locmode); #endif #ifdef TIOCFLUSH { int x=1; ioctl(fd,TIOCFLUSH,&x); } #endif #endif return OK; default: return ERROR; } }
void loop() { speed = getspeed(); delay(1000); }
static void set(char *argv[], struct termios *sp) { const Tty_t *tp; register int c,off; char *cp; char *ep; while(cp = *argv++) { off = 0; if(*cp=='-') { cp++; off=1; } if(!(tp=lookup(cp)) || (off && (tp->type!=BIT) && (tp->type!=TABS))) error(ERROR_exit(1),"%s: unknown mode",cp); switch(tp->type) { case CHAR: if(off) error(ERROR_exit(1),"%s: unknown mode",cp); if(!*argv) error(ERROR_exit(1),"missing argument to %s",cp); c = gettchar(*argv++); if(c>=0) sp->c_cc[tp->mask] = c; else sp->c_cc[tp->mask] = _POSIX_VDISABLE; break; case BIT: case BITS: switch(tp->field) { case C_FLAG: if(off) sp->c_cflag &= ~tp->mask; else sp->c_cflag |= tp->mask; break; case I_FLAG: if(off) sp->c_iflag &= ~tp->mask; else sp->c_iflag |= tp->mask; break; case O_FLAG: sp->c_oflag &= ~tp->mask; sp->c_oflag |= tp->val; break; case L_FLAG: if(off) sp->c_lflag &= ~tp->mask; else sp->c_lflag |= tp->mask; break; } break; case TABS: sp->c_oflag &= ~tp->mask; if(off) sp->c_oflag |= tp->val; break; #ifdef TIOCSWINSZ case WIND: { struct winsize win; int n; if(ioctl(0,TIOCGWINSZ,&win)<0) error(ERROR_system(1),"cannot set %s",tp->name); if(!(cp= *argv)) { sfprintf(sfstdout,"%d\n",tp->mask?win.ws_col:win.ws_row); break; } argv++; n=strtol(cp,&cp,10); if(*cp) error(ERROR_system(1),"%d: invalid number of %s",argv[-1],tp->name); if(tp->mask) win.ws_col = n; else win.ws_row = n; if(ioctl(0,TIOCSWINSZ,&win)<0) error(ERROR_system(1),"cannot set %s",tp->name); break; } #endif case NUM: cp = *argv; if (!cp) { if (tp->field == C_SPEED) { if (tp = getspeed(*tp->name == 'i' ? cfgetispeed(sp) : cfgetospeed(sp))) sfprintf(sfstdout, "%s\n", tp->name); break; } error(ERROR_exit(1), "%s: missing numeric argument", tp->name); } argv++; c = (int)strtol(cp, &ep, 10); if (*ep) error(ERROR_exit(1), "%s: %s: numeric argument expected", tp->name, cp); switch (tp->field) { #if _mem_c_line_termios case C_LINE: sp->c_line = c; break; #endif case C_SPEED: if(getspeed(c)) { if (*tp->name != 'o') cfsetispeed(sp, c); if (*tp->name != 'i') cfsetospeed(sp, c); } else error(ERROR_exit(1), "%s: %s: invalid speed", tp->name, cp); break; case T_CHAR: sp->c_cc[tp->mask] = c; break; } break; case SPEED: cfsetospeed(sp, tp->mask); cfsetispeed(sp, tp->mask); break; case SIZE: sp->c_cflag &= ~CSIZE; sp->c_cflag |= tp->mask; break; case SANE: sane(sp); break; #if defined(OLCUC) && defined(IUCLC) case CASE: if(off) { sp->c_iflag |= IUCLC; sp->c_oflag |= OLCUC; } else { sp->c_iflag &= ~IUCLC; sp->c_oflag &= ~OLCUC; } break; #endif /* OLCUC && IUCLC */ } } }