Skip to content

Commit

Permalink
update: version 4.6.2 fix the motor bug
Browse files Browse the repository at this point in the history
  • Loading branch information
@FennMai
FennMai committed Jun 26, 2024
1 parent 110e798 commit 5d77178
Show file tree
Hide file tree
Showing 8 changed files with 101 additions and 149 deletions.
1 change: 0 additions & 1 deletion Basic_Development/New_Car_Control/.~lock.Test_Engineering.ods#
View file

This file was deleted.

28 changes: 0 additions & 28 deletions Basic_Development/New_Car_Control/Encoder_Base/DC_MotoTest.cpp
View file

This file was deleted.

135 changes: 71 additions & 64 deletions Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
/**
/**
* @author FENN MAI
* @date 06/05/2024
* @version Basic 4.6
* @brief pos-vol-pid control test
* @date 26/06/2024
* @version Basic 4.6.2
* @brief 左右电机控制测试
* pos-vol-pid control test
*/

#include <iostream>
Expand Down Expand Up @@ -51,71 +52,77 @@ int main() {
PID position_pid2(0.1, 0.01, 0.05); // 位置PID motor2
PID speed_pid2(0.1, 0.01, 0.05); // 速度PID motor2

int target_pulses = calculatePulsesForDistance(1.0); // 计算前进1米所需的脉冲数
// 计算前进1米所需的脉冲数
int target_pulses = calculatePulsesForDistance(1.0);

// 必须步骤，重置编码器位置，都设定为0
motor1->resetEncoder();
motor2->resetEncoder();

int previous_pulses1 = 0;
int previous_pulses2 = 0;
double delta_time = 0.1; // 假设每0.1秒进行一次速度计算

motor1->setSpeed(128);
motor2->setSpeed(128);
gpioDelay(2500000);
// 控制左轮（motor1）前进
motor1->setSpeed(50);
motor1->run(FORWARD);
std::cout << "左轮-FORWARD" << std::endl;

// 获取编码器的方向
int dir1 = motor1->getDirection();
std::cout << "左轮方向：" << dir1 << std::endl;

gpioDelay(2500000);

// 获取编码器的方向
dir1 = motor1->getDirection();
std::cout << "左轮方向：" << dir1 << std::endl;
gpioDelay(2500000);
// 停止电机1
motor1->run(BRAKE);
std::cout << "左轮-BRAKE" << std::endl;
gpioDelay(5000000);

// 控制右轮（motor2）前进
motor2->setSpeed(50);
motor2->run(FORWARD);

bool reached_target1 = false;
bool reached_target2 = false;
while (!reached_target1 || !reached_target2) {
// Motor 1
int actual_pulses1 = motor1->readEncoder();
double position_error1 = target_pulses - actual_pulses1;
double desired_speed1 = position_pid1.calculate(target_pulses, actual_pulses1);
double actual_speed1 = calculateSpeed(actual_pulses1, previous_pulses1, delta_time);
previous_pulses1 = actual_pulses1;
double pwm_value1 = speed_pid1.calculate(desired_speed1, actual_speed1);
motor1->setSpeed(static_cast<uint8_t>(std::min(std::max(pwm_value1, 0.0), 255.0)));

int direction1 = motor1->getDirection();
std::string direction_str1 = (direction1 == 1) ? "顺时针" : (direction1 == -1) ? "逆时针" : "静止";

std::cout << "电机编号：1；理想前进值：" << target_pulses
<< "；实际前进值：" << actual_pulses1
<< "；误差：" << position_error1
<< "；方向：" << direction_str1
<< "；PWM值：" << pwm_value1 << std::endl;

// Motor 2
int actual_pulses2 = motor2->readEncoder();
double position_error2 = target_pulses - actual_pulses2;
double desired_speed2 = position_pid2.calculate(target_pulses, actual_pulses2);
double actual_speed2 = calculateSpeed(actual_pulses2, previous_pulses2, delta_time);
previous_pulses2 = actual_pulses2;
double pwm_value2 = speed_pid2.calculate(desired_speed2, actual_speed2);
motor2->setSpeed(static_cast<uint8_t>(std::min(std::max(pwm_value2, 0.0), 255.0)));

int direction2 = motor2->getDirection();
std::string direction_str2 = (direction2 == 1) ? "顺时针" : (direction2 == -1) ? "逆时针" : "静止";

std::cout << "电机编号：2；理想前进值：" << target_pulses
<< "；实际前进值：" << actual_pulses2
<< "；误差：" << position_error2
<< "；方向：" << direction_str2
<< "；PWM值：" << pwm_value2 << std::endl;

// 判断是否达到目标位置
if (abs(position_error1) < 618) { // 允许的误差范围
reached_target1 = true;
}
if (abs(position_error2) < 618) { // 允许的误差范围
reached_target2 = true;
}

gpioDelay(static_cast<unsigned int>(delta_time * 1000 * 1000)); // 延时delta_time秒
}

motor1->run(RELEASE);
motor2->run(RELEASE);
std::cout << "右轮-FORWARD" << std::endl;
// 获取编码器的方向
int dir2 = motor2->getDirection();
std::cout << "右轮方向：" << dir2 << std::endl;
// 持续5s
gpioDelay(2500000);
// 获取编码器的方向
dir2 = motor2->getDirection();
std::cout << "右轮方向：" << dir2 << std::endl;
gpioDelay(2500000);
// 停止电机2
motor2->run(BRAKE);
std::cout << "右轮-RELEASE" << std::endl;

// // 控制左轮（motor1）后退
// motor1->setSpeed(50);
// motor1->run(BACKWARD);
// std::cout << "Left motor (motor1) running backward at speed 50" << std::endl;
// gpioDelay(5000000);
// motor1->run(RELEASE);
// std::cout << "Left motor (motor1) stopped" << std::endl;

// // 控制右轮（motor2）后退
// motor2->setSpeed(50);
// motor2->run(BACKWARD);
// std::cout << "Right motor (motor2) running backward at speed 50" << std::endl;
// gpioDelay(5000000);
// motor2->run(RELEASE);
// std::cout << "Right motor (motor2) stopped" << std::endl;

// // 控制左右轮同时前进
// motor1->setSpeed(50);
// motor2->setSpeed(50);
// motor1->run(FORWARD);
// motor2->run(FORWARD);
// std::cout << "Both motors running forward at speed 50" << std::endl;
// gpioDelay(5000000);
// motor1->run(RELEASE);
// motor2->run(RELEASE);
// std::cout << "Both motors stopped" << std::endl;
gpioTerminate();
return 0;
}
25 changes: 7 additions & 18 deletions Basic_Development/New_Car_Control/Encoder_Base/EncoderShield.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,39 +53,28 @@ void EncoderMotor::run(uint8_t cmd) {
MDIR = cmd;
uint16_t pwm_val = _speed * 16; // Adjusted for the PCA9685 PWM range
switch (cmd) {
// case FORWARD:
// MC->setPin(IN2pin, 0);
// MC->setPWM(IN1pin, pwm_val);
// break;
// case BACKWARD:
// MC->setPin(IN1pin, 0);
// MC->setPWM(IN2pin, pwm_val);
// break;
// case RELEASE:
// MC->setPin(IN1pin, 0);
// MC->setPin(IN2pin, 0);
// break;
// case BRAKE:
// MC->setPin(IN1pin, 1);
// MC->setPin(IN2pin, 1);
// break;
case FORWARD:
// BACKWARD：设置IN2pin为低电平（0），并为IN1pin设置PWM值。
case BACKWARD:
std::cout << "Setting IN2pin: " << static_cast<int>(IN2pin) << " to 0" << std::endl;
std::cout << "Setting PWM IN1pin: " << static_cast<int>(IN1pin) << " to " << pwm_val << std::endl;
MC->setPin(IN2pin, 0);
MC->setPWM(IN1pin, pwm_val);
break;
case BACKWARD:
// FORWARD：设置IN1pin为低电平（0），并为IN2pin设置PWM值。
case FORWARD:
std::cout << "Setting IN1pin: " << static_cast<int>(IN1pin) << " to 0" << std::endl;
std::cout << "Setting PWM IN2pin: " << static_cast<int>(IN2pin) << " to " << pwm_val << std::endl;
MC->setPin(IN1pin, 0);
MC->setPWM(IN2pin, pwm_val);

break;
// RELEASE：将IN1pin和IN2pin都设置为低电平（0），释放电机。
case RELEASE:
std::cout << "Releasing motor" << std::endl;
MC->setPin(IN1pin, 0);
MC->setPin(IN2pin, 0);
break;
// BRAKE：将IN1pin和IN2pin都设置为高电平（1），刹车。
case BRAKE:
std::cout << "Braking motor" << std::endl;
MC->setPin(IN1pin, 1);
Expand Down
25 changes: 6 additions & 19 deletions Basic_Development/New_Car_Control/Encoder_Base/encoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,29 +16,15 @@ B | | | | | |
*/



#include "encoder.h"

// Encoder 构造函数
Encoder::Encoder(int gpioA, int gpioB, encoderCallback_t callback) {
this->gpioA = gpioA;
this->gpioB = gpioB;
this->callback = callback;
position = 0;
levA = 0;
levB = 0;
lastGpio = -1;
direction = 0;

// 将 gpioA 和 gpioB 引脚设置为输入模式
Encoder::Encoder(int gpioA, int gpioB, encoderCallback_t callback)
: gpioA(gpioA), gpioB(gpioB), position(0), direction(0), levA(0), levB(0), lastGpio(-1), callback(callback) {
gpioSetMode(gpioA, PI_INPUT);
gpioSetMode(gpioB, PI_INPUT);
// 启用了 gpioA 和 gpioB 引脚的上拉电阻。上拉电阻用于确保引脚在未连接或未驱动时具有确定的电平（在这里是高电平）。
gpioSetPullUpDown(gpioA, PI_PUD_UP);
gpioSetPullUpDown(gpioB, PI_PUD_UP);
// gpioA 和 gpioB 引脚设置了一个回调函数 _pulseEx。
// 当引脚的电平发生变化（从高到低或从低到高）时，将调用 _pulseEx 函数。
// 通过这种方式，我们可以检测编码器的脉冲信号
gpioSetAlertFuncEx(gpioA, _pulseEx, this);
gpioSetAlertFuncEx(gpioB, _pulseEx, this);
}
Expand All @@ -51,13 +37,14 @@ Encoder::~Encoder() {

// 静态脉冲回调函数
void Encoder::_pulseEx(int gpio, int level, uint32_t tick, void *user) {
Encoder *mySelf = (Encoder *) user;
Encoder *mySelf = static_cast<Encoder *>(user);
mySelf->_pulse(gpio, level, tick);
}

// 实例脉冲回调函数
void Encoder::_pulse(int gpio, int level, uint32_t tick) {
if (gpio == gpioA) levA = level; else levB = level;
if (gpio == gpioA) levA = level;
else levB = level;

if (gpio != lastGpio) {
lastGpio = gpio;
Expand All @@ -79,7 +66,7 @@ void Encoder::_pulse(int gpio, int level, uint32_t tick) {
}

// 获取编码器位置
int32_t Encoder::getPosition() {
int Encoder::getPosition() {
return position;
}

Expand Down
28 changes: 10 additions & 18 deletions Basic_Development/New_Car_Control/Encoder_Base/encoder.h
View file
Original file line number Diff line number Diff line change
@@ -1,36 +1,28 @@
/**
*
* @author FENN MAI
* @date 06/05/2024
* @version Basic 4.0
* @brief 编码电机初次测试
*/

#ifndef _ENCODER_H_
#define _ENCODER_H_
#ifndef ENCODER_H
#define ENCODER_H

#include <pigpio.h>

typedef void (*encoderCallback_t)(int);
typedef void (*encoderCallback_t)(int direction);

class Encoder {
public:
Encoder(int gpioA, int gpioB, encoderCallback_t callback = nullptr);
~Encoder();
int32_t getPosition();
int getPosition();
void reset();
int getDirection(); // 新增方法，获取方向
int getDirection();

private:
int gpioA, gpioB;
int32_t position;
int lastGpio;
int position;
int direction;
int levA, levB;
int lastGpio;
encoderCallback_t callback;
int direction; // 记录方向

static void _pulseEx(int gpio, int level, uint32_t tick, void *user);
void _pulse(int gpio, int level, uint32_t tick);
static void _pulseEx(int gpio, int level, uint32_t tick, void *user);
};

#endif
#endif // ENCODER_H
Binary file modified Basic_Development/New_Car_Control/P1.zip
View file
Binary file not shown.
8 changes: 7 additions & 1 deletion Basic_Development/New_Car_Control/System document.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,4 +42,10 @@ Raspi_i2c --> Emakefun_MotorDriver -->
20240607:
1. pos-vol-pid control test
2. motor2 still have bug
3. the pos and pulse have bug
3. the pos and pulse have bug

20240626：
1. 缠了黑色带子的电机--以我的视觉方向平行车头朝向，视觉上是是右边
2. 现在的代码，可以测试可以运行
3. 但是！一个很重要的问题，有个编码器没有变化
4.

0 comments on commit 5d77178

Please sign in to comment.