ROS机器人STM32地盘程序

ROS机器人是的stm32底盘代码。包括PID算法，IMU九轴陀螺仪，和上位机串口通讯，PWM控制电机

#include “driver_motor.h“

#define Millisecond（） HAL_GetTick（）
#define constrain（valuelowhigh） （（value）<（low）?（low）:（（value）>（high）?（high）:（value）））

/** PID config **/
float motor_driver::_p = 0.2;	// P constant
float motor_driver::_i = 0.5;	// I constant
float motor_driver::_d = 0.3;	// D constant

/* motor param */
float motor_driver::_ticks_per_rev = 1560; /* wheel encoder‘s no of ticks per rev（gear_ratio * pulse_per_rev） */
float motor_driver::_motor_max_rpm = 300;	/* motor‘s maximum RPM */


motor_driver::motor_driver（TIM_HandleTypeDef& encoder_htim 
														int8_t encoder_direction
														TIM_HandleTypeDef& pwm_htim
														uint32_t pwm_channel
														uint16_t pwm_max
														GPIO_TypeDef* gpiox_in
														uint16_t pin_in1
														uint16_t pin_in2）
														:_pwm_htim（pwm_htim）
														_encoder_htim（encoder_htim）
{
	err_total = 0;
	err_previous = 0;
	/* pwm driver */
	//_pwm_htim = pwm_htim;
	_pwm_channel = pwm_channel;
	_pwm_max = pwm_max;
	_gpiox_in = gpiox_in;
	_pin_in1 = pin_in1;
	_pin_in2 = pin_in2;
	
	/* PWM清零 */
	__HAL_TIM_SET_COMPARE（&_pwm_htim_pwm_channel 0）;
	HAL_TIM_PWM_Start（&_pwm_htim _pwm_channel）;
	/* 制动 */
	HAL_GPIO_WritePin（_gpiox_in _pin_in1 GPIO_PIN_SET）;
	HAL_GPIO_WritePin（_gpiox_in _pin_in2 GPIO_PIN_SET）;
	
	/* encoder */
	//_encoder_htim = encoder_htim;
	_encoder_direction = encoder_direction;
	tick_total = 0;	/* tick总里程 */
	tick_last = 0;
	HAL_TIM_Encoder_Start（&_encoder_htim TIM_CHANNEL_ALL）;
}

void motor_driver::set_rpm（float rpm）
{
	int16_t _pwm;
	double err;
	
	rpm_cur = read_rpm（）;
	/* pid */
	err = constrain（rpm -_motor_max_rpm _motor_max_rpm） - rpm_cur;
	err_total += err;
	
	err_total = constrain（err_total -_pwm_max/_i _pwm_max/_i）;
	
	_pwm = _p*err + _i*err_total + _d*（err-err_previous）;

	err_previous = err;
	
	/* pwm */
	_pwm = constrain（_pwm -_pwm_max _pwm_max）;
	/* < 30 ~ 40的pwm电机根本不会转，但是却在耗电 */
	if（abs（_pwm） < 40） _pwm = 0;
	/* spin */
	spin（_pwm）;
	//print_usart1（“set_rpm[%6.1f] tick_cur[%4d] rpm_cur[%6.2f] err[%7.2lf] err_total[%7.2lf] pwm[%3d]\r\n“ rpm tick_cur rpm_cur err err_total_pwm）;
}

