diff --git a/Basic_Development/New_Car_Control/.~lock.Test_Engineering.ods# b/Basic_Development/New_Car_Control/.~lock.Test_Engineering.ods# deleted file mode 100644 index 33c1567..0000000 --- a/Basic_Development/New_Car_Control/.~lock.Test_Engineering.ods# +++ /dev/null @@ -1 +0,0 @@ -,zmai,macc,06.06.2024 23:44,file:///home/zmai/.config/libreoffice/4; \ No newline at end of file diff --git a/Basic_Development/New_Car_Control/Encoder_Base/DC_MotoTest.cpp b/Basic_Development/New_Car_Control/Encoder_Base/DC_MotoTest.cpp deleted file mode 100644 index c4f2b15..0000000 --- a/Basic_Development/New_Car_Control/Encoder_Base/DC_MotoTest.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "Emakefun_MotorShield.h" - -int main() { - Emakefun_MotorShield Pwm = Emakefun_MotorShield(); - Pwm.begin(50); - Emakefun_DCMotor *DCmotor1 = Pwm.getMotor(1); - Emakefun_DCMotor *DCmotor2 = Pwm.getMotor(2); - Emakefun_DCMotor *DCmotor3 = Pwm.getMotor(3); - Emakefun_DCMotor *DCmotor4 = Pwm.getMotor(4); - - DCmotor1->setSpeed(255); - DCmotor2->setSpeed(255); - DCmotor3->setSpeed(255); - DCmotor4->setSpeed(255); - - while (1) { - DCmotor1->run(FORWARD); - DCmotor2->run(FORWARD); - DCmotor3->run(FORWARD); - DCmotor4->run(FORWARD); - delay(1000); - DCmotor1->run(BACKWARD); - DCmotor2->run(BACKWARD); - DCmotor3->run(BACKWARD); - DCmotor4->run(BACKWARD); - delay(1000); - } -} \ No newline at end of file diff --git a/Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp b/Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp index e39e035..6b9967b 100644 --- a/Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp +++ b/Basic_Development/New_Car_Control/Encoder_Base/EncoderMotTest.cpp @@ -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 @@ -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(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(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(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; } diff --git a/Basic_Development/New_Car_Control/Encoder_Base/EncoderShield.cpp b/Basic_Development/New_Car_Control/Encoder_Base/EncoderShield.cpp index 88d48e3..01da56d 100644 --- a/Basic_Development/New_Car_Control/Encoder_Base/EncoderShield.cpp +++ b/Basic_Development/New_Car_Control/Encoder_Base/EncoderShield.cpp @@ -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(IN2pin) << " to 0" << std::endl; std::cout << "Setting PWM IN1pin: " << static_cast(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(IN1pin) << " to 0" << std::endl; std::cout << "Setting PWM IN2pin: " << static_cast(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); diff --git a/Basic_Development/New_Car_Control/Encoder_Base/encoder.cpp b/Basic_Development/New_Car_Control/Encoder_Base/encoder.cpp index 538e344..8c53717 100644 --- a/Basic_Development/New_Car_Control/Encoder_Base/encoder.cpp +++ b/Basic_Development/New_Car_Control/Encoder_Base/encoder.cpp @@ -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); } @@ -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(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; @@ -79,7 +66,7 @@ void Encoder::_pulse(int gpio, int level, uint32_t tick) { } // 获取编码器位置 -int32_t Encoder::getPosition() { +int Encoder::getPosition() { return position; } diff --git a/Basic_Development/New_Car_Control/Encoder_Base/encoder.h b/Basic_Development/New_Car_Control/Encoder_Base/encoder.h index 1c27396..ff7a08a 100644 --- a/Basic_Development/New_Car_Control/Encoder_Base/encoder.h +++ b/Basic_Development/New_Car_Control/Encoder_Base/encoder.h @@ -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 -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 diff --git a/Basic_Development/New_Car_Control/P1.zip b/Basic_Development/New_Car_Control/P1.zip index 6130ada..940a0f1 100644 Binary files a/Basic_Development/New_Car_Control/P1.zip and b/Basic_Development/New_Car_Control/P1.zip differ diff --git a/Basic_Development/New_Car_Control/System document.md b/Basic_Development/New_Car_Control/System document.md index 7f2157a..7116bb5 100644 --- a/Basic_Development/New_Car_Control/System document.md +++ b/Basic_Development/New_Car_Control/System document.md @@ -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 \ No newline at end of file +3. the pos and pulse have bug + +20240626： +1. 缠了黑色带子的电机--以我的视觉方向平行车头朝向，视觉上是是右边 +2. 现在的代码，可以测试可以运行 +3. 但是！一个很重要的问题，有个编码器没有变化 +4. \ No newline at end of file