Skip to content

Commit 82289af

Browse files
makssvelichkomakssvelichko
committed
Issue #17: changed motor direction control from STM32 to PCA9685 board
1 parent b580f63 commit 82289af

5 files changed

Lines changed: 86 additions & 48 deletions

File tree

‎KPI_Rover/Motors/PCA9685.c‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,3 +99,16 @@ void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off)
9999
/* Each channel has 4 registers, starting at LED0_ON_L (0x06) */
100100
pca9685_write4(LED0_ON_L + 4 * channel, on, off);
101101
}
102+
103+
void PCA9685_SetPin(uint8_t channel, uint8_t val)
104+
{
105+
if (channel > 15) return;
106+
107+
if (val) {
108+
// Full ON: Bit 4 в ON_H 1 - 4096, OFF в 0
109+
pca9685_write4(LED0_ON_L + 4 * channel, 0x1000, 0x0000);
110+
} else {
111+
// Full OFF: ON в 0, Bit 4 в OFF_H 1 - 4096
112+
pca9685_write4(LED0_ON_L + 4 * channel, 0x0000, 0x1000);
113+
}
114+
}

‎KPI_Rover/Motors/PCA9685.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,11 @@
1+
#ifndef PCA_9685_H_
2+
#define PCA_9685_H_
3+
14
#pragma once
25
#include <stdint.h>
36

47
void PCA9685_Init(void);
58
void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off);
9+
void PCA9685_SetPin(uint8_t channel, uint8_t val);
10+
11+
#endif // PCA_9685_H_

‎KPI_Rover/Motors/drvMotors.c‎

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,12 @@ void DriverMotor_Init(drvMotor_t* motor)
1010
}
1111
else if (motor->pwm_src == PWM_SRC_PCA9685) {
1212
PCA9685_SetPWM(motor->pwm.pca.channel, 0, 0);
13+
PCA9685_SetPin(motor->in1_pca_channel, 0);
14+
PCA9685_SetPin(motor->in2_pca_channel, 0);
1315
}
1416

1517
motor->enabled = false;
18+
motor->last_forward = false;
1619
DriverMotor_setDirection(motor, true);
1720
}
1821

@@ -29,21 +32,38 @@ void DriverMotor_Disable(drvMotor_t* motor)
2932
}
3033

3134
void DriverMotor_setDirection(drvMotor_t* motor, bool forward){
32-
HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, forward ? GPIO_PIN_SET : GPIO_PIN_RESET);
33-
HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, forward ? GPIO_PIN_RESET : GPIO_PIN_SET);
35+
if (motor->pwm_src == PWM_SRC_PCA9685 && motor->last_forward == forward) {
36+
return;
37+
}
38+
39+
if (motor->pwm_src == PWM_SRC_TIM) {
40+
HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, forward ? GPIO_PIN_SET : GPIO_PIN_RESET);
41+
HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, forward ? GPIO_PIN_RESET : GPIO_PIN_SET);
42+
}
43+
else if (motor->pwm_src == PWM_SRC_PCA9685) {
44+
if (forward) {
45+
PCA9685_SetPin(motor->in1_pca_channel, 1); // IN1 = HIGH
46+
PCA9685_SetPin(motor->in2_pca_channel, 0); // IN2 = LOW
47+
} else {
48+
PCA9685_SetPin(motor->in1_pca_channel, 0); // IN1 = LOW
49+
PCA9685_SetPin(motor->in2_pca_channel, 1); // IN2 = HIGH
50+
}
51+
52+
motor->last_forward = forward;
53+
}
3454
}
3555

3656
void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm)
3757
{
3858
if (!motor->enabled)
39-
pwm = 0;
59+
pwm = 0;
4060

4161
if (motor->pwm_src == PWM_SRC_TIM) {
4262
__HAL_TIM_SET_COMPARE(
43-
motor->pwm.tim.htim,
44-
motor->pwm.tim.channel,
45-
pwm
46-
);
63+
motor->pwm.tim.htim,
64+
motor->pwm.tim.channel,
65+
pwm
66+
);
4767
}
4868
else if (motor->pwm_src == PWM_SRC_PCA9685) {
4969
if (pwm > 4095) pwm = 4095;
@@ -52,4 +72,3 @@ void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm)
5272
}
5373

5474

55-

