Skip to content

Commit

Permalink
update motor control
Browse files Browse the repository at this point in the history
  • Loading branch information
@tinhnn
tinhnn committed Sep 18, 2020
1 parent 4ee2b04 commit 0daef2f
Show file tree
Hide file tree
Showing 10 changed files with 145 additions and 128 deletions.
14 changes: 12 additions & 2 deletions App/autobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,16 @@ void setup()
tf_broadcaster.init(nh);

// Initialize motor control board
motor_control.init();
struct robot_param param;
param.wheel_base = WHEEL_BASE;
param.track_width = TRACK_WIDTH;
param.wheel_radius = WHEEL_RADIUS;
param.motor_max_rotation = MOTOR_MAX_ROTATION;
param.motor_max_speed = MOTOR_MAX_SPEED;
param.motor_min_speed = MOTOR_MIN_SPEED;
param.anglr_max_speed = ANGLR_MAX_SPEED;
param.anglr_min_speed = ANGLR_MIN_SPEED;
motor_control.init(param);

// Initialize IMU
sensor.init();
Expand All @@ -77,6 +86,7 @@ void loop()
else {
motor_control.write_velocity(goal_velocity[0],goal_velocity[1]);
}
//motor_control.write_velocity(0.1, 0.1);

evtTimer[UPDATE_MOTOR_VEL] = cur_t;
}
Expand Down Expand Up @@ -274,7 +284,7 @@ void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg)
goal_velocity[ANGULAR] = cmd_vel_msg.angular.z;

goal_velocity[LINEAR] = constrain(goal_velocity[LINEAR], MOTOR_MIN_SPEED, MOTOR_MAX_SPEED);
goal_velocity[ANGULAR] = constrain(goal_velocity[ANGULAR], MIN_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY);
goal_velocity[ANGULAR] = constrain(goal_velocity[ANGULAR], ANGLR_MIN_SPEED, ANGLR_MAX_SPEED);
evtTimer[REV_CMD_VEL] = get_currenttime();
}

Expand Down
21 changes: 21 additions & 0 deletions App/autobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,27 @@
extern "C" {
#endif

#define PI 3.14159265359 //


/* ROBOT Parameter */
#define WHEEL_BASE 0.15 /* [m] distance between front and rear wheel */
#define TRACK_WIDTH 0.15 /* [m] distance between 2 wheel */
#define WHEEL_RADIUS 0.03 /* [m] wheel's radius */

#define TURNING_RADIUS 0.1435 /* [m] */
#define ROBOT_RADIUS 0.220 /* [m] */

/* Encoder Parameter */
#define ENCODER_MIN -2147483648 /* [raw] */
#define ENCODER_MAX 2147483648 /* [raw] */

/* Motor Parameter */
#define MOTOR_MAX_ROTATION 100 /* [rpm] */
#define MOTOR_MAX_SPEED 2*PI*WHEEL_RADIUS*MOTOR_MAX_ROTATION/60 /* [m/s] */
#define MOTOR_MIN_SPEED -MOTOR_MAX_SPEED /* [m/s] */
#define ANGLR_MAX_SPEED (MOTOR_MAX_SPEED / TRACK_WIDTH) /* [rad/s] */
#define ANGLR_MIN_SPEED -ANGLR_MAX_SPEED /* [rad/s] */


/*******************************************************************************
Expand Down
17 changes: 10 additions & 7 deletions Core/Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,12 @@ UART_HandleTypeDef huart3;

/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask",
.priority = (osPriority_t) osPriorityNormal,
.stack_size = 128 * 4
};
osThreadAttr_t defaultTask_attributes;
//const osThreadAttr_t defaultTask_attributes = {
// .name = "defaultTask",
// .priority = (osPriority_t) osPriorityNormal,
// .stack_size = 128 * 4
//};
/* USER CODE BEGIN PV */

/* USER CODE END PV */
Expand Down Expand Up @@ -106,7 +107,9 @@ int main(void)
MX_USART3_UART_Init();
MX_I2C1_Init();
/* USER CODE BEGIN 2 */

defaultTask_attributes.name = "defaultTask";
defaultTask_attributes.priority = (osPriority_t) osPriorityNormal;
defaultTask_attributes.stack_size = 128 * 4;
/* USER CODE END 2 */

/* Init scheduler */
Expand Down Expand Up @@ -360,7 +363,7 @@ void StartDefaultTask(void *argument)
for(;;)
{
loop();
osDelay(1);
//osDelay(1);
}
/* USER CODE END 5 */
}
Expand Down
8 changes: 4 additions & 4 deletions Core/Src/stm32f7xx_hal_msp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,10 +94,10 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)

