ROS机器人底盘(8)-PIBOT的Firmware的代码分析(2)

PID

直接贴出代码，应该不难理解
pid.h

#ifndef PIBOT_PID_H_
#define PIBOT_PID_H_

class PID{
    public:
        PID(float* input, float* feedback, float kp, float ki, float kd, unsigned short max_output);
        short compute(float interval);
        void clear();
    private:
        float kp;
        float ki;
        float kd;
        float max_output;
        float* input;
        float* feedback;

        float error;
        float integra;
        float derivative;

        float previous_error;
};

#endif

pid.cpp

#include "pid.h"

#include "board.h"
#include <stdio.h>

PID::PID(float* _input, float* _feedback, float _kp, float _ki, float _kd, unsigned short _max_output)
    :input(_input), feedback(_feedback), kp(_kp), ki(_ki), kd(_kd), max_output(_max_output*1.0){

    clear();

    printf("pid=%ld %ld %ld\r\n", long(kp*1000), long(ki*1000), long(kd*1000));
}

void PID::clear(){
    error = integra = derivative = previous_error =0;
}

short PID::compute(float interval){
    error = *input - *feedback;

    integra = integra + error*interval;
    derivative = (error - previous_error) / interval;

    previous_error = error;
    

    if (ki != 0)
    #ifdef PID_DEBUG_OUTPUT
        printf("integra=%ld max_output=%ld %ld\r\n", long(integra*1000), long(-(max_output/ki*1000)), long(max_output/ki*1000));
    #endif
        if (integra < -(max_output/ki))
        {
            printf("integra clear-\r\n");
            integra = -(max_output/ki);
        }
        if (integra > max_output/ki)
        {
            printf("integra clear+\r\n");
            integra = max_output/ki;
        }

    float val = error*kp + integra*ki + derivative*kd;

    if (val < -max_output)
        val = -max_output+1;
    else if (val > max_output)
        val = max_output-1;

#ifdef PID_DEBUG_OUTPUT
    printf("error=%ld integra=%ld derivative=%ld val=%ld\r\n", long(error*1000), long(integra*1000), long(derivative*1000), long(val*1000));
#endif

    return val;
}
通讯部分

Transport接口

#ifndef PIBOT_TRANSPORT_H_
#define PIBOT_TRANSPORT_H_

class Transport{
    public:
        virtual bool init()=0;
        virtual bool read(unsigned char& ch)=0;
        virtual void write(unsigned char* data, unsigned char len) = 0;

};
#endif

数据报接口

#ifndef PIBOT_DATA_FRAME_H_
#define PIBOT_DATA_FRAME_H_

enum MESSAGE_ID{
    ID_GET_VERSION = 0,
    ID_SET_ROBOT_PARAMTER = 1,
    ID_GET_ROBOT_PARAMTER = 2,
    ID_CLEAR_ODOM = 3,
    ID_SET_VELOCITY = 4,
    ID_GET_ODOM = 5,
    ID_GET_PID_DATA = 6,
    ID_MESSGAE_MAX
};

//消息关注回调接口
class Notify{
    public:
        virtual void update(const MESSAGE_ID id, void* data) = 0;
};

class Dataframe{
    public:
        virtual bool init()=0;
        virtual void register_notify(const MESSAGE_ID id, Notify* _nf)=0;//注册对某个消息的关注
        virtual bool data_recv(unsigned char c)=0;//处理接收数据，返回true 表示接收到完整的数据包
        virtual bool data_parse()=0;//解析包
        virtual bool interact(const MESSAGE_ID id)=0;//master调用用来发送数据、接收响应
};

#endif

bsp接口，基本根据接口名字定义就知道功能，不再赘述

#ifndef PIBOT_BOARD_H_
#define PIBOT_BOARD_H_

class Queue;

enum USART_NUMBER
{
    USART_0 = 0,
    USART_1,
    USART_2,
    USART_3,
    USART_4,
    USART_5,
    USART_6,
};

#define PIN_MODE_INPUT 0x0
#define PIN_MODE_OUTPUT 0x1
#define PIN_MODE_INPUT_PULLUP 0x2

class Board{
  public:
    virtual void enable_irq()=0;
    virtual void disable_irq()=0;
    virtual void usart_debug_init()=0;
    virtual void usart_init(unsigned char num, unsigned long buad)=0;
    virtual Queue* usart_getDataQueue(unsigned char num)=0;

    virtual void usart_write(unsigned char num, unsigned char ch)=0;
    virtual void usart_write(unsigned char num, unsigned char* data, unsigned char len)=0;

    virtual void set_config(unsigned char* data, unsigned short len)=0;
    virtual void get_config(unsigned char* data, unsigned short len)=0;

    virtual void pin_init(unsigned char pin, unsigned char mode)=0;
    virtual void pin_write(unsigned char pin, unsigned char level)=0;
    virtual unsigned char pin_read(unsigned char pin)=0;

