Double
/
RobotHardware-UESTC
forked from logzhan/RobotHardware-UESTC
1
0
Fork 0
Code Pull Requests 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_TB6612=1
-DMOTOR_DRIVER_DRV8870=2 -DMOTOR_DRIVER_DRV8870=2
-DMOTOR_DRIVER=MOTOR_DRIVER_DRV8870 -DMOTOR_DRIVER=MOTOR_DRIVER_TB6612
#uart #uart
-DMASTER_USART=3 -DMASTER_USART=3

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

@ -82,17 +82,17 @@
<Book> <Book>
<Number>0</Number> <Number>0</Number>
<Title>Base Board Schematics (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>1</Number> <Number>1</Number>
<Title>Display Board Schematics (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>2</Number> <Number>2</Number>
<Title>User Manual (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>3</Number> <Number>3</Number>
@ -139,7 +139,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key> <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>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>

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

@ -68,7 +68,7 @@ struct head
char version[16]; //固件版本 char version[16]; //固件版本
char time[16]; //构建时间 char time[16]; //构建时间
} }
``` ```
- 交互数据测试 - 交互数据测试
- 发送(十六进制): `5a 00 00 5a` - 发送(十六进制): `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` - 接收(十六进制): `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 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` - 接收(十六进制): `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 固定帧头:0x5a
消息id:0x02 消息id:0x02
@ -326,7 +326,7 @@ mz=[7B 94 94 C3] // -297.160004
{ {
float encoder_count[4]; // 各电机编码器计数 float encoder_count[4]; // 各电机编码器计数
} }
``` ```
### 3.2.10 电机控制 ### 3.2.10 电机控制
- 请求:Master->Board - 请求:Master->Board

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

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

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

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

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

@ -19,13 +19,14 @@ public:
virtual void motionSolver(float* robot_speed, float* motor_speed) { virtual void motionSolver(float* robot_speed, float* motor_speed) {
// robot_speed[0] x robot_speed[1] y robot_speed[2] z // 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[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) { 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 dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius); float dth = ( motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval; float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval; float vth = 1000 * dth / interval;

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

@ -4,10 +4,9 @@
#include "model.h" #include "model.h"
#include "math.h" #include "math.h"
//定义Motor数目 // 定义Motor数目
#define MOTOR_COUNT 3 #define MOTOR_COUNT 3
#define sqrt_of_3 1.732f #define sqrt_of_3 1.732f
// 3轮全向模型接口实现 // 3轮全向模型接口实现
@ -17,31 +16,35 @@ public:
Omni3() {} Omni3() {}
Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {} 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 // 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[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[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[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) { 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 if (motor_dis[0] != 0 || motor_dis[1] != 0 || motor_dis[2] != 0)
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; // speed
float dvth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius); 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_x = dvx / interval;
odom->vel_y = dvy / interval; odom->vel_y = dvy / interval;
odom->vel_z = dvth / interval; odom->vel_z = dvth / interval;
float th = odom->z; float th = odom->z;
//odometry // 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 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 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); float dth = (motor_dis[0] + motor_dis[1] + motor_dis[2]) / (3 * body_radius);
odom->x += dx; odom->x += dx;
odom->y += dy; 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.input[i] = int(input[i]);
DataHolder::get()->pid_data.output[i] = int(feedback[i]); DataHolder::get()->pid_data.output[i] = int(feedback[i]);
} }
log("output=%ld %ld", output[0], output[1]);
#if PID_DEBUG_OUTPUT #if PID_DEBUG_OUTPUT
#if MOTOR_COUNT==2 #if MOTOR_COUNT==2
log("output=%ld %ld", output[0], output[1]); log("output=%ld %ld", output[0], output[1]);
@ -486,7 +486,8 @@ void Robot::CalcOdom()
#endif #endif
float dis[MOTOR_COUNT] = {0}; float dis[MOTOR_COUNT] = {0};
for (int i=0;i<MOTOR_COUNT;i++) { 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; 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 #if ODOM_DEBUG_OUTPUT
log(" %ld ", long(dis[i]*1000000)); log(" %ld ", long(dis[i]*1000000));

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

@ -82,17 +82,17 @@
<Book> <Book>
<Number>0</Number> <Number>0</Number>
<Title>Base Board Schematics (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>1</Number> <Number>1</Number>
<Title>Display Board Schematics (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>2</Number> <Number>2</Number>
<Title>User Manual (MCBSTM32E)</Title> <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>
<Book> <Book>
<Number>3</Number> <Number>3</Number>
@ -199,7 +199,7 @@
<Group> <Group>
<GroupName>FWlib</GroupName> <GroupName>FWlib</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -339,7 +339,7 @@
<Group> <Group>
<GroupName>CMSIS</GroupName> <GroupName>CMSIS</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -415,7 +415,7 @@
<Group> <Group>
<GroupName>STARTUP</GroupName> <GroupName>STARTUP</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

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

@ -64,7 +64,14 @@ if (pwm_value > 5) {
​ 目前银星机器人采用的是霍尔编码器，但是其磁环的参数是未知的。常见的磁环的有22个极性。那么电机转动一圈下来就会产生44个脉冲计数。霍尔编码器原理详情见《霍尔编码器原理》。 ​ 目前银星机器人采用的是霍尔编码器，但是其磁环的参数是未知的。常见的磁环的有22个极性。那么电机转动一圈下来就会产生44个脉冲计数。霍尔编码器原理详情见《霍尔编码器原理》。
```c ```c
// 运动距离 = (编码器数值 / 编码器一圈脉冲计数) * 减速比 * 轮子直径 / 2 * PI // 运动距离 = 编码器数值 / 编码器一圈脉冲计数 / 减速比 * 轮子直径 * PI
dis = (encoder_num / ENCODER_RATIO) * reduction ratio * wheel_diameter / 2 * 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;
``` ```