__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB8 ------> I2C1_SCL
PB9 ------> I2C1_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_9;
GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
Expand Down Expand Up @@ -130,10 +130,10 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)
__HAL_RCC_I2C1_CLK_DISABLE();

/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB8 ------> I2C1_SCL
PB9 ------> I2C1_SDA
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6);
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8);

HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9);

Expand Down
7 changes: 5 additions & 2 deletions Middlewares/MOTOR_CTRL/PCA9685/PCA9685.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@

#include <stdio.h>
#include "PCA9685.h"
#include "main.h"

extern I2C_HandleTypeDef hi2c1;


/*******************************************************************************
Expand All @@ -14,9 +17,9 @@ PCA9685::PCA9685(uint8_t addr)
}


void PCA9685::init(uint16_t freq, I2C_HandleTypeDef *i2c_hdl)
void PCA9685::init(uint16_t freq)
{
_i2c_hdl = i2c_hdl;
_i2c_hdl = &hi2c1;
_pwm.begin(_i2c_hdl);
_freq = freq;
_pwm.setPWMFreq(_i2c_hdl, _freq); // This is the maximum PWM frequency
Expand Down
2 changes: 1 addition & 1 deletion Middlewares/MOTOR_CTRL/PCA9685/PCA9685.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ class PCA9685
public:
PCA9685(uint8_t addr = 0x60);

void init(uint16_t freq = 1600, I2C_HandleTypeDef *i2c_hdl = NULL);
void init(uint16_t freq = 1600);
DCMotor *getMotor(uint8_t n);
StepperMotor *getStepper(uint16_t steps, uint8_t n);
friend class DCMotor;
Expand Down
49 changes: 28 additions & 21 deletions Middlewares/MOTOR_CTRL/PCA9685/PWM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#include <math.h>

PWM::PWM(uint8_t addr) {
_i2caddr = addr;
_i2caddr = addr<<1;
}

void PWM::begin(I2C_HandleTypeDef *i2c_hdl) {
Expand Down Expand Up @@ -34,33 +34,40 @@ void PWM::setPWMFreq(I2C_HandleTypeDef *i2c_hdl, float freq) {
}

void PWM::setPWM(I2C_HandleTypeDef *i2c_hdl, uint8_t num, uint16_t on, uint16_t off) {
uint8_t tx;
tx = LED0_ON_L + 4 * num;
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &tx, 1);
tx = (uint8_t)on;
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &tx, 1);
tx = (uint8_t)(on >> 8);
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &tx, 1);
tx = (uint8_t)off;
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &tx, 1);
tx = (uint8_t)(off >> 8);
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &tx, 1);

while (HAL_I2C_GetState(i2c_hdl) != HAL_I2C_STATE_READY) {}
uint32_t ret;
uint8_t reg_addr;
uint8_t message[4];

reg_addr = LED0_ON_L + 4 * num;
message[0] = on & 0xFF;
message[1] = on >> 8;
message[2] = off & 0xFF;
message[3] = off >> 8;

ret = HAL_I2C_Mem_Write(i2c_hdl, _i2caddr, reg_addr, 1, message, 4, 1000);
if(!ret){
// error handle
}

}

uint8_t PWM::read8(I2C_HandleTypeDef *i2c_hdl, uint8_t addr) {
uint32_t ret;
uint8_t rx = 0;

HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &addr, 1);
while (HAL_I2C_GetState(i2c_hdl) != HAL_I2C_STATE_READY) {}

HAL_I2C_Master_Receive_IT(i2c_hdl, _i2caddr, &rx, 1);
ret = HAL_I2C_Mem_Read(i2c_hdl, _i2caddr, addr, 1, &rx, 1, 1000);
if(!ret){
// error handle
}

return rx;
}

void PWM::write8(I2C_HandleTypeDef *i2c_hdl, uint8_t addr, uint8_t d) {
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &addr, 1);
HAL_I2C_Master_Transmit_IT(i2c_hdl, _i2caddr, &d, 1);
while (HAL_I2C_GetState(i2c_hdl) != HAL_I2C_STATE_READY) {}
uint32_t ret;

ret = HAL_I2C_Mem_Write(i2c_hdl, _i2caddr, addr, 1, &d, 1, 1000);
if(!ret){
// error handle
}
}
107 changes: 42 additions & 65 deletions Middlewares/MOTOR_CTRL/motor_controller.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,43 +1,46 @@
/* Copyright (c) 2020 Nguyen Nhan Tinh. All rights reserved. */

