|
1 tag中在static void compute_angle_send_to_anthor0(int distance1, int distance2,int distance3) 根据距离,计算角度- cos = (dis1*dis1 + dis3_constans* dis3_constans - dis2*dis2)/(2*dis1*dis3_constans);
- angle = acos(cos)*180/3.1415926;
- printf("cos = %f, arccos = %f\r\n",cos,angle);
- sprintf(dist_str, "angle: %3.2f m", angle);
复制代码 并且拟合出单个字符
- if(dis1 > 1)
- {
- if(angle > 110)
- {
- printf("turn right\r\n");
- angle_msg[10] = 'R';
- }
- else if(angle < 75)
- {
- printf("turn left\r\n");
- angle_msg[10] = 'L';
- }
- else
- {
- printf("forward\r\n");
- angle_msg[10] = 'F';
- }
- }
- else
- {
- printf("stay here\r\n");
- angle_msg[10] = 'S';
- }
复制代码 最后通过无线发送给基站0
- dwt_writetxdata(sizeof(angle_msg), angle_msg, 0);
- dwt_writetxfctrl(sizeof(angle_msg), 0);
- /* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
- * set by dwt_setrxaftertxdelay() has elapsed. */
- dwt_starttx(DWT_START_TX_IMMEDIATE );
- while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
- { };
复制代码
2 基站收到这个信息发送到串口
- else if (memcmp(rx_buffer, angle_msg, ALL_MSG_COMMON_LEN) == 0 && ANCHOR_IND == 0)
- {
- if(rx_buffer[LOCATION_FLAG_IDX] == 1)//location infomartion
- {
- rx_buffer[ALL_MSG_TAG_IDX] = tag_index;
- USART_puts(&rx_buffer[LOCATION_INFO_START_IDX],rx_buffer[LOCATION_INFO_LEN_IDX]);
- }
- else //follow car
- {
- putchar(rx_buffer[10]);
- }
- }
- }
复制代码
|
|