    virtual void pwm_init(unsigned char khz)=0;
    virtual void pwm_output(unsigned char pin, unsigned short pwm)=0;

    virtual unsigned long get_tick_count()=0;

    static Board* get();
};

#endif

附上Mega2560的实现board_mega2560

#include "board_mega2560.h"
#include "Arduino.h"
#include "EEPROM.h"

#include "TimerOne.h"
Board_Mega2560 Board_Mega2560::board;

#define CONFIG_EEPROM_BASE 0

int serial_puts(char c, struct __file*){
  Serial3.write(c);
  return c;
}

void printf_begin(void){
  fdevopen(&serial_puts, 0);
}

Board* Board::get(){
    return &Board_Mega2560::board;
}


Board_Mega2560::Board_Mega2560(){
    
}

Board_Mega2560::~Board_Mega2560(){
    
}

void Board_Mega2560::enable_irq(){
  interrupts();
}

void Board_Mega2560::disable_irq(){
  noInterrupts();
}

void Board_Mega2560::usart_debug_init()
{
  usart_init(USART_3, 115200);
  printf_begin();
}

void Board_Mega2560::usart_init(unsigned char num, unsigned long buad){
  if (num == (unsigned char)USART_0){
    printf("uart0 start\r\n");
    Serial.begin(buad);
  }else if (num == (unsigned char)USART_3){
    Serial3.begin(buad);
  }
}


Queue* Board_Mega2560::usart_getDataQueue(unsigned char num){
  if (num == (unsigned char)USART_0){
    return &usart1_queue;
  }

  return 0;
}

void Board_Mega2560::usart_write(unsigned char num, unsigned char ch){
  if (num == (unsigned char)USART_0){
    Serial.write(ch);
  }
}

void Board_Mega2560::usart_write(unsigned char num, unsigned char* data, unsigned char len){
  if (num == (unsigned char)USART_0){
    Serial.write((char*)data, len);
  }else if (num == (unsigned char)USART_3){
    Serial3.write((char*)data, len);
  }
}


void serialEvent(){
  if (Serial.available()){
    if (!Board::get()->usart_getDataQueue(USART_0)->put(Serial.read())){
        //err
    }
  }
}

void Board_Mega2560::set_config(unsigned char* data, unsigned short len){
  for(unsigned short i=0; i<len;i++){
    EEPROM.write(CONFIG_EEPROM_BASE+i, data[i]);
    delayMicroseconds(2);
  }
}

void Board_Mega2560::get_config(unsigned char* data, unsigned short len){
  for(unsigned short i=0; i<len;i++){
    data[i] = EEPROM.read(CONFIG_EEPROM_BASE+i);
    delayMicroseconds(2);
  }
}

void Board_Mega2560::pin_init(unsigned char pin, unsigned char mode){
  pinMode(pin, mode);
}

void Board_Mega2560::pin_write(unsigned char pin, unsigned char level){
  digitalWrite(pin, level);
}

unsigned char Board_Mega2560::pin_read(unsigned char pin){
  return digitalRead(pin);
}

void Board_Mega2560::pwm_init(unsigned char khz){
  Timer1.initialize(1000.0/khz);
}

void Board_Mega2560::pwm_output(unsigned char pin, unsigned short value){
  if (value== 0)
    Timer1.disablePwm(pin);
    Timer1.pwm(pin, value);
}

unsigned long Board_Mega2560::get_tick_count(){
  return millis();
}

下面看下通讯类的具体实现usart_transport，实现了init read write接口
usart_transport.h

#ifndef PIBOT_USART_TRANSPORT_H_
#define PIBOT_USART_TRANSPORT_H_

#include "transport.h"

class USART_transport:public Transport{
  public:
    USART_transport(unsigned char num, unsigned long buad);
    bool init();
    bool read(unsigned char& ch);
    void write(unsigned char* data, unsigned char len);
  private:
    unsigned char usart_num;
    unsigned long usart_buad;
};
#endif

usart_transport.cpp

#include "usart_transport.h"

#include "board.h"
#include <stdio.h>

#include "queue.h"

USART_transport::USART_transport(unsigned char num, unsigned long buad):usart_num(num), usart_buad(buad){

}

bool USART_transport::init()
{
    //printf("port=%d %ld\r\n", usart_num, usart_buad);
    Board::get()->usart_init(usart_num, usart_buad);//这里根据bsp的库来实现串口读写
    return true;
}

bool USART_transport::read(unsigned char& ch){
    return Board::get()->usart_getDataQueue(usart_num)->get(ch);
}

void USART_transport::write(unsigned char* data, unsigned char len){
    Board::get()->usart_write(usart_num, data, len);
}

继续看下dataframe的实现simple_dataframe
simple_dataframe.h定义了simple_dataframe的共用的数据结构

#ifndef PIBOT_SIMPLE_DATAFRAME_H_
#define PIBOT_SIMPLE_DATAFRAME_H_

