一起来开发一个磁条机器人（六）

一起来开发一个磁条机器人（六） MOmo 2023-04-23 09:46:58 789
机器人学

描述

以下两篇文章将是这一系列最后的两篇，将对全部的代码进行剖析

开发的时间并不多，有些写的并不是最优的实现，请多指教

代码

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 status
enum Robot_status
{
    READY = 0,
    BLOCK,
    RUNNING,
    DERAILED
};

// command people
enum Command_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,
};

enum Motor_state
{
    MOTOR_ENABLE = 0,
    MOTOR_DISABLE,
};

struct Command
{
    enum Command_kind m_command_kind;
    int m_command_value;
};

struct Robot
{
    enum Robot_status m_robot_status;
    enum Robot_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(struct Robot robot, struct Command command);

int query(struct Robot robot, struct Command command, enum Motor_state motor, int query_ind);
struct Command translate_infor_2_command(char info_buffer[256]);
struct Robot translate_magnetic_2_motor(struct Robot robot, int magnetic_value);

#endif

control.c

#include "control.h"

int query(struct Robot robot, struct Command command, enum Motor_state motor, int query_ind)
{
    switch (query_ind)
    {
        // query state
        case 0:
            printf("< answer >< OK >[ query ][ all ]\n");
            if (robot.m_robot_status == 0)
            {
                printf("----- robot   - state: READY");
            }
            else if(robot.m_robot_status == 1)
            {
                printf("----- robot   - state: BLOCK");
            }
            else if (robot.m_robot_status == 2)
            {
                printf("----- robot   - state: RUNNING");
            }
            else if (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;
        case 1:
            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 speed
        case 2:
            printf("< answer >< OK >[ query ][ speed ]: %d\n", robot.m_speed);
            break;
        // query target
        case 3:
            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; 
    }
    return 0;
}

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;
}

struct Command translate_infor_2_command(char info_buffer[256])
{
    struct Command 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;
    char TagName[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;
                }    
            }
            else if(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;
            }
            else if(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;
                }
                else if (strcmp(second_str, "speed") == 0)
                {
                    printf("< command >[ query ][ speed ]\n");
                    command_output.m_command_kind = QUERY_SPEED;
                    command_output.m_command_value = -1;
                }
                else if (strcmp(second_str, "target") == 0)
                {
                    printf("< command >[ query ][ target ]\n");
                    command_output.m_command_kind = QUERY_TARGET;
                    command_output.m_command_value = -1;
                }
                else if (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;
            }
            else if(strcmp(command_buffer, "stop") == 0)
            {
                printf("< command >[ stop ]\n");
                command_output.m_command_kind = STOP;
                command_output.m_command_value = -1;
            }
            else if(strcmp(command_buffer, "charge") == 0)
            {
                printf("< command >[ charge ]\n");
                command_output.m_command_kind = CHARGE;
                command_output.m_command_value = CARD_ID_CHARGE;
            }
            else if(strcmp(command_buffer, "forward") == 0)
            {
                printf("< command >[ walk ][ forward ]\n");
                command_output.m_command_kind = WALK_FORWARD;
                command_output.m_command_value = -1;
            }
            else if(strcmp(command_buffer, "back") == 0)
            {
                printf("< command >[ walk ][ rear ]\n");
                command_output.m_command_kind = WALK_REAR;
                command_output.m_command_value = -1;
            }
            else if(strcmp(command_buffer, "left") == 0)
            {
                printf("< command >[ walk ][ left ]\n");
                command_output.m_command_kind = WALK_LEFT;
                command_output.m_command_value = -1;
            }
            else if(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;

}

struct Robot translate_magnetic_2_motor(struct Robot 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;
    }
    else 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] == 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;
    }
    else 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] == 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;
    }
    else 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] == 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;
    }
    else 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] == 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;
    }
    else if (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);
    }
    else if (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);
    }
    else if (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);
    }
    else if (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);
    }
    else if (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的知识和算法，敬请期待

声明：本文内容由易百纳平台入驻作者撰写，文章观点仅代表作者本人，不代表易百纳立场。如有内容侵权或者其他问题，请联系本站进行删除。
MOmo
红包 点赞 收藏 评论 打赏
  • 分享
  • 举报
评论
0个
时间排序
内容存在敏感词
手气红包
    易百纳技术社区暂无数据
相关专栏
关于作者
MOmo

MOmo

暂无个性签名~

原创9
阅读9.6k
收藏0
点赞0
评论0
TA最新文章
打赏用户 共 0 位
我要创作
分享技术经验，可获取创作收益
分类专栏
热门专栏
置顶时间设置
结束时间
删除原因
  • 广告/SPAM
  • 恶意灌水
  • 违规内容
  • 文不对题
  • 重复发帖
打赏作者
易百纳技术社区
MOmo
您的支持将鼓励我继续创作！
打赏金额：
￥1易百纳技术社区
￥5易百纳技术社区
￥10易百纳技术社区
￥50易百纳技术社区
￥100易百纳技术社区
支付方式：
微信支付
支付宝支付
易百纳技术社区微信支付
易百纳技术社区
打赏成功!

感谢您的打赏，如若您也想被打赏，可前往 发表专栏 哦~

举报反馈

举报类型

  • 内容涉黄/赌/毒
  • 内容侵权/抄袭
  • 政治相关
  • 涉嫌广告
  • 侮辱谩骂
  • 其他

详细说明

审核成功

发布时间设置
立即发布
延迟发布
发布时间：
是否关联周任务-专栏模块
是
否

审核失败

失败原因
备注
拼手气红包 红包规则
祝福语
恭喜发财，大吉大利！
红包金额
元
红包最小金额不能低于5元
红包数量
个
红包数量范围10~50个
余额支付
当前余额：
可前往问答、专栏板块获取收益 去获取
取 消 确 定

小包子的红包

恭喜发财，大吉大利

红包规则
开

已领取20/40，共1.6元 红包规则

    易百纳技术社区