void motor_driver::spin（int16_t pwm）
{
	/* 死区时间 */
	if（（rpm_cur > 0 && pwm < 0） || （rpm_cur < 0 && pwm > 0））
	{
		__HAL_TIM_SET_COMPARE（&_pwm_htim_pwm_channel 0）;
//		HAL_GPIO_WritePin（_gpiox_in _pin_in1 GPIO_PIN_RESET）;
//		HAL_GPIO_WritePin（_gpiox_in _pin_in2 GPIO_PIN_RESET）;
	}
	if（pwm > 0） /* 正转 */
	{
		HAL_GPIO_WritePin（_gpiox_in _pin_in2 GPIO_PIN_RESET）;
		HAL_GPIO_WritePin（_gpiox_in _pin_in1 GPIO_PIN_SET）;
	}
	if（pwm < 0） /* 反转 */
	{
		HAL_GPIO_WritePin（_gpiox_in _pin_in1 GPIO_PIN_RESET）;
		HAL_GPIO_WritePin（_gpiox_in _pin_in2 GPIO_PIN_SET）;
		pwm = -pwm;
	}
	__HAL_TIM_SET_COMPARE（&_pwm_htim_pwm_channel pwm）;
}

double motor_driver::read_rpm（）
{
	uint32_t cur_milli_time;
	double delta_mins_time;
	
	t

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4801  2017-10-02 00:18  X-2_F107RC_GY85_V2.1\.mxproject

     文件       4043  2017-09-16 22:57  X-2_F107RC_GY85_V2.1\BSP\driver_motor.cpp

     文件       1647  2017-09-18 15:20  X-2_F107RC_GY85_V2.1\BSP\driver_motor.h

     文件      10147  2017-09-16 22:44  X-2_F107RC_GY85_V2.1\BSP\driver_oled12864.c

     文件       1891  2017-09-09 00:30  X-2_F107RC_GY85_V2.1\BSP\driver_oled12864.h

     文件      20180  2017-09-09 10:16  X-2_F107RC_GY85_V2.1\BSP\driver_oled12864_word.h

     文件       6821  2017-09-16 23:07  X-2_F107RC_GY85_V2.1\BSP\driver_ps2joystick.c

     文件       2177  2017-09-16 22:35  X-2_F107RC_GY85_V2.1\BSP\driver_ps2joystick.h

     文件       5010  2017-09-08 00:48  X-2_F107RC_GY85_V2.1\BSP\imu_gy85.cpp

     文件       2759  2017-09-16 22:35  X-2_F107RC_GY85_V2.1\BSP\imu_gy85.h

     文件       4415  2017-10-02 00:38  X-2_F107RC_GY85_V2.1\BSP\user_comm.c

     文件        542  2017-09-16 23:34  X-2_F107RC_GY85_V2.1\BSP\user_comm.h

     文件        636  2017-10-21 14:20  X-2_F107RC_GY85_V2.1\BSP\user_protcol.h

     文件       2828  2017-09-21 19:24  X-2_F107RC_GY85_V2.1\BSP\user_robot_base_2.cpp

     文件        802  2017-09-21 19:23  X-2_F107RC_GY85_V2.1\BSP\user_robot_base_2.h

     文件        201  2017-09-07 15:35  X-2_F107RC_GY85_V2.1\BSP\user_rosserial.cpp

     文件       1584  2017-09-16 22:45  X-2_F107RC_GY85_V2.1\BSP\user_rosserial.h

     文件      14686  2017-10-24 11:51  X-2_F107RC_GY85_V2.1\BSP\user_system.cpp

     文件       1400  2017-10-02 00:20  X-2_F107RC_GY85_V2.1\BSP\user_system.h

     文件       6250  2017-08-24 11:22  X-2_F107RC_GY85_V2.1\BSP\Wire.cpp

     文件       3113  2017-08-31 11:06  X-2_F107RC_GY85_V2.1\BSP\Wire.h

     文件       4208  2017-06-11 11:04  X-2_F107RC_GY85_V2.1\BSP\Wirebase.cpp

     文件       4669  2017-08-22 16:29  X-2_F107RC_GY85_V2.1\BSP\Wirebase.h

     文件     499383  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f100xb.h

     文件     544592  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f100xe.h

     文件     450054  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f101x6.h

     文件     460427  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f101xb.h

     文件     538241  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f101xe.h

     文件     553283  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f101xg.h

     文件     542782  2017-08-11 12:37  X-2_F107RC_GY85_V2.1\Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f102x6.h

............此处省略696个文件信息