#include "motor_controller.h"
#include "main.h"

extern I2C_HandleTypeDef hi2c1;

#define LEFT 0
#define RIGHT 1
#define PI 3.14159265359
#define constrain(val,low,high) ((val)<=(low)?(low):((val)>=(high)?(high):(val)))

motor_control::motor_control()
:_motor_drive(0x60)
{
_motor_drive = PCA9685(0x60);
//TODO
}

void motor_control::init(void)
void motor_control::init(struct robot_param param)
{
_motor_drive.init(1000, &hi2c1);
//
_robot_param = param;

/* initial board */
_motor_drive.init(1000); /* PWM freq = 1kHz */
// Get motor pin control
motor[MOTOR_FRONT_LEFT] = _motor_drive.getMotor(MOTOR_FRONT_LEFT + 1);
motor[MOTOR_FRONT_RIGHT] = _motor_drive.getMotor(MOTOR_FRONT_RIGHT + 1);
motor[MOTOR_REAR_LEFT] = _motor_drive.getMotor(MOTOR_REAR_LEFT + 1);
motor[MOTOR_REAR_RIGHT] = _motor_drive.getMotor(MOTOR_REAR_RIGHT + 1);
motor[MOTOR_FRONT_LEFT] = _motor_drive.getMotor(MOTOR_FRONT_LEFT + 1); // motor 1
motor[MOTOR_FRONT_RIGHT] = _motor_drive.getMotor(MOTOR_FRONT_RIGHT + 1); // motor 2
motor[MOTOR_REAR_LEFT] = _motor_drive.getMotor(MOTOR_REAR_LEFT + 1); // motor 3
motor[MOTOR_REAR_RIGHT] = _motor_drive.getMotor(MOTOR_REAR_RIGHT + 1); // motor 4
}

