forked from logzhan/RobotHardware-UESTC
374 lines
9.8 KiB
C++
374 lines
9.8 KiB
C++
#include "board_stm32.h"
|
|
#include "bsplib.h"
|
|
#include <stdio.h>
|
|
Board_STM32 Board_STM32::board;
|
|
|
|
#define CONFIG_EEPROM_BASE ((uint32_t)0x800F400)
|
|
|
|
#ifndef MOTOR_DRIVER
|
|
#define MOTOR_DRIVER MOTOR_DRIVER_TB6612
|
|
#endif
|
|
|
|
Board *Board::get()
|
|
{
|
|
return &Board_STM32::board;
|
|
}
|
|
|
|
Board_STM32::Board_STM32()
|
|
{
|
|
}
|
|
|
|
Board_STM32::~Board_STM32()
|
|
{
|
|
}
|
|
|
|
void Board_STM32::init()
|
|
{
|
|
System_TimerInit();
|
|
DOInit();
|
|
DIInit();
|
|
}
|
|
|
|
void Board_STM32::enable_irq()
|
|
{
|
|
}
|
|
|
|
void Board_STM32::disable_irq()
|
|
{
|
|
}
|
|
|
|
void Board_STM32::Usart_DebugInit()
|
|
{
|
|
USART_InitGPIO(1, 115200, 0);
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
{
|
|
#endif
|
|
int fputc(int ch, FILE *f)
|
|
{
|
|
USART_PutChar(1, ch);
|
|
return (ch);
|
|
}
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
void Board_STM32::usart_init(unsigned char num, unsigned long buad)
|
|
{
|
|
if (num == (unsigned char)USART_1)
|
|
{
|
|
USART_InitGPIO(1, 115200, 0);
|
|
}
|
|
else if (num == (unsigned char)USART_3)
|
|
{
|
|
USART_InitGPIO(3, 115200, 0);
|
|
}
|
|
}
|
|
|
|
Queue *Board_STM32::usart_getDataQueue(unsigned char num)
|
|
{
|
|
if (num == (unsigned char)USART_1)
|
|
{
|
|
}
|
|
else if (num == (unsigned char)USART_3)
|
|
{
|
|
return &usart3_queue;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void Board_STM32::usart_write(unsigned char num, unsigned char ch)
|
|
{
|
|
if (num == (unsigned char)USART_1)
|
|
{
|
|
USART_PutChar(1, ch);
|
|
}
|
|
else if (num == (unsigned char)USART_3)
|
|
{
|
|
USART_PutChar(3, ch);
|
|
}
|
|
}
|
|
|
|
void Board_STM32::usart_write(unsigned char num, unsigned char *data, unsigned char len)
|
|
{
|
|
if (num == (unsigned char)USART_1)
|
|
{
|
|
while (len--)
|
|
USART_PutChar(1, *data++);
|
|
}
|
|
else if (num == (unsigned char)USART_3)
|
|
{
|
|
while (len--)
|
|
USART_PutChar(3, *data++);
|
|
}
|
|
}
|
|
|
|
void Board_STM32::set_config(unsigned char *data, unsigned short len)
|
|
{
|
|
if (len % 2 == 0)
|
|
{
|
|
Flash_EnableWrite();
|
|
for (int i = 0; i < len / 2; i++)
|
|
{
|
|
uint16_t a = 0;
|
|
memcpy(&a, data + i * 2, 2);
|
|
Flash_WriteHalfWord(i * 2, a);
|
|
delay_us(10);
|
|
}
|
|
Flash_DisableWrite();
|
|
}
|
|
}
|
|
|
|
void Board_STM32::get_config(unsigned char *data, unsigned short len)
|
|
{
|
|
if (len % 2 == 0){
|
|
for (int i = 0; i < len / 2; i++){
|
|
uint16_t a = Flash_ReadHalfWord(i * 2);
|
|
memcpy(data + i * 2, &a, 2);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Board_STM32::DOInit(void)
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
|
|
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
|
|
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_5;
|
|
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
|
|
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
|
GPIO_Init(GPIOC, &GPIO_InitStructure);
|
|
GPIO_SetBits(GPIOC, GPIO_Pin_13);
|
|
}
|
|
|
|
void Board_STM32::DIInit(void)
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
|
|
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
|
|
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
}
|
|
|
|
void Board_STM32::setDOState(unsigned char _id, unsigned char operation)
|
|
{
|
|
if ( _id == _RUN_LED) {
|
|
if (operation == 0) {
|
|
GPIO_SetBits(GPIOC, GPIO_Pin_13);
|
|
} else if (operation == 1) {
|
|
GPIO_ResetBits(GPIOC, GPIO_Pin_13);
|
|
} else if (operation == 2) {
|
|
GPIO_ToggleBits(GPIOC, GPIO_Pin_13);
|
|
}
|
|
} else if ( _id == _JS_CMD) {
|
|
if (operation == 0) {
|
|
GPIO_SetBits(GPIOA, GPIO_Pin_5);
|
|
} else if (operation == 1) {
|
|
GPIO_ResetBits(GPIOA, GPIO_Pin_5);
|
|
} else if (operation == 2) {
|
|
GPIO_ToggleBits(GPIOA, GPIO_Pin_5);
|
|
}
|
|
} else if ( _id == _JS_CS) {
|
|
if (operation == 0) {
|
|
GPIO_SetBits(GPIOA, GPIO_Pin_0);
|
|
} else if (operation == 1) {
|
|
GPIO_ResetBits(GPIOA, GPIO_Pin_0);
|
|
} else if (operation == 2) {
|
|
GPIO_ToggleBits(GPIOA, GPIO_Pin_0);
|
|
}
|
|
} else if ( _id == _JS_CLK) {
|
|
if (operation == 0) {
|
|
GPIO_SetBits(GPIOA, GPIO_Pin_1);
|
|
} else if (operation == 1) {
|
|
GPIO_ResetBits(GPIOA, GPIO_Pin_1);
|
|
} else if (operation == 2) {
|
|
GPIO_ToggleBits(GPIOA, GPIO_Pin_1);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool Board_STM32::getDIState(unsigned char _id)
|
|
{
|
|
if ( _id == _JS_DAT) {
|
|
return GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Board_STM32::motor_init(unsigned char num, unsigned long period)
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|
GPIO_StructInit(&GPIO_InitStructure);
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
|
|
// STBY
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
GPIO_ResetBits(GPIOA, GPIO_Pin_12);
|
|
|
|
if (num == MOTOR_1) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_12);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_13);
|
|
PB_PWMChannel_Init(TIM2, 4, 0, period, 0);
|
|
#else
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_12);
|
|
PB_PWMChannel_Init(TIM1, 1, 0, period, 0);
|
|
PB_PWMChannel_Init(TIM2, 4, 0, period, 0);
|
|
#endif
|
|
} else if (num == MOTOR_2) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14 | GPIO_Pin_15;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_14);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_15);
|
|
|
|
PB_PWMChannel_Init(TIM2, 3, 0, period, 0);
|
|
#else
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_14);
|
|
|
|
PB_PWMChannel_Init(TIM1, 4, 0, period, 0);
|
|
PB_PWMChannel_Init(TIM2, 3, 0, period, 0);
|
|
#endif
|
|
} else if (num == MOTOR_3) {
|
|
|
|
} else if (num == MOTOR_4) {
|
|
|
|
}
|
|
}
|
|
|
|
void Board_STM32::motor_pwm(unsigned char num, long pwm_value)
|
|
{
|
|
if (num == MOTOR_1) {
|
|
if (pwm_value > 5) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_SetBits(GPIOB, GPIO_Pin_13);
|
|
PB_Set_PWM(TIM2, 4, (uint16_t)pwm_value);
|
|
#else
|
|
PB_Set_PWM(TIM2, 4, (uint16_t)pwm_value);
|
|
PB_Set_PWM(TIM1, 1, 0);
|
|
#endif
|
|
} else if (pwm_value < -5) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_13);
|
|
PB_Set_PWM(TIM2, 4, (uint16_t)-pwm_value);
|
|
#else
|
|
PB_Set_PWM(TIM1, 1, (uint16_t)-pwm_value);
|
|
PB_Set_PWM(TIM2, 4, 0);
|
|
#endif
|
|
} else {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_13);
|
|
PB_Set_PWM(TIM2, 4, 0);
|
|
#else
|
|
PB_Set_PWM(TIM1, 1, 0);
|
|
PB_Set_PWM(TIM2, 4, 0);
|
|
#endif
|
|
}
|
|
} else if (num == MOTOR_2) {
|
|
if (pwm_value > 5) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_14);
|
|
PB_Set_PWM(TIM2, 3, (uint16_t)pwm_value);
|
|
#else
|
|
PB_Set_PWM(TIM1, 4, (uint16_t)pwm_value);
|
|
PB_Set_PWM(TIM2, 3, 0);
|
|
#endif
|
|
} else if (pwm_value < -5) {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_SetBits(GPIOB, GPIO_Pin_14);
|
|
PB_Set_PWM(TIM2, 3, (uint16_t)-pwm_value);
|
|
#else
|
|
PB_Set_PWM(TIM1, 4, 0);
|
|
PB_Set_PWM(TIM2, 3, (uint16_t)-pwm_value);
|
|
#endif
|
|
} else {
|
|
#if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_14);
|
|
PB_Set_PWM(TIM2, 3, 0);
|
|
#else
|
|
PB_Set_PWM(TIM1, 4, 0);
|
|
PB_Set_PWM(TIM2, 3, 0);
|
|
#endif
|
|
}
|
|
} else if (num == MOTOR_3) {
|
|
|
|
} else if (num == MOTOR_4) {
|
|
|
|
}
|
|
}
|
|
|
|
unsigned long Board_STM32::getTickCount()
|
|
{
|
|
return getSystemTime() / 1000;
|
|
}
|
|
|
|
void Board_STM32::encoder_init(unsigned char motor_id)
|
|
{
|
|
if (motor_id == MOTOR_1){
|
|
Encoder_InitGPIO(TIM3, 0);
|
|
}else if (motor_id == MOTOR_2){
|
|
Encoder_InitGPIO(TIM4, 0);
|
|
}
|
|
}
|
|
|
|
long Board_STM32::GetEncoderCount(unsigned char motorId)
|
|
{
|
|
if (motorId == MOTOR_1){
|
|
return Encoder_GetTIM3();
|
|
}else if (motorId == MOTOR_2){
|
|
return Encoder_GetTIM4();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void Board_STM32::i2c_init()
|
|
{
|
|
I2C_InitGPIO();
|
|
}
|
|
|
|
unsigned char Board_STM32::i2c_write_byte(unsigned char equipment_address, unsigned char reg_address, unsigned char pt_char)
|
|
{
|
|
return I2C_WriteByte(equipment_address, reg_address, pt_char);
|
|
}
|
|
|
|
unsigned char Board_STM32::i2c_write_buf(unsigned char equipment_address, unsigned char reg_address, unsigned char *pt_char, unsigned char size)
|
|
{
|
|
return I2C_WriteBuf(equipment_address, reg_address, pt_char, size);
|
|
}
|
|
|
|
unsigned char Board_STM32::i2c_read_byte(unsigned char equipment_address, unsigned char reg_address, unsigned char *pt_char)
|
|
{
|
|
return I2C_ReadByte(equipment_address, reg_address, pt_char);
|
|
}
|
|
|
|
unsigned char Board_STM32::i2c_read_buf(unsigned char equipment_address, unsigned char reg_address, unsigned char *pt_char, unsigned char size)
|
|
{
|
|
return I2C_ReadBuf(equipment_address, reg_address, pt_char, size);
|
|
}
|