logzhan
/
RobotHardware-UESTC
1
0
Fork 2
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

更新文档与部分代码注释

main
UESTCsecurity 2024-01-24 20:44:32 +08:00
parent dfeedf5920
commit 4f26f0ddae
11 changed files with 296 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
Hardware/Firmware/GeekRobotTiny_Firmware v1.0/Project/Project.uvguix.ASUS
View File

File diff suppressed because one or more lines are too long

2
Hardware/Pibot驱动底盘/PiRobot_Firmware v1.0/STM32/params.mk
View File

@ -20,7 +20,7 @@
-DMOTOR_DRIVER_TB6612=1
-DMOTOR_DRIVER_DRV8870=2
-DMOTOR_DRIVER=MOTOR_DRIVER_DRV8870
-DMOTOR_DRIVER=MOTOR_DRIVER_TB6612
#uart
-DMASTER_USART=3

8
Hardware/Pibot驱动底盘/PiRobot_Firmware v1.0/STM32/pibot.uvoptx
View File

@ -82,17 +82,17 @@
<Book>
<Number>0</Number>
<Title>Base Board Schematics (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-base-board-schematics.pdf</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-base-board-schematics.pdf</Path>
</Book>
<Book>
<Number>1</Number>
<Title>Display Board Schematics (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-display-board-schematics.pdf</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-display-board-schematics.pdf</Path>
</Book>
<Book>
<Number>2</Number>
<Title>User Manual (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e.chm</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e.chm</Path>
</Book>
<Book>
<Number>3</Number>
@ -139,7 +139,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U303030303030303030303031 -I0 -O206 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F10x_128.FLM -FS08000000 -FL020000 -FP0($$Device:STM32F103C8$Flash\STM32F10x_128.FLM)</Name>
<Name>-U303030303030303030303031 -O206 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8004 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F10x_128.FLM -FS08000000 -FL020000 -FP0($$Device:STM32F103C8$Flash\STM32F10x_128.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>

10
Hardware/Pibot驱动底盘/Pibot官方配置教程.md
View File

@ -68,7 +68,7 @@ struct head
char version[16]; //固件版本
char time[16]; //构建时间
}
```
```
- 交互数据测试
- 发送(十六进制): `5a 00 00 5a`
- 接收(十六进制): `5a 00 20 76 32 2e 30 2e 30 00 00 00 00 00 00 00 00 00 00 32 30 32 30 30 31 30 39 2d 6d 33 65 33 00 00 00 d1`
@ -136,7 +136,7 @@ struct head
- 交互数据测试
- 发送(十六进制): `5a 02 00 5c`
- 接收(十六进制): `5a 02 40 41 00 af 00 2c 00 0a 40 01 8c 0a 00 00 0a 00 fa 00 32 00 00 00 c8 00 47 5a 00 01 0f 0f 32 33 34 35 36 37 38 39 30 31 32 33 34 35 36 37 38 39 30 31 32 33 34 35 36 37 38 39 30 31 32 33 34 35 36 82`
![get params](png/get_params.png)
![get params](png/get_params.png)
```
固定帧头:0x5a
消息id:0x02
@ -227,8 +227,8 @@ struct head
short v_liner_x; //线速度 前>0 后<0 cm/s
short v_liner_y; //差分轮 为0 cm/s
short v_angular_z; //角速度 左>0 右<0 0.01rad/s 100 means 1 rad/s
long x; //里程计坐标x cm (这里long为4字节，下同)
long y; //里程计坐标y cm
long x; //里程计坐标x cm (这里long为4字节，下同)
long y; //里程计坐标y cm
short yaw; //里程计航角 0.01rad 100 means 1 rad
}
```
@ -326,7 +326,7 @@ mz=[7B 94 94 C3] // -297.160004
{
float encoder_count[4]; // 各电机编码器计数
}
```
```
### 3.2.10 电机控制
- 请求:Master->Board

2
Hardware/UPbot-Tools/app.py
View File

@ -14,7 +14,7 @@ from PyQt5.QtCore import QObject,pyqtSignal
import pb
import threading
port = "COM6"
port = "COM4"
pypibot.assistant.enableGlobalExcept()
# log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")

275
Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/BSPLIB/encoder.c
View File

@ -1,154 +1,163 @@
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
#include "encoder.h"
#include "nvic.h"
#include "print.h"
#define ENCODER_TIM_PERIOD (u16)(60000) // number of pulses per revolution
#define ENCODER_TIM_PERIOD (u16)(60000) // number of pulses per revolution
void Encoder_Init(TIM_TypeDef* TIMx , unsigned char GPIO_AF) //Initialize encoder mode, input parameter TIM1 TIM2 TIM3
{
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
if( TIMx == TIM2){
void Encoder_Init(TIM_TypeDef *TIMx, unsigned char GPIO_AF) // Initialize encoder mode, input parameter TIM1 TIM2 TIM3
{
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
if (TIMx == TIM2)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 , ENABLE); /* enable clock */
if(GPIO_AF == 0){
//---------------------------------------------------------------TIM2 CHI1 CHI2---PA0 PA1;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); /* enable clock */
if (GPIO_AF == 0)
{
//---------------------------------------------------------------TIM2 CHI1 CHI2---PA0 PA1;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
else if (GPIO_AF == 1)
{
//---------------------------------------------------------------TIM2 CHI1 CHI2---PA15 PB3;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_FullRemap_TIM2, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); // This is very important!
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
TIM2_NVIC_Configuration(); // enable interrupt
}
else if(GPIO_AF == 1){
//---------------------------------------------------------------TIM2 CHI1 CHI2---PA15 PB3;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
GPIO_PinRemapConfig(GPIO_FullRemap_TIM2,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);// This is very important!
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
else if (TIMx == TIM3)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOA, ENABLE); /* enable clock */
if (GPIO_AF == 0)
{
//---------------------------------------------------------------TIM3 CHI1 CHI2---PA6 PA7;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
else if (GPIO_AF == 1)
{
//---------------------------------------------------------------TIM3 CHI1 CHI2---PB4 PB5;
GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
TIM3_NVIC_Configuration(); // enable interrupt
}
TIM2_NVIC_Configuration(); //enable interrupt
else if (TIMx == TIM4)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); /* enable clock */
if (GPIO_AF == 0)
{
//---------------------------------------------------------------TIM4 CHI1 CHI2---PB6 PB7;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
else if (GPIO_AF == 1)
{
//---------------------------------------------------------------TIM4 CHI1 CHI2---PD12 PD13;
GPIO_PinRemapConfig(GPIO_Remap_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}
TIM4_NVIC_Configuration();
}
else if (TIMx == TIM5)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
if (GPIO_AF == 0)
{
//---------------------------------------------------------------TIM5 CHI1 CHI2---PA0 PA1;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
TIM5_NVIC_Configuration();
}
// 设定TIM_CKD_DIV1和TIM_ICFilter主要起到的是滤除高频信号噪声的作用
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; /* prescler : 72M/72 */
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /* count upwards */
TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIMx, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIMx, &TIM_ICInitStructure);
TIM_Cmd(TIMx, ENABLE);
TIMx->CNT = 0x7fff;
}
else if( TIMx == TIM3){
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA , ENABLE); /* enable clock */
if(GPIO_AF == 0){
//---------------------------------------------------------------TIM3 CHI1 CHI2---PA6 PA7;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
else if (GPIO_AF == 1){
//---------------------------------------------------------------TIM3 CHI1 CHI2---PB4 PB5;
GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
TIM3_NVIC_Configuration(); //enable interrupt
float PB_Get_Encode_TIM2(void)
{
float cnt;
cnt = (float)((uint16_t)0x7fff) - (float)((uint16_t)(TIM2->CNT)); //! (float) is must
TIM2->CNT = 0x7fff;
return cnt;
}
else if( TIMx == TIM4){
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 , ENABLE); /* enable clock */
if(GPIO_AF == 0){
//---------------------------------------------------------------TIM4 CHI1 CHI2---PB6 PB7;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
else if (GPIO_AF == 1){
//---------------------------------------------------------------TIM4 CHI1 CHI2---PD12 PD13;
GPIO_PinRemapConfig(GPIO_Remap_TIM4,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}
TIM4_NVIC_Configuration();
}
else if( TIMx == TIM5){
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5 , ENABLE);
if(GPIO_AF == 0){
//---------------------------------------------------------------TIM5 CHI1 CHI2---PA0 PA1;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
TIM5_NVIC_Configuration();
float Get_EncoderTIM3(void)
{
float cnt;
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM3->CNT));
TIM3->CNT = 30000;
return cnt;
}
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler= 0x0; /* prescler : 72M/72 */
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; /* count upwards */
TIM_TimeBaseInit(TIMx , &TIM_TimeBaseStructure);
float Get_EncoderTIM4(void)
{
float cnt;
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM4->CNT));
TIM4->CNT = 30000;
return cnt;
}
TIM_EncoderInterfaceConfig(TIMx, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIMx, &TIM_ICInitStructure);
TIM_Cmd(TIMx, ENABLE);
TIMx->CNT = 0x7fff;
}
float PB_Get_Encode_TIM2(void)
{
float cnt;
cnt = (float)((uint16_t)0x7fff) - (float)((uint16_t)(TIM2->CNT)) ; //! (float) is must
TIM2->CNT = 0x7fff;
return cnt;
}
float Get_EncoderTIM3(void)
{
float cnt;
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM3->CNT)) ;
TIM3->CNT = 30000;
return cnt;
}
float Get_EncoderTIM4(void)
{
float cnt;
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM4->CNT)) ;
TIM4->CNT = 30000;
return cnt;
}
float PB_Get_Encode_TIM5(void)
{
float cnt;
cnt = (float)((uint16_t)0x7fff) - (float)((uint16_t)(TIM5->CNT)) ;
TIM5->CNT = 0x7fff;
return cnt;
}
float PB_Get_Encode_TIM5(void)
{
float cnt;
cnt = (float)((uint16_t)0x7fff) - (float)((uint16_t)(TIM5->CNT));
TIM5->CNT = 0x7fff;
return cnt;
}
#ifdef __cplusplus
}

