描述
以下两篇文章将是这一系列最后的两篇，将对全部的代码进行剖析
开发的时间并不多，有些写的并不是最优的实现，请多指教
代码control.h、control.c
这两个文件是主控的核心代码
在control.h头文件中我们定义了一些常量和结构体
在control.c中做了两件事：解析命令buffer，根据磁信息调整左右电机转速control.h
// copyright: @fuxi_robot corporation#ifndef __CONTROL_H__#define __CONTROL_H__#include“main.h”#include<stdio.h>#include<string.h>#define OBSTACLE_DISTANCE_MIN 40#define OBSTACLE_SMOOTH_NUM 10#define MOTOR_SPEED_MAX 5500#define MOTOR_SPEED_MIN 1000#define MOTOR_SPEED_INITIAL 1500#define MOTOR_SPEED_DELTA 400// card_id function#define CARD_ID_STOPPED 90#define CARD_ID_CHARGE 91#define CARD_ID_SPEED_LOW 92#define CARD_ID_SPEED_MIDDLE 93#define CARD_ID_SPEED_HIGH 94#define CARD_ID_SLEEP_15 95#define CARD_ID_SLEEP_30 96#define CARD_ID_SLEEP_60 97#define MAGNETIC_BUFFER_LEN 8#define abs(x)((x)>0?(x):-(x))// robot statusenumRobot_status{ READY =0, BLOCK, RUNNING, DERAILED};// command peopleenumCommand_kind{ RUN =0, STOP, SET_SPEED, SET_TARGET, CHARGE, WALK_FORWARD, WALK_REAR, WALK_LEFT, WALK_RIGHT, QUERY_STATUS, QUERY_SPEED, QUERY_TARGET, QUERY_ALL, COMMAND_ERROR,};enumMotor_state{ MOTOR_ENABLE =0, MOTOR_DISABLE,};structCommand{enumCommand_kind m_command_kind;int m_command_value;};structRobot{enumRobot_status m_robot_status;enumRobot_status m_robot_status_last;int m_is_derailed;int m_change_derailed_first;int m_is_blocked;int m_change_block_first;int m_speed;int m_target;int m_motor_left_value;int m_motor_right_value;int m_direction;int m_magnetic_value;int m_magnetic_status_0_1[MAGNETIC_BUFFER_LEN];int m_obstacle_left_distance;int m_obstacle_right_distance;int m_obstacle_left_distance_memory[OBSTACLE_SMOOTH_NUM];int m_obstacle_right_distance_memory[OBSTACLE_SMOOTH_NUM];int m_obstacle_left_distance_sum;int m_obstacle_right_distance_sum;};int run(structRobot robot,structCommand command);int query(structRobot robot,structCommand command,enumMotor_state motor,int query_ind);structCommand translate_infor_2_command(char info_buffer[256]);structRobot translate_magnetic_2_motor(structRobot robot,int magnetic_value);#endif<
control.c
#include“control.h”int query(structRobot robot,structCommand command,enumMotor_state motor,int query_ind){switch(query_ind){// query statecase0: printf(“< answer >< OK >[ query ][ all ]\n”);if(robot.m_robot_status ==0){ printf(“—– robot – state: READY”);}elseif(robot.m_robot_status ==1){ printf(“—– robot – state: BLOCK”);}elseif(robot.m_robot_status ==2){ printf(“—– robot – state: RUNNING”);}elseif(robot.m_robot_status ==3){ printf(“—– robot – state: DERAILED”);} printf(“, speed: %d , target: %d , left: %d , right: %d , direction:%d\n”, robot.m_speed, robot.m_target, robot.m_motor_left_value, robot.m_motor_right_value, robot.m_direction); printf(“—– robot – magnetic value: %d , status: %d %d %d% d %d %d %d %d\n”,robot.m_magnetic_value,robot.m_magnetic_status_0_1[0],robot.m_magnetic_status_0_1[1],robot.m_magnetic_status_0_1[2],robot.m_magnetic_status_0_1[3],robot.m_magnetic_status_0_1[4],robot.m_magnetic_status_0_1[5],robot.m_magnetic_status_0_1[6],robot.m_magnetic_status_0_1[7]); printf(“—– derailed and blocked – derailed: %d , blocked: %d\n”, robot.m_is_derailed, robot.m_is_blocked); printf(“—– obstacle – left: %d , right: %d\n”, robot.m_obstacle_left_distance, robot.m_obstacle_right_distance); printf(“—– motor – status: %d\n”, motor); printf(“—– command – kind: %d , value: %d\n”, command.m_command_kind, command.m_command_value); printf(“—– last status: %d \n”, robot.m_robot_status_last);break;case1:switch(robot.m_robot_status){case READY: printf(“< answer >< OK >[ query ][ state ]: READY\n”);break;case BLOCK: printf(“< answer >< OK >[ query ][ state ]: BLOCKED\n”);break;case RUNNING: printf(“< answer >< OK >[ query ][ state ]: RUNNING\n”);break;case DERAILED: printf(“< answer >< OK >[ query ][ state ]: DERAILED\n”);break;}break;// query speedcase2: printf(“< answer >< OK >[ query ][ speed ]: %d\n”, robot.m_speed);break;// query targetcase3:if(robot.m_target !=–1){ printf(“< answer >< OK >[ query ][ target ]: %d\n”, robot.m_target);}else{ printf(“< answer >< OK >[ query ][ target ]: no target!\n”);}break;}return0;}int char_2_int(char* source){int number_length = strlen(source);int number_sum =0;int number_positive =1;int k =0;if(source[0]––==0){ number_positive =–1; k =1;}for(int i = k; i < number_length; i++){ number_sum = number_sum *10+(source[i]–0);} number_sum = number_sum * number_positive;// printf(“understand 111\n”);// for (int i = 0 ; i < strlen(source); i++)// {// printf(“%c”, source[i]);// }// printf(“\n”);// return number_sum;}structCommand translate_infor_2_command(char info_buffer[256]){structCommand command_output;int buffer_size =1;int buffer_length =0;for(int i =0; i <256; i ++){if(info_buffer[i]==){ buffer_size =2;}if(info_buffer[i]==;){ buffer_length = i+1;break;}}// printf(“info_buffer 111\n”);// for (int i = 0 ; i < strlen(info_buffer); i++)// {// printf(“%c”, info_buffer[i]);// }// printf(“\n”);// move out ;char* a = info_buffer;charTagName[256]; memset(TagName,0x00,sizeof(TagName)); strncpy(TagName, a, buffer_length–1);char* aa =TagName;// printf(“after 111\n”);// for (int i = 0 ; i < strlen(aa); i++)// {// printf(“%c”, aa[i]);// }// printf(“\n”);char seg[]=” “;char*command_buffer = strtok(aa, seg);if(command_buffer!=NULL){char*second_str = strtok(NULL, seg);// printf(“second 111\n”);// for (int i = 0 ; i < strlen(second_str); i++)// {// printf(“%c”, second_str[i]);// }// printf(“\n”);if(second_str!=NULL){if(strcmp(command_buffer,“speed”)==0){int speed = char_2_int(second_str);if(abs(speed)> abs(MOTOR_SPEED_MAX)|| abs(speed)< abs(MOTOR_SPEED_MIN)){ printf(“< command >< ERROR >: Speed is too high or too slow, error!\n”); command_output.m_command_kind = COMMAND_ERROR; command_output.m_command_value =–1;}else{ printf(“< command >[ set ][ speed ]: %d\n”, speed); command_output.m_command_kind = SET_SPEED; command_output.m_command_value = speed;}}elseif(strcmp(command_buffer,“to”)==0){int target = char_2_int(second_str); printf(“<command>[ set ][ target ]: %d\n”, target); command_output.m_command_kind = SET_TARGET; command_output.m_command_value = target;}elseif(strcmp(command_buffer,“query”)==0){if(strcmp(second_str,“state”)==0){ printf(“< command >[ query ][ state ]\n”); command_output.m_command_kind = QUERY_STATUS; command_output.m_command_value =–1;}elseif(strcmp(second_str,“speed”)==0){ printf(“< command >[ query ][ speed ]\n”); command_output.m_command_kind = QUERY_SPEED; command_output.m_command_value =–1;}elseif(strcmp(second_str,“target”)==0){ printf(“< command >[ query ][ target ]\n”); command_output.m_command_kind = QUERY_TARGET; command_output.m_command_value =–1;}elseif(strcmp(second_str,“all”)==0){ printf(“< command >[ query ][ all ]\n”); command_output.m_command_kind = QUERY_ALL; command_output.m_command_value =–1;}else{ printf(“< command >< ERROR >: Command is error!\n”); command_output.m_command_kind = COMMAND_ERROR; command_output.m_command_value =–1;}}}else{if(strcmp(command_buffer,“start”)==0){ printf(“< command >[ start ]\n”); command_output.m_command_kind = RUN; command_output.m_command_value =–1;}elseif(strcmp(command_buffer,“stop”)==0){ printf(“< command >[ stop ]\n”); command_output.m_command_kind = STOP; command_output.m_command_value =–1;}elseif(strcmp(command_buffer,“charge”)==0){ printf(“< command >[ charge ]\n”); command_output.m_command_kind = CHARGE; command_output.m_command_value = CARD_ID_CHARGE;}elseif(strcmp(command_buffer,“forward”)==0){ printf(“< command >[ walk ][ forward ]\n”); command_output.m_command_kind = WALK_FORWARD; command_output.m_command_value =–1;}elseif(strcmp(command_buffer,“back”)==0){ printf(“< command >[ walk ][ rear ]\n”); command_output.m_command_kind = WALK_REAR; command_output.m_command_value =–1;}elseif(strcmp(command_buffer,“left”)==0){ printf(“< command >[ walk ][ left ]\n”); command_output.m_command_kind = WALK_LEFT; command_output.m_command_value =–1;}elseif(strcmp(command_buffer,“right”)==0){ printf(“< command >[ walk ][ right ]\n”); command_output.m_command_kind = WALK_RIGHT; command_output.m_command_value =–1;}else{ printf(“< command >< ERROR >: Command is error!\n”); command_output.m_command_kind = COMMAND_ERROR; command_output.m_command_value =–1;}}}return command_output;}structRobot translate_magnetic_2_motor(structRobot robot,int magnetic_value){ robot.m_magnetic_value = magnetic_value;int temp = magnetic_value;int i =0;while( i < MAGNETIC_BUFFER_LEN){ robot.m_magnetic_status_0_1[i]=0; robot.m_magnetic_status_0_1[i]= temp%2; temp = temp/2; i++;}int magnetic_value_right = robot.m_magnetic_status_0_1[0]+ robot.m_magnetic_status_0_1[1]+ robot.m_magnetic_status_0_1[2]+robot.m_magnetic_status_0_1[3];int magnetic_value_left = robot.m_magnetic_status_0_1[4]+ robot.m_magnetic_status_0_1[5]+ robot.m_magnetic_status_0_1[6]+robot.m_magnetic_status_0_1[7];int magnetic_diff = magnetic_value_left – magnetic_value_right;if( robot.m_magnetic_status_0_1[7]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[4]==1){ robot.m_motor_left_value =(robot.m_speed + MOTOR_SPEED_DELTA); robot.m_motor_right_value = robot.m_speed;}elseif(robot.m_magnetic_status_0_1[7]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[3]==1){ robot.m_motor_left_value =(robot.m_speed +1.5* MOTOR_SPEED_DELTA); robot.m_motor_right_value = robot.m_speed;}elseif(robot.m_magnetic_status_0_1[7]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[2]==1){ robot.m_motor_left_value =(robot.m_speed +2* MOTOR_SPEED_DELTA); robot.m_motor_right_value = robot.m_speed –2* MOTOR_SPEED_DELTA;}elseif(robot.m_magnetic_status_0_1[7]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[1]==1){ robot.m_motor_left_value =(robot.m_speed +3* MOTOR_SPEED_DELTA); robot.m_motor_right_value = robot.m_speed –3* MOTOR_SPEED_DELTA;}elseif(robot.m_magnetic_status_0_1[7]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[0]==1){ robot.m_motor_left_value =(robot.m_speed +6* MOTOR_SPEED_DELTA); robot.m_motor_right_value = robot.m_speed –6* MOTOR_SPEED_DELTA;}elseif(robot.m_magnetic_status_0_1[0]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[3]==1){ robot.m_motor_left_value = robot.m_speed; robot.m_motor_right_value =(robot.m_speed + MOTOR_SPEED_DELTA);}elseif(robot.m_magnetic_status_0_1[0]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[4]==1){ robot.m_motor_left_value = robot.m_speed; robot.m_motor_right_value =(robot.m_speed +1.5* MOTOR_SPEED_DELTA);}elseif(robot.m_magnetic_status_0_1[0]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[5]==1){ robot.m_motor_left_value = robot.m_speed –2* MOTOR_SPEED_DELTA; robot.m_motor_right_value =(robot.m_speed +2* MOTOR_SPEED_DELTA);}elseif(robot.m_magnetic_status_0_1[0]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[6]==1){ robot.m_motor_left_value = robot.m_speed –3* MOTOR_SPEED_DELTA; robot.m_motor_right_value =(robot.m_speed +3* MOTOR_SPEED_DELTA);}elseif(robot.m_magnetic_status_0_1[0]==0&& robot.m_magnetic_status_0_1[1]==0&& robot.m_magnetic_status_0_1[2]==0&& robot.m_magnetic_status_0_1[3]==0&& robot.m_magnetic_status_0_1[4]==0&& robot.m_magnetic_status_0_1[5]==0&& robot.m_magnetic_status_0_1[6]==0&& robot.m_magnetic_status_0_1[7]==1){ robot.m_motor_left_value = robot.m_speed –6* MOTOR_SPEED_DELTA; robot.m_motor_right_value =(robot.m_speed +6* MOTOR_SPEED_DELTA);}else{ robot.m_motor_left_value = robot.m_speed; robot.m_motor_right_value = robot.m_speed;}if(robot.m_motor_left_value <0){ robot.m_motor_left_value =0;}if(robot.m_motor_right_value <0){ robot.m_motor_right_value =0;}return robot;}<
usart.c
增加两段代码，用于串口通信 /* USER CODE BEGIN 0 */#include“stm32f1xx_hal.h”#include<stdio.h>/* USER CODE END 0 */ /* USER CODE BEGIN 1 */int fputc(int ch,FILE*f){ HAL_UART_Transmit(&huart1,(uint8_t*)&ch,1,0xffff);return ch;}int fgetc(FILE*f){uint8_t ch =0; HAL_UART_Receive(&huart1,&ch,1,0xffff);return ch;}/* USER CODE END 1 */总结
至此我们完成了一个磁条机器人的开发工作。我们也顺利的结束这一专栏吧。
之后我想会写一些关于AMR的知识和算法，敬请期待免责声明：文章内容来自互联网，本站仅提供信息存储空间服务，真实性请自行鉴别，本站不承担任何责任，如有侵权等情况，请与本站联系删除。
转载请注明出处：云恒制造：一起来开发一个磁条机器人（六） https://www.bxbdf.com/a/19695.shtml