#include "dataframe.h"
#include <string.h>

static const unsigned short MESSAGE_BUFFER_SIZE = 255;

#define FIX_HEAD 0x5A

struct Head{
    unsigned char flag;// 头部标记,固定值:0X5A
    unsigned char msg_id;// 消息ID,表示消息具体作用,决定消息体具体格式
    unsigned char length;// 消息体长度
};


struct Message{
    struct Head head;
    unsigned char data[MESSAGE_BUFFER_SIZE];
    unsigned char check;
    unsigned char recv_count;//已经接收的字节数

    Message(){}
    Message(unsigned char msg_id, unsigned char* data=0,unsigned char len=0){
        head.flag = FIX_HEAD;
        head.msg_id = msg_id;
        head.length = recv_count = len;
        check = 0;

        if (data != 0 && len !=0)
            memcpy(this->data, data, len);
        
        unsigned char* _send_buffer = (unsigned char*)this;

        unsigned int i = 0;
        for (i = 0; i < sizeof(head)+head.length; i++)
            check += _send_buffer[i];
        
        _send_buffer[sizeof(head)+head.length] = check;
    }
};

enum RECEIVE_STATE{
    STATE_RECV_FIX=0,
    STATE_RECV_ID,
    STATE_RECV_LEN,
    STATE_RECV_DATA,
    STATE_RECV_CHECK
};

#endif

slave的实现

#ifndef PIBOT_SIMPLE_DATAFRAME_SLAVE_H_
#define PIBOT_SIMPLE_DATAFRAME_SLAVE_H_

#include "simple_dataframe.h"

class Transport;
class Simple_dataframe : public Dataframe{
    public:
        Simple_dataframe(Transport* trans=0);
        
        void register_notify(const MESSAGE_ID id, Notify* _nf){
            if (id >= ID_MESSGAE_MAX)
                return;
            nf[id] = _nf;
        }

        bool init();
        bool data_recv(unsigned char c);
        bool data_parse();
        bool interact(const MESSAGE_ID id){return true;};
    private:
        bool send_message(const MESSAGE_ID id);
        bool send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len);
        bool send_message(Message* msg);
    private:
        Notify* nf[ID_MESSGAE_MAX]; 

        Message active_rx_msg;

        RECEIVE_STATE recv_state;
        Transport* trans; 
};
#endif
总结

这里使用的通讯接口及数据接口类使得更换通讯口或者通讯协议变得简单，只按照接口实现transport和dataframe即可替换目前使用的串口及目前简单的通讯协议。同时底层的接口也解耦，更换STM32或者启动的板子也变得极为容易

最后编辑于 ：
©著作权归作者所有,转载或内容合作请联系作者
  • 人面猴
    序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
    沈念sama阅读 159,117评论 4赞 362
  • 死咒
    序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
    沈念sama阅读 67,328评论 1赞 293
  • 救了他两次的神仙让他今天三更去死
    文/潘晓璐 我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
    开封第一讲书人阅读 108,839评论 0赞 243
  • 道士缉凶录：失踪的卖姜人
    文/不坏的土叔 我叫张陵，是天一观的道长。 经常有香客问我，道长，这世上最难降的妖魔是什么？ 我笑而不...
    开封第一讲书人阅读 44,007评论 0赞 206
  • 港岛之恋（遗憾婚礼）
    正文 为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
    茶点故事阅读 52,384评论 3赞 287
  • 恶毒庶女顶嫁案：这布局不是一般人想出来的
    文/花漫 我一把揭开白布。 她就那样静静地躺着，像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上，一...
    开封第一讲书人阅读 40,629评论 1赞 219
  • 城市分裂传说
    那天，我揣着相机与录音，去河边找鬼。 笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。 我是一名探鬼主播，决...
    沈念sama阅读 31,880评论 2赞 313
  • 双鸳鸯连环套：你想象不到人心有多黑
    文/苍兰香墨 我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
    开封第一讲书人阅读 30,593评论 0赞 198
  • 万荣杀人案实录
    序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
    沈念sama阅读 34,313评论 1赞 243
  • 护林员之死
    正文 独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 30,575评论 2赞 246
  • 白月光启示录
    正文 我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 32,066评论 1赞 260
  • 活死人
    序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
    沈念sama阅读 28,392评论 2赞 253
  • 日本核电站爆炸内幕
    正文 年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
    茶点故事阅读 33,052评论 3赞 236
  • 男人毒药：我在死后第九天来索命
    文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 26,082评论 0赞 8
  • 一桩弑父案，背后竟有这般阴谋
    文/苍兰香墨 我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 26,844评论 0赞 195
  • 情欲美人皮
    我被黑心中介骗来泰国打工， 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。 一个月前我还...
    沈念sama阅读 35,662评论 2赞 274
  • 代替公主和亲
    正文 我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
    茶点故事阅读 35,575评论 2赞 270