11
Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/Lib/KinematicModels/differential.h
View File

@ -19,15 +19,16 @@ public:
virtual void motionSolver(float* robot_speed, float* motor_speed) {
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
motor_speed[0] = (-robot_speed[0] + robot_speed[2] * body_radius)/ wheel_radius;
motor_speed[1] = (robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
motor_speed[1] = ( robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
}
//反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
virtual void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval) {
float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval;
float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
float dth = ( motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval;
odom->vel_x = vxy;
odom->vel_y = 0;

45
Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/Lib/KinematicModels/omni3.h
View File

@ -4,10 +4,9 @@
#include "model.h"
#include "math.h"
//定义Motor数目
// 定义Motor数目
#define MOTOR_COUNT 3
#define sqrt_of_3 1.732f
// 3轮全向模型接口实现
@ -17,31 +16,35 @@ public:
Omni3() {}
Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
//运动解算 把给到机器人的速度分解为各个轮子速度
void motionSolver(float* robot_speed, float* motor_speed) {
// 运动解算 把给到机器人的速度分解为各个轮子速度
void motionSolver(float *robot_speed, float *motor_speed)
{
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius)/ wheel_radius;
motor_speed[1] = (-robot_speed[0]*sqrt_of_3*0.5 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
motor_speed[2] = (robot_speed[0]*sqrt_of_3*0.5 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
}
motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius) / wheel_radius;
motor_speed[1] = (-robot_speed[0] * sqrt_of_3 * 0.5 - robot_speed[1] * 0.5 + robot_speed[2] * body_radius) / wheel_radius;
motor_speed[2] = (robot_speed[0] * sqrt_of_3 * 0.5 - robot_speed[1] * 0.5 + robot_speed[2] * body_radius) / wheel_radius;
}
//反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval) {
if (motor_dis[0]!=0 || motor_dis[1]!=0 || motor_dis[2]!=0){
//speed
float dvx = (-motor_dis[1]+motor_dis[2])*sqrt_of_3/3.0f;
float dvy = (motor_dis[0]*2-motor_dis[1]-motor_dis[2])/3.0f;
float dvth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
odom->vel_x = dvx / interval;
odom->vel_y = dvy / interval;
// 反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
void get_odom(struct Odom *odom, float *motor_dis, unsigned long interval)
{
if (motor_dis[0] != 0 || motor_dis[1] != 0 || motor_dis[2] != 0)
{
// speed
float dvx = (-motor_dis[1] + motor_dis[2] ) * sqrt_of_3 / 3.0f;
float dvy = ( motor_dis[0] * 2 - motor_dis[1] - motor_dis[2]) / 3.0f;
float dvth = ( motor_dis[0] + motor_dis[1] + motor_dis[2]) / (3 * body_radius);
odom->vel_x = dvx / interval;
odom->vel_y = dvy / interval;
odom->vel_z = dvth / interval;
float th = odom->z;
//odometry
float dx = (-sin(th)*motor_dis[0]*2+(-sqrt_of_3*cos(th)+sin(th))*motor_dis[1]+(sqrt_of_3*cos(th)+sin(th))*motor_dis[2])/3.0f;
float dy = (cos(th)*motor_dis[0]*2+(-sqrt_of_3*sin(th)-cos(th))*motor_dis[1]+(sqrt_of_3*sin(th)-cos(th))*motor_dis[2])/3.0f;
float dth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
// odometry
float dx = (-sin(th) * motor_dis[0] * 2 + (-sqrt_of_3 * cos(th) + sin(th)) * motor_dis[1] + (sqrt_of_3 * cos(th) + sin(th)) * motor_dis[2]) / 3.0f;
float dy = (cos(th) * motor_dis[0] * 2 + (-sqrt_of_3 * sin(th) - cos(th)) * motor_dis[1] + (sqrt_of_3 * sin(th) - cos(th)) * motor_dis[2]) / 3.0f;
float dth = (motor_dis[0] + motor_dis[1] + motor_dis[2]) / (3 * body_radius);
odom->x += dx;
odom->y += dy;

5
Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/User/robot.cpp
View File

@ -432,7 +432,7 @@ void Robot::DoKinmatics()
DataHolder::get()->pid_data.input[i] = int(input[i]);
DataHolder::get()->pid_data.output[i] = int(feedback[i]);
}
log("output=%ld %ld", output[0], output[1]);
#if PID_DEBUG_OUTPUT
#if MOTOR_COUNT==2
log("output=%ld %ld", output[0], output[1]);
@ -486,7 +486,8 @@ void Robot::CalcOdom()
#endif
float dis[MOTOR_COUNT] = {0};
for (int i=0;i<MOTOR_COUNT;i++) {
//根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
// 根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
// 距离 = 编码器增量 * PI * 轮子直径(mm) * 0.001 / 编码器分辨率 / 电机的减速比
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*DataHolder::get()->parameter.params.wheel_diameter*0.001/DataHolder::get()->parameter.params.encoder_resolution/DataHolder::get()->parameter.params.motor_ratio;
#if ODOM_DEBUG_OUTPUT
log(" %ld ", long(dis[i]*1000000));

12
Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/pibot.uvoptx
View File

@ -82,17 +82,17 @@
<Book>
<Number>0</Number>
<Title>Base Board Schematics (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-base-board-schematics.pdf</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-base-board-schematics.pdf</Path>
</Book>
<Book>
<Number>1</Number>
<Title>Display Board Schematics (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-display-board-schematics.pdf</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e-display-board-schematics.pdf</Path>
</Book>
<Book>
<Number>2</Number>
<Title>User Manual (MCBSTM32E)</Title>
<Path>D:\Program Files\Keilv5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e.chm</Path>
<Path>C:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\1.1.0\Documents\mcbstm32e.chm</Path>
</Book>
<Book>
<Number>3</Number>
@ -199,7 +199,7 @@
<Group>
<GroupName>FWlib</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -339,7 +339,7 @@
<Group>
<GroupName>CMSIS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -415,7 +415,7 @@
<Group>
<GroupName>STARTUP</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

11
Hardware/银星机器人底盘/README.md
View File

@ -64,7 +64,14 @@ if (pwm_value > 5) {
​ 目前银星机器人采用的是霍尔编码器，但是其磁环的参数是未知的。常见的磁环的有22个极性。那么电机转动一圈下来就会产生44个脉冲计数。霍尔编码器原理详情见《霍尔编码器原理》。
```c
// 运动距离 = (编码器数值 / 编码器一圈脉冲计数) * 减速比 * 轮子直径 / 2 * PI
dis = (encoder_num / ENCODER_RATIO) * reduction ratio * wheel_diameter / 2 * PI;
// 运动距离 = 编码器数值 / 编码器一圈脉冲计数 / 减速比 * 轮子直径 * PI
dis = (encoder_num / ENCODER_RATIO) / reduction ratio * wheel_diameter / PI;
```
​ 对应的，在`robot.cpp`中，可以看到里程计的换算，轮子直径原始是mm单位乘以0.001转m：
```C++
// 距离 = 编码器的增量 * PI * 轮子直径/ 编码器一圈脉冲计数 / 电机减速比
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*DataHolder::get()->parameter.params.wheel_diameter*0.001/DataHolder::get()->parameter.params.encoder_resolution/DataHolder::get()->parameter.params.motor_ratio;
```