bool motor_control::write_velocity(float linear_x, float angular_z)
{
bool ret;
double wheel_velocity[2];

wheel_velocity[LEFT] = linear_x - angular_z * TRACK_WIDTH / 2;
wheel_velocity[RIGHT] = linear_x + angular_z * TRACK_WIDTH / 2;
wheel_velocity[LEFT] = linear_x - angular_z * _robot_param.track_width / 2;
wheel_velocity[RIGHT] = linear_x + angular_z * _robot_param.track_width / 2;

wheel_velocity[LEFT] = constrain(wheel_velocity[LEFT], -MOTOR_MAX_SPEED, MOTOR_MAX_SPEED);
wheel_velocity[RIGHT] = constrain(wheel_velocity[RIGHT], -MOTOR_MAX_SPEED, MOTOR_MAX_SPEED);
wheel_velocity[LEFT] = constrain(wheel_velocity[LEFT], _robot_param.motor_min_speed, _robot_param.motor_max_speed);
wheel_velocity[RIGHT] = constrain(wheel_velocity[RIGHT], _robot_param.motor_min_speed, _robot_param.motor_max_speed);

/* Convert to duty */
float left_duty = (float)(wheel_velocity[LEFT] / MOTOR_MAX_SPEED * MAX_DUTY_CYCLE);
float right_duty = (float)(wheel_velocity[RIGHT] / MOTOR_MAX_SPEED * MAX_DUTY_CYCLE);
float left_duty = (float)(wheel_velocity[LEFT] / _robot_param.motor_max_speed * MAX_DUTY_CYCLE);
float right_duty = (float)(wheel_velocity[RIGHT] / _robot_param.motor_max_speed * MAX_DUTY_CYCLE);

/* write duty to HW */
ret = write_duty2HW(left_duty, right_duty);
Expand All @@ -47,71 +50,45 @@ bool motor_control::write_velocity(float linear_x, float angular_z)

bool motor_control::write_duty2HW(float left_duty, float right_duty)
{
// TODO
return true;
}
/*
bool writeVelocity(int64_t left_value, int64_t right_value)
{
uint8_t duty;
if(left_value == 0) {
uint8_t speed;
if(left_duty == 0) {
motor[MOTOR_FRONT_LEFT]->run(RELEASE);
motor[MOTOR_REAR_LEFT]->run(RELEASE);
}
else if(left_value > 0) {
duty = (uint8_t)(left_value*255/MOTOR_MAX_SPEED);
motor[MOTOR_FRONT_LEFT]->setSpeed(duty);
motor[MOTOR_REAR_LEFT]->setSpeed(duty);
else if(left_duty > 0) {
speed = (uint8_t)(left_duty*255);
motor[MOTOR_FRONT_LEFT]->setSpeed(speed);
motor[MOTOR_REAR_LEFT]->setSpeed(speed);
motor[MOTOR_FRONT_LEFT]->run(FORWARD);
motor[MOTOR_REAR_LEFT]->run(FORWARD);
}
else {
duty = (uint8_t)(-1*left_value*255/MOTOR_MAX_SPEED);
motor[MOTOR_FRONT_LEFT]->setSpeed(duty);
motor[MOTOR_REAR_LEFT]->setSpeed(duty);
speed = (uint8_t)(-1*left_duty*255);
motor[MOTOR_FRONT_LEFT]->setSpeed(speed);
motor[MOTOR_REAR_LEFT]->setSpeed(speed);
motor[MOTOR_FRONT_LEFT]->run(BACKWARD);
motor[MOTOR_REAR_LEFT]->run(BACKWARD);
}

if(right_value == 0) {
motor[MOTOR_FRONT_LEFT]->run(RELEASE);
motor[MOTOR_REAR_LEFT]->run(RELEASE);
if(right_duty == 0) {
motor[MOTOR_FRONT_RIGHT]->run(RELEASE);
motor[MOTOR_REAR_RIGHT]->run(RELEASE);
}
else if(right_value > 0) {
duty = (uint8_t)(right_value*255/MOTOR_MAX_SPEED);
motor[MOTOR_FRONT_LEFT]->setSpeed(duty);
motor[MOTOR_REAR_LEFT]->setSpeed(duty);
motor[MOTOR_FRONT_LEFT]->run(FORWARD);
motor[MOTOR_REAR_LEFT]->run(FORWARD);
else if(right_duty > 0) {
speed = (uint8_t)(right_duty*255);
motor[MOTOR_FRONT_RIGHT]->setSpeed(speed);
motor[MOTOR_REAR_RIGHT]->setSpeed(speed);
motor[MOTOR_FRONT_RIGHT]->run(FORWARD);
motor[MOTOR_REAR_RIGHT]->run(FORWARD);
}
else {
duty = (uint8_t)(-1*right_value*255/MOTOR_MAX_SPEED);
motor[MOTOR_FRONT_LEFT]->setSpeed(duty);
motor[MOTOR_REAR_LEFT]->setSpeed(duty);
motor[MOTOR_FRONT_LEFT]->run(BACKWARD);
motor[MOTOR_REAR_LEFT]->run(BACKWARD);
speed = (uint8_t)(-1*right_duty*255);
motor[MOTOR_FRONT_RIGHT]->setSpeed(speed);
motor[MOTOR_REAR_RIGHT]->setSpeed(speed);
motor[MOTOR_FRONT_RIGHT]->run(BACKWARD);
motor[MOTOR_REAR_RIGHT]->run(BACKWARD);
}

return true;
}

bool controlMotor(const float wheel_radius, const float wheel_separation, float* value)
{
bool ret = false;
float wheel_velocity_cmd[2];
float lin_vel = value[LEFT];
float ang_vel = value[RIGHT];
wheel_velocity_cmd[LEFT] = lin_vel - (ang_vel * wheel_separation / 2);
wheel_velocity_cmd[RIGHT] = lin_vel + (ang_vel * wheel_separation / 2);
//wheel_velocity_cmd[LEFT] = constrain(wheel_velocity_cmd[LEFT] * VELOCITY_CONSTANT_VALUE / wheel_radius, -1*AUTOBOT_MAX_VELOCITY, AUTOBOT_MAX_VELOCITY);
//wheel_velocity_cmd[RIGHT] = constrain(wheel_velocity_cmd[RIGHT] * VELOCITY_CONSTANT_VALUE / wheel_radius, -1*AUTOBOT_MAX_VELOCITY, AUTOBOT_MAX_VELOCITY);
ret = writeVelocity((int64_t)wheel_velocity_cmd[LEFT], (int64_t)wheel_velocity_cmd[RIGHT]);
return ret;
}
*/
Loading

0 comments on commit 0daef2f

Please sign in to comment.