ROS机器人小车底盘DIY有何难？不枉做一个程序猿，躯壳码上有功能
arduino stm32F103c8t6系统板的编程任务：
驱动TB6612
驱动GY85
驱动ps2遥控接收器
编码器的检测
PID速位控制
运动学算法
rosserial通讯协议
arduino好处是库比较多，上手快捷。
可以找一些开源的资料，按需整合，移植，调试。
但是有些还会遇到困难，需要自己创作。
问题：
比如编码器的库，可能找了很多都不太合适，需要改成中断的方案。
stm32使用arduino encoder库的改造草案https://blog.csdn.net/qq_38288618/article/details/106839319
比如rosserial的通讯协议探索。
ROS中rosserial通讯协议初探https://blog.csdn.net/qq_38288618/article/details/102931684
需要用ros配合来生成arduino用的库遇到问题。
ROS与Arduino硬件之rosserial_arduino（win10）https://blog.csdn.net/qq_38288618/article/details/104082877
等等吧，相信花点时间都能攻克。
上主文件的码：
#define USE_STM32_HW_SERIAL
#include <wirish_debug.h>
//#include <ros.h>
#include "zr_ros.h"
//header file for publishing velocities for odom
#include <zr_movebase_msgs/Velocities.h>
//header file for cmd_subscribing to "cmd_vel"
#include <geometry_msgs/Twist.h>
//header file for pid server
#include <zr_movebase_msgs/PID.h>
//header files for imu
#include <ros_arduino_msgs/RawImu.h>
#include <geometry_msgs/Vector3.h>
#include <ros/time.h>
//#include <zr_protocol/hw_info.h>
#include <zr_protocol/hw_cmd_.h>
#include "zr_info.h"
#include "config.h"
#include "Motor.h"
#include "PID.h"
#include "Kinematics.h"
#include <Wire.h>
#include "imu_configuration.h"
#include <PS2X_lib.h> //for v1.6
#include "PS2X_zzz.h"
PS2X_ZZZ PS2X_ez(_PS2_CLK, _PS2_CMD, _PS2_SEL, _PS2_DAT, true, true);
//Kinematics kinematics(MAX_RPM, WHEEL_DIAMETER, BASE_WIDTH, PWM_BITS);
Kinematics kinematics(Kinematics::ZR_BASE, MAX_RPM, WHEEL_DIAMETER, FR_WHEELS_DISTANCE, LR_WHEELS_DISTANCE, PWM_BITS);
//Motor motor1(MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); //front
Motor motor1(MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); //front
Motor motor2(MOTOR2_PWM, MOTOR2_IN_A, MOTOR2_IN_B); //front
Motor motor3(MOTOR3_PWM, MOTOR3_IN_A, MOTOR3_IN_B); //back
Motor motor4(MOTOR4_PWM, MOTOR4_IN_A, MOTOR4_IN_B); //back
//COUNTS_PER_REV = 0 if no encoder
int Motor::counts_per_rev_ = COUNTS_PER_REV;
PID motor1_pid(-255, 255, K_P, K_I, K_D);
PID motor2_pid(-255, 255, K_P, K_I, K_D);
PID motor3_pid(-255, 255, K_P, K_I, K_D);
PID motor4_pid(-255, 255, K_P, K_I, K_D);
//zzz ros ----------------------------------------------------------------
double g_req_angular_vel_z = 0;
double g_req_linear_vel_x = 0;
double g_req_linear_vel_y = 0;
unsigned char led_blink=0;
unsigned char ros_msg_ctrl=1;
unsigned char stick_wait_ok=0;
unsigned char stick_setup_ok=1;
unsigned long g_prev_loop_first_time = 0;
unsigned long g_prev_stick_ctrl_time = 0;
unsigned long g_prev_stick_time = 0;
unsigned long g_prev_imu_check_time = 0;
unsigned long g_prev_led_blink_time = 0;
unsigned long g_prev_command_time = 0;
unsigned long g_prev_control_time = 0;
unsigned long g_publish_vel_time = 0;
unsigned long g_prev_imu_time = 0;
unsigned long g_prev_debug_time = 0;
void init_g_prev_time(unsigned long t);
bool g_is_first = true;
char g_buffer[50];
//callback function prototypes
void commandCallback(const geometry_msgs::Twist& cmd_msg);
void PIDCallback(const zr_movebase_msgs::PID& pid);
ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
ros::Subscriber<zr_movebase_msgs::PID> pid_sub("pid", PIDCallback);
ros_arduino_msgs::RawImu raw_imu_msg;
ros::Publisher raw_imu_pub("raw_imu", &raw_imu_msg);
zr_movebase_msgs::Velocities raw_vel_msg;
ros::Publisher raw_vel_pub("raw_vel", &raw_vel_msg);
//zzz ros-------------------------------------------------------------------
#define IO_REG_TYPE uint32_t
#define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin)))
#define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin))
#define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0)
typedef struct {
volatile IO_REG_TYPE * pin1_register;
volatile IO_REG_TYPE * pin2_register;
IO_REG_TYPE pin1_bitmask;
IO_REG_TYPE pin2_bitmask;
uint8_t pin1;
uint8_t pin2;
uint8_t state;
int32_t position;
} Encoder_internal_state_t;
Encoder_internal_state_t * interruptArgs[4];
Encoder_internal_state_t encoder1;
Encoder_internal_state_t encoder2;
Encoder_internal_state_t encoder3;
Encoder_internal_state_t encoder4;
uint8_t oldstate=0;
float stick_a=0.0;
float stick_v=0.0;
float stick_vy=0.0;
void stick_callback(PS2X_ZZZ::PSS stick_dat){
//
if(stick_wait_ok){
int d=stick_dat.lx-128;
int d_abs;
int d_f;
d_f=d<0?-1:1;
d_abs=abs(d);
d=d_abs<2?0:d;
d_f=d==0?0:d_f;
stick_a=((float)d-2.0*d_f)/126.0;
d=stick_dat.ry-128;
d_f=d<0?-1:1;
d_abs=abs(d);
d=d_abs<2?0:d;
d_f=d==0?0:d_f;
stick_v=-((float)d-2.0*d_f)/126.0;//add"-" zzz 6612 is not same as 8833
//!!!!MECANUM add y director//
d=stick_dat.rx-128;
d_f=d<0?-1:1;
d_abs=abs(d);
d=d_abs<2?0:d;
d_f=d==0?0:d_f;
stick_vy=((float)d-2.0*d_f)/126.0;
//Serial.println(stick_dat.lx,DEC);
ros_msg_ctrl=0;
//stick yaokong youxian
g_req_linear_vel_x=stick_v*0.4;//0.1;//0.4mps
g_req_linear_vel_y=stick_vy*0.4;//0.1;//0.4mps
g_req_angular_vel_z=stick_a*3.14/2;//45du/s
g_prev_stick_ctrl_time=millis();
g_prev_command_time =millis();
}
}
void update1(Encoder_internal_state_t *arg) {
//uint8_t state = arg->state & 3;
//if (digitalRead(arg->pin1)) state |= 4;
//if (digitalRead(arg->pin2)) state |= 8;
uint8_t p1val = DIRECT_PIN_READ(arg->pin1_register, arg->pin1_bitmask);
uint8_t p2val = DIRECT_PIN_READ(arg->pin2_register, arg->pin2_bitmask);
uint8_t state = arg->state & 3;
if (p1val) state |= 4;
if (p2val) state |= 8;
arg->state = (state >> 2);
switch (state) {
case 1: case 7: case 8: case 14:
arg->position++;
return;
case 2: case 4: case 11: case 13:
arg->position--;
return;
case 3: case 12:
arg->position += 2;
return;
case 6: case 9:
arg->position -= 2;
return;
}
}
void update(void) {
//noInterrupts();
update1(interruptArgs[0]);
update1(interruptArgs[1]);
update1(interruptArgs[2]);
update1(interruptArgs[3]);
//interrupts();
}
//void callback_srv(const zr_protocol::hw_info::Request & req, zr_protocol::hw_info::Response & res){
// res.output=RosDeviceInfo::getInfoValue(req.input);
//}
//ros::ServiceServer<zr_protocol::hw_info::Request, zr_protocol::hw_info::Response> server("zr_hw_info",&callback_srv);
void callback_hw_cmd(const zr_protocol::hw_cmd::Request & req, zr_protocol::hw_cmd::Response & res) {
if (strcmp(req.cmd, "chr") == 0) {
uint8_t dat[8] ;
char i=1;
char maxi=req.input_length>9?9:req.input_length;
while( i<req.input_length){dat[i-1]=req.input[i];i++;}
i--;
while( i<8){dat[i]=0;i++;}
//lcd.createChar(req.input[0], dat);
res.output = ( unsigned char*) "ok";
res.output_length=2;
} else {
res.output = ( unsigned char*) RosDeviceInfo::getInfoValue(req.cmd);
res.output_length=zr::buffStrLen((char*) res.output);
}
}
ros::ServiceServer<zr_protocol::hw_cmd::Request, zr_protocol::hw_cmd::Response> server_hw_cmd("zr_hw_cmd", &callback_hw_cmd);
void ROS_setup()
{
nh.initNode();
ArduinoHardware* hw = nh.getHardware();
hw->setBaud(500000);
hw->init();
//Serial.begin(500000);
nh.advertiseService(server_hw_cmd);
nh.advertise(raw_imu_pub);
nh.advertise(raw_vel_pub);
nh.subscribe(pid_sub);
nh.subscribe(cmd_sub);
}
void init_encoder_pin_data(uint8_t pin1,uint8_t pin2,Encoder_internal_state_t *encoder,uint8_t idx){
encoder->pin1 = pin1;
encoder->pin2 = pin2;
encoder->pin1_register = PIN_TO_BASEREG(pin1);
encoder->pin1_bitmask = PIN_TO_BITMASK(pin1);
encoder->pin2_register = PIN_TO_BASEREG(pin2);
encoder->pin2_bitmask = PIN_TO_BITMASK(pin2);
//encoder->state=0;
encoder->position=0;
pinMode(pin1, INPUT);
//digitalWrite(pin1, HIGH);
//pinMode(pin1, INPUT_PULLUP);
digitalWrite(pin1, LOW);
pinMode(pin1, INPUT_PULLDOWN);
pinMode(pin2, INPUT);
//digitalWrite(pin2, HIGH);
//pinMode(pin2, INPUT_PULLUP);
digitalWrite(pin2, LOW);
pinMode(pin2, INPUT_PULLDOWN);
interruptArgs[idx]=encoder;
//
delayMicroseconds(2000);
uint8_t s = 0;
if (DIRECT_PIN_READ(encoder->pin1_register, encoder->pin1_bitmask)) s |= 1;
if (DIRECT_PIN_READ(encoder->pin2_register, encoder->pin2_bitmask)) s |= 2;
encoder->state = s;
}
Kinematics::outputf req_rpm;
int spd1,spd2,spd3,spd4;
float set1,set2,set3,set4;
float cur1,cur2,cur3,cur4;
float pid_p,pid_i,pid_d;
void moveBase()
{
//get the required rpm for each motor based on required velocities
req_rpm = kinematics.getRPM(g_req_linear_vel_x, g_req_linear_vel_y, g_req_angular_vel_z);
//the required rpm is capped at -/+ MAX_RPM to prevent the PID from having too much error
//the PWM value sent to the motor driver is the calculated PID based on required RPM vs measured RPM
set1=req_rpm.motor1;//(float) constrain(req_rpm.motor1, -MAX_RPM, MAX_RPM);
cur1=motor1.rpm;
spd1=motor1_pid.compute(set1, cur1);
motor1.spin(spd1);
set3=req_rpm.motor3;//(float) constrain(req_rpm.motor3, -MAX_RPM, MAX_RPM);
cur3=motor3.rpm;
spd3=motor3_pid.compute(set3, cur3);
motor3.spin(spd3);
set2=-req_rpm.motor2;//(float) constrain(-req_rpm.motor2, -MAX_RPM, MAX_RPM);
cur2=motor2.rpm;
spd2=motor2_pid.compute(set2, cur2);
motor2.spin(spd2);
set4=-req_rpm.motor4;//(float) constrain(-req_rpm.motor4, -MAX_RPM, MAX_RPM);
cur4=motor4.rpm;
spd4=motor4_pid.compute(set4, cur4);
motor4.spin(spd4);
}
void stopBase()
{
g_req_linear_vel_x = 0.0;
g_req_linear_vel_y = 0.0;
g_req_angular_vel_z = 0.0;
}
void publishVelocities()
{
//update the current speed of each motor based on encoder's count
// motor1.updateSpeed(encoder1.position);
// motor2.updateSpeed(encoder2.position);
// motor3.updateSpeed(encoder3.position);
// motor4.updateSpeed(encoder4.position);
Kinematics::velocities vel;
vel = kinematics.getVelocities(motor1.rpm, -motor2.rpm, motor3.rpm, -motor4.rpm);
//fill in the object
raw_vel_msg.linear_x = vel.linear_x;
raw_vel_msg.linear_y = vel.linear_y;
raw_vel_msg.angular_z = vel.angular_z;
//
//Serial.println("encoder :");
//Serial.print(encoder1.position);Serial.print(" , ");
//Serial.print(encoder2.position);Serial.print(" , ");
//Serial.print(encoder3.position);Serial.print(" , ");
//Serial.println(encoder4.position);
//Serial.println("publishVelocities");
//Serial.print("linear_x :");Serial.println(vel.linear_x);
//Serial.print("linear_y :0");
//Serial.print("angular_z :");Serial.println(vel.angular_z);
//publish raw_vel_msg object to ROS
raw_vel_pub.publish(&raw_vel_msg);
}
void checkIMU_zzz()
{
//this function checks if IMU is present
Serial.println("Accelerometer");
raw_imu_msg.accelerometer = checkAccelerometer();
raw_imu_msg.accelerometer = checkAccelerometer();
raw_imu_msg.accelerometer = checkAccelerometer();
Serial.println("gyroscope");
raw_imu_msg.gyroscope = checkGyroscope();
Serial.println("magnetometer");
raw_imu_msg.magnetometer = checkMagnetometer();
Serial.println(raw_imu_msg.accelerometer);
Serial.println(raw_imu_msg.gyroscope);
Serial.println(raw_imu_msg.magnetometer);
if (!raw_imu_msg.accelerometer)
{
Serial.println("Accelerometer NOT FOUND!");
}
if (!raw_imu_msg.gyroscope)
{
Serial.println("Gyroscope NOT FOUND!");
}
if (!raw_imu_msg.magnetometer)
{
Serial.println("Magnetometer NOT FOUND!");
}
g_is_first = false;
}
void publishIMU_zzz()
{
if (raw_imu_msg.accelerometer || raw_imu_msg.gyroscope || raw_imu_msg.magnetometer){
//this function publishes raw IMU reading
raw_imu_msg.header.stamp = nh.now();
raw_imu_msg.header.frame_id = "imu_link";
//measure accelerometer
if (raw_imu_msg.accelerometer){
measureAcceleration();
raw_imu_msg.raw_linear_acceleration = raw_acceleration;
}
//measure gyroscope
if (raw_imu_msg.gyroscope){
measureGyroscope();
raw_imu_msg.raw_angular_velocity = raw_rotation;
}
//measure magnetometer
if (raw_imu_msg.magnetometer){
measureMagnetometer();
raw_imu_msg.raw_magnetic_field = raw_magnetic_field;
}
//publish raw_imu_msg object to ROS
// Serial.println("publish raw_imu_msg------");
// Serial.println("raw_acceleration");
// Serial.print(raw_acceleration.x);Serial.print(" , ");
// Serial.print(raw_acceleration.y);Serial.print(" , ");
// Serial.println(raw_acceleration.z);
// Serial.println("raw_rotation");
// Serial.print(raw_rotation.x);Serial.print(" , ");
// Serial.print(raw_rotation.y);Serial.print(" , ");
// Serial.println(raw_rotation.z);
// Serial.println("raw_magnetic_field");
// Serial.print(raw_magnetic_field.x);Serial.print(" , ");
// Serial.print(raw_magnetic_field.y);Serial.print(" , ");
// Serial.println(raw_magnetic_field.z);
raw_imu_pub.publish(&raw_imu_msg);
}
}
void printDebug()
{
sprintf (g_buffer, "Encoder FrontLeft: %ld", encoder1.position);
nh.loginfo(g_buffer);
sprintf (g_buffer, "Encoder RearLeft: %ld", encoder3.position);
nh.loginfo(g_buffer);
sprintf (g_buffer, "Encoder FrontRight: %ld", encoder2.position);
nh.loginfo(g_buffer);
sprintf (g_buffer, "Encoder RearRight: %ld", encoder4.position);
nh.loginfo(g_buffer);
}
void printDebug(float dat1,float dat2,float dat3,float dat4)
{
sprintf (g_buffer, "dat1: %lf", dat1);
nh.loginfo(g_buffer);
sprintf (g_buffer, "dat2: %lf", dat2);
nh.loginfo(g_buffer);
sprintf (g_buffer, "dat3: %lf", dat3);
nh.loginfo(g_buffer);
sprintf (g_buffer, "dat4: %lf", dat4);
nh.loginfo(g_buffer);
}
void init_g_prev_time(unsigned long t){
g_prev_loop_first_time=t;
g_prev_stick_ctrl_time = t;
g_prev_stick_time = t;
g_prev_imu_check_time = t;
g_prev_led_blink_time = t;
g_prev_command_time = t;
g_prev_control_time = t;
g_publish_vel_time = t;
g_prev_imu_time = t;
g_prev_debug_time = t;
}
void setup() {
motor1.spin(0);
motor2.spin(0);
motor3.spin(0);
motor4.spin(0);
afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY);
PS2X_ez.PS2_PSS_callback=stick_callback;
init_encoder_pin_data(MOTOR1_ENCODER_A,MOTOR1_ENCODER_B,&encoder1,0);//front
init_encoder_pin_data(MOTOR2_ENCODER_A,MOTOR2_ENCODER_B,&encoder2,1);//front
init_encoder_pin_data(MOTOR3_ENCODER_A,MOTOR3_ENCODER_B,&encoder3,2);//front
init_encoder_pin_data(MOTOR4_ENCODER_A,MOTOR4_ENCODER_B,&encoder4,3);//front
Timer4.pause();
Timer4.setPrescaleFactor(72); // 1 µs resolution
Timer4.setMode(TIMER_CH3, TIMER_OUTPUTCOMPARE);
Timer4.setCount(0);
Timer4.setOverflow(100);//脉冲5000hz/s，中断设置10000次/s检测,保证高低电平各取样2个点
Timer4.setCompare(TIMER_CH1, 100); // somewhere in the middle
Timer4.attachInterrupt(TIMER_CH1, update);
Timer4.refresh();
Timer4.resume();
//Serial.begin(9600);
delay(10);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(PC13,HIGH);
pinMode(PB7, INPUT_PULLDOWN);
Wire.setClock(400000);
Wire.begin();
ROS_setup();
delay(100);
checkIMU_zzz();//cost long time.
init_g_prev_time(millis());
}
int c=0;
long pp=-999;
int pwm1,pwm2,pwm3,pwm4;
void loop() {
//delay(5);
//if(c%100==0){ c=0; } c++;
//this block drives the robot based on defined rate
if (stick_wait_ok==0){
if(millis()>g_prev_loop_first_time+1000){
stick_wait_ok=1;//wait 1s ,kepp no cmd
stick_setup_ok=PS2X_ez.ps_setup;//wait 1s ,try to conect stick device.
}
}
if(stick_setup_ok){//< 1s value==1,> 1s value == the last PS2X_ez.ps_setup
if ((millis() - g_prev_stick_time) >= (100)){
stick_a=0.0;
stick_v=0.0;
PS2X_ez.PS2_loop();
g_prev_stick_time = millis();
}
}//else no stick device or device error.
if ((millis() - g_prev_stick_ctrl_time) >= (250)){
ros_msg_ctrl=1;
}
if ((millis() - g_prev_command_time) >= (200)){
stopBase();
}
//this block drives the robot based on defined rate
if ((millis() - g_prev_control_time) >= 20){//(1000 / COMMAND_RATE)
motor1.updateSpeed(encoder1.position);
motor2.updateSpeed(encoder2.position);
motor3.updateSpeed(encoder3.position);
motor4.updateSpeed(encoder4.position);
moveBase();
//test:
// pwm1=motor1_pid.compute(constrain(20, -MAX_RPM, MAX_RPM), motor1.rpm);
// motor1.spin(pwm1);
// pwm2=motor2_pid.compute(constrain(-20, -MAX_RPM, MAX_RPM), motor2.rpm);
// motor2.spin(pwm2);
// pwm3=motor3_pid.compute(constrain(20, -MAX_RPM, MAX_RPM), motor3.rpm);
// motor3.spin(pwm3);
// pwm4=motor4_pid.compute(constrain(-20, -MAX_RPM, MAX_RPM), motor4.rpm);
// motor4.spin(pwm4);
g_prev_control_time = millis();
}
//this block publishes velocity based on defined rate
if ((millis() - g_publish_vel_time) >= (1000 / VEL_PUBLISH_RATE)){
publishVelocities();
// Serial.println(String("1posi:")+encoder1.position);
// Serial.println(String("1rpm:")+motor1.rpm);
// Serial.println(String("1pwm:")+pwm1);
// Serial.println(String("2posi:")+encoder2.position);
// Serial.println(String("2rpm:")+motor2.rpm);
// Serial.println(String("2pwm:")+pwm2);
g_publish_vel_time = millis();
}
//this block publishes the IMU data based on defined rate
if ((millis() - g_prev_imu_time) >= (1000 / IMU_PUBLISH_RATE)){
//sanity check if the IMU exits
if (g_is_first){
checkIMU_zzz();
}else{
//publish the IMU data
publishIMU_zzz();
}
g_prev_imu_time = millis();
}
if ((millis() - g_prev_led_blink_time) >= (500 )){
if(led_blink==0){
led_blink=1;
}else{
led_blink=0;
}
digitalWrite(PC13, led_blink);
g_prev_led_blink_time = millis();
}
if ((millis() - g_prev_imu_check_time) >= (100 )){
//checkIMU_zzz();
g_prev_imu_check_time = millis();
}
//this block displays the encoder readings. change DEBUG to 0 if you don't want to display
if(DEBUG){
if ((millis() - g_prev_debug_time) >= (1000 / DEBUG_RATE)){
g_prev_debug_time = millis();
}
}
//call all the callbacks waiting to be called
nh.spinOnce();
}
void PIDCallback(const zr_movebase_msgs::PID& pid)
{
//callback function every time PID constants are received from lino_pid for tuning
//this callback receives pid object where P,I, and D constants are stored
pid_p=pid.p;
pid_i=pid.i;
pid_d=pid.d;
//printDebug((float)pid_p,(float)pid_i,(float)pid_d,(float)0.0);//==0??
motor1_pid.updateConstants(pid.p, pid.i, pid.d);
motor2_pid.updateConstants(pid.p, pid.i, pid.d);
motor3_pid.updateConstants(pid.p, pid.i, pid.d);
motor4_pid.updateConstants(pid.p, pid.i, pid.d);
}
void commandCallback(const geometry_msgs::Twist& cmd_msg)
{
//callback function every time linear and angular speed is received from 'cmd_vel' topic
//this callback function receives cmd_msg object where linear and angular speed are stored
if(ros_msg_ctrl){
g_req_linear_vel_x = cmd_msg.linear.x;
g_req_linear_vel_y = cmd_msg.linear.y;
g_req_angular_vel_z = -cmd_msg.angular.z;//fanle??
}
g_prev_command_time = millis();
}随便来个小车实况图，没有梳妆，显得凌乱了些。
演示：
安装麦轮，底盘遥控演示
安装普通轮，小车配上雷达，树莓派安装ROS 自动导航控制的演示
总结：
用ROS框架的大脑驱动廉价模块搭建成的低成本车，赋予Ta灵魂，也基本做到"辞能达意“了。
机器人得有个名字，就叫Ta”晶意智能“车吧。
不满意小车的表现怎么办？多添加些其他传感器？
让我们深入研究ROS，优化算法，用智慧补足短板吧。