‎KPI_Rover/Motors/drvMotors.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,12 @@ typedef struct {
1818

1919
pwm_source_t pwm_src;
2020

21+
uint8_t in1_pca_channel;
22+
uint8_t in2_pca_channel;
23+
24+
bool last_forward;
25+
26+
2127
union {
2228
struct {
2329
TIM_HandleTypeDef* htim;

‎KPI_Rover/Motors/ulMotorsController.c‎

Lines changed: 34 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -52,51 +52,52 @@ static const uint16_t key_setpoint[ULMOTORS_NUM_MOTORS] = {
5252
MOTOR_RR_SETPOINT
5353
};
5454

55-
static void ulMotorsController_UpdateFeedback(ulMotorsController_t* ctrl)
55+
static void Motors_UpdateFromDB_FloatArray(const uint16_t* keys, float* dst, int count)
5656
{
57-
for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) {
58-
int32_t rpm_db = 0;
59-
60-
if (ulDatabase_getInt32(key_rpm[i], &rpm_db)) {
61-
ctrl->measured_rpm[i] = (float)rpm_db;
57+
for (int i = 0; i < count; i++) {
58+
int32_t val = 0;
59+
if (ulDatabase_getInt32(keys[i], &val)) {
60+
dst[i] = (float)val;
6261
}
6362
}
6463
}
6564

65+
static void ulMotorsController_UpdateFeedback(ulMotorsController_t* ctrl)
66+
{
67+
Motors_UpdateFromDB_FloatArray(key_rpm, ctrl->measured_rpm, ULMOTORS_NUM_MOTORS);
68+
}
69+
70+
static void ulMotorsController_UpdateFromDB(ulMotorsController_t* ctrl)
71+
{
72+
Motors_UpdateFromDB_FloatArray(key_setpoint, ctrl->setpoint_rpm, ULMOTORS_NUM_MOTORS);
73+
}
74+
6675

6776
static void init_motors_hw_mapping(ulMotorsController_t* ctrl)
6877
{
6978
// MOTOR_1_FL
70-
ctrl->motors[0].IN1_port = GPIOE;
71-
ctrl->motors[0].IN1_pin = GPIO_PIN_2;
72-
ctrl->motors[0].IN2_port = GPIOE;
73-
ctrl->motors[0].IN2_pin = GPIO_PIN_4;
7479
ctrl->motors[0].pwm_src = PWM_SRC_PCA9685;
75-
ctrl->motors[0].pwm.pca.channel = 0;
80+
ctrl->motors[0].pwm.pca.channel = 0; // MOTOR_1_PWM
81+
ctrl->motors[0].in1_pca_channel = 1; // MOTOR_1_FWD
82+
ctrl->motors[0].in2_pca_channel = 2; // MOTOR_1_RVR
7683

7784
// MOTOR_2_FR
78-
ctrl->motors[1].IN1_port = GPIOE;
79-
ctrl->motors[1].IN1_pin = GPIO_PIN_5;
80-
ctrl->motors[1].IN2_port = GPIOE;
81-
ctrl->motors[1].IN2_pin = GPIO_PIN_6;
8285
ctrl->motors[1].pwm_src = PWM_SRC_PCA9685;
83-
ctrl->motors[1].pwm.pca.channel = 1;
86+
ctrl->motors[1].pwm.pca.channel = 4; // MOTOR_2_PWM
87+
ctrl->motors[1].in1_pca_channel = 5; // MOTOR_2_FWD
88+
ctrl->motors[1].in2_pca_channel = 6; // MOTOR_2_RVR
8489

8590
// MOTOR_3_RL
86-
ctrl->motors[2].IN1_port = GPIOD;
87-
ctrl->motors[2].IN1_pin = GPIO_PIN_0;
88-
ctrl->motors[2].IN2_port = GPIOD;
89-
ctrl->motors[2].IN2_pin = GPIO_PIN_1;
9091
ctrl->motors[2].pwm_src = PWM_SRC_PCA9685;
91-
ctrl->motors[2].pwm.pca.channel = 2;
92+
ctrl->motors[2].pwm.pca.channel = 15; // MOTOR_3_PWM
93+
ctrl->motors[2].in1_pca_channel = 14; // MOTOR_3_FWD
94+
ctrl->motors[2].in2_pca_channel = 13; // MOTOR_3_RVR
9295

9396
// MOTOR_4_RR
94-
ctrl->motors[3].IN1_port = GPIOD;
95-
ctrl->motors[3].IN1_pin = GPIO_PIN_2;
96-
ctrl->motors[3].IN2_port = GPIOD;
97-
ctrl->motors[3].IN2_pin = GPIO_PIN_3;
9897
ctrl->motors[3].pwm_src = PWM_SRC_PCA9685;
99-
ctrl->motors[3].pwm.pca.channel = 3;
98+
ctrl->motors[3].pwm.pca.channel = 11; // MOTOR_4_PWM
99+
ctrl->motors[3].in1_pca_channel = 10; // MOTOR_4_FWD
100+
ctrl->motors[3].in2_pca_channel = 9; // MOTOR_4_RVR
100101

101102
}
102103

@@ -157,18 +158,6 @@ void ulMotorsController_Init(ulMotorsController_t* ctrl)
157158
}
158159

159160

160-
static void ulMotorsController_UpdateFromDB(ulMotorsController_t* ctrl)
161-
{
162-
for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) {
163-
int32_t sp_db = 0;
164-
165-
if (ulDatabase_getInt32(key_setpoint[i], &sp_db)) {
166-
ctrl->setpoint_rpm[i] = (float)sp_db;
167-
}
168-
}
169-
}
170-
171-
172161
void ulMotorsController_Run(ulMotorsController_t* ctrl)
173162
{
174163
static char teleplot_buf[512];
@@ -262,6 +251,7 @@ static void MotorsTimerCallback(void *argument)
262251

263252
case MOTORS_STATE_IDLE:
264253
{
254+
ulMotorsController_UpdateFromDB(&g_motors_ctrl);
265255
bool need_run = false;
266256
for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) {
267257
if (fabsf(g_motors_ctrl.setpoint_rpm[i]) > 1.0f) {
@@ -285,6 +275,12 @@ static void MotorsTimerCallback(void *argument)
285275

286276
case MOTORS_STATE_RUN:
287277
{
278+
static uint8_t sp_div = 0;
279+
280+
if (++sp_div >= 4) {
281+
sp_div = 0;
282+
ulMotorsController_UpdateFromDB(&g_motors_ctrl);
283+
}
288284
ulMotorsController_UpdateFeedback(&g_motors_ctrl);
289285
ulMotorsController_Run(&g_motors_ctrl);
290286

@@ -345,8 +341,6 @@ void ulMotorsController_Task(void* argument)
345341

346342
for (;;) {
347343

348-
ulMotorsController_UpdateFromDB(&g_motors_ctrl);
349-
350344
osDelay(20);
351345
}
352346
}

0 commit comments

Comments
 (0)