diff --git a/Core/Inc/params.h b/Core/Inc/params.h index 10070a4..69d897c 100644 --- a/Core/Inc/params.h +++ b/Core/Inc/params.h @@ -7,7 +7,15 @@ #define SENSITIVITY_SCALE_FACTOR 16.4 #define GYRO_OFFSET 2.558 -#define GYRO_GAIN_R 0.12 // IMUが-1600 ~ 1600だったので、-180~180にするために0.12をかけた -#define GYRO_GAIN_L 0.13 // IMU値が左右方向で差があったためゲインを少し変更 +#define GYRO_GAIN_R 0.125 // IMUが-1600 ~ 1600だったので、-180~180にするために0.12をかけた +#define GYRO_GAIN_L 0.125 // IMU値が左右方向で差があったためゲインを少し変更 +// 左回転だと足りず、右回転だと行き過ぎ +// 左回転のログ +// 0.115: 5.5回転 +// 0.12: 5.25回転 +// 0.125: 5.05回転 +// 右回転のログ +// 0.12: 5.25 +// 0.125: 4.65 #endif diff --git a/Core/Inc/params.hpp b/Core/Inc/params.hpp index 1897105..d7f1e4b 100644 --- a/Core/Inc/params.hpp +++ b/Core/Inc/params.hpp @@ -3,11 +3,14 @@ #include "main.h" namespace MotorParam { - const float Ke = 0.207/1000.0; // 逆起電圧定数[V/min^-1] - const float Kt = 1.98/1000.0; // トルク定数[Nm/A] + // const float Ke = (2*3.1415926/60)*(0.207/1000.0); // 逆起電圧定数[V*s/rad], 0.00002168 + const float Ke = (2*3.1415926/60)*0.207; // 逆起電圧定数[V*s/rad], 0.00002168 + // const float Kt = 1.98/1000.0; // トルク定数[Nm/A] + const float Kt = 1.98; // トルク定数[Nm/A] const float R = 1.07; // 巻線抵抗[Ω] const float GEAR_RATIO = 43.0/13.0; // ギア比 - const float m = 90.0/1000.0; + // const float m = 90.0/1000.0; + const float m = 90.0; // [g] const float r = (12.0 + 0.6)*0.001; // タイヤ半径[m], 両面テープの厚さ分0.1*2 const int bit = 4096; // const float stm32_vat = 3.3; @@ -15,6 +18,12 @@ namespace MotorParam { const float TREAD_WIDTH = 6.5/1000; const uint8_t RATE = 1; const float PULSE_PER_TIRE_ONEROTATION = 54193.2; // タイヤ一回転あたりのパルス数：4096*4*(43/13) = 54193.2.. + const float DUTY_GAIN = 1.1; // 右モータのゲイン + // 左にどんどん擦れている: 1.23 + // もっとずれた: 1.25 + // マシになった： 1.20 + // 変わらず： 1.15 + // 変わらず: 1.1 } namespace LinearVelocityPID { @@ -24,11 +33,13 @@ namespace LinearVelocityPID { extern float current_linear_vel; extern float calculated_linear_vel; extern float current_distance; - const float Kp = 1.0; + const float Kp = 10.0; const float Ki = 0.3; const float Kd = 0.0; const uint16_t MAX_PID_ERROR_SUM = 10; const int16_t MIN_PID_ERROR_SUM = 0; + const float MAX_SPEED = 1000.0; // [mm/s] + const float MIN_SPEED = 100.0; // [mm/s] } namespace AngularVelocityPID { @@ -38,8 +49,8 @@ namespace AngularVelocityPID { extern float current_angular_vel; extern float calculated_angular_vel; extern float current_angle; - const float Kp = 1.0; - const float Ki = 0.0; + const float Kp = 100.0; + const float Ki = 0.8; const float Kd = 0.0; // TODO: PIDのパラメータ調整 const uint16_t MAX_PID_ERROR_SUM = 60; @@ -51,7 +62,8 @@ namespace Battery { } namespace RobotControllerParam { - const float MAX_SPEED = 200; // [mm/s] + const float MAX_SPEED = 300.0; // [mm/s] + const float MAX_OMEGA = 200.0; // [rad/s] // const float ACCEL = 50; // [mm/s^2] // const float DECEL = 50; // [mm/s^2] const float TARGET_DISTANCE = 0.3; diff --git a/Core/Inc/robot_controller.hpp b/Core/Inc/robot_controller.hpp index 58173b2..38f1919 100644 --- a/Core/Inc/robot_controller.hpp +++ b/Core/Inc/robot_controller.hpp @@ -16,6 +16,7 @@ class RobotController { public: + bool is_running; RobotController(); void mainControl(); void motorControl(float target_linear_vel, float target_angular_vel); diff --git a/Core/Src/motor.cpp b/Core/Src/motor.cpp index fe81314..781e797 100644 --- a/Core/Src/motor.cpp +++ b/Core/Src/motor.cpp @@ -34,21 +34,22 @@ extern "C" { LinearVelocityPID::calculated_linear_vel = Motor::linearVelocityPIDControl(LinearVelocityPID::target_linear_vel, LinearVelocityPID::current_linear_vel, LinearVelocityPID::vel_pid_error_sum); AngularVelocityPID::calculated_angular_vel = Motor::angularVelocityPIDControl(AngularVelocityPID::target_angular_vel, AngularVelocityPID::current_angular_vel, AngularVelocityPID::w_pid_error_sum); + // タイヤの回転速度は[rpm]ではなく、[rad/s], 300[rad/s]くらいが定格, 1500が最大 motor_r.rotation_speed = motor_r.calcMotorSpeed(LinearVelocityPID::calculated_linear_vel, AngularVelocityPID::calculated_angular_vel); // [rpm] motor_l.rotation_speed = motor_l.calcMotorSpeed(LinearVelocityPID::calculated_linear_vel, AngularVelocityPID::calculated_angular_vel); // [rpm] int duty_r = motor_r.calcDuty(torque); int duty_l = motor_l.calcDuty(torque); - if(duty_r < 0) duty_r = 0; - if(duty_l < 0) duty_l = 0; - motor_r.duty = duty_r*1.25; - motor_l.duty = duty_l; - // motor_r.Run(GPIO_PIN_RESET); - // motor_l.Run(GPIO_PIN_SET); + motor_r.duty = abs(duty_r)*MotorParam::DUTY_GAIN; + motor_l.duty = abs(duty_l); + motor_r.Run(duty_r < 0 ? GPIO_PIN_SET : GPIO_PIN_RESET); + motor_l.Run(duty_l < 0 ? GPIO_PIN_RESET : GPIO_PIN_SET); } } float CommonMotorControl::calcTorque(float target_a) { - return (MotorParam::m*target_a*MotorParam::r)/MotorParam::GEAR_RATIO; + // shishikawaさんのmotor.jsより(r*m*a)/(2*n) + // https://github.com/meganetaaan/M5Mouse/blob/master/mouse/motor.js + return (MotorParam::m*target_a*0.001*MotorParam::r)/(2*MotorParam::GEAR_RATIO); } // 車体の線形速度v[mm/s]を計算, rotation_speedはモータ（車輪ではなく）の角速度w[rad/s] @@ -75,9 +76,13 @@ Motor::Motor(TIM_HandleTypeDef &htim_x, uint16_t mode_channel, GPIO_PinState mod } float Motor::calcMotorSpeed(float calculated_linear_vel, float calculated_angular_vel){ - float vel = calculated_linear_vel + left_or_right*(MotorParam::TREAD_WIDTH/2)*calculated_angular_vel; - float rotation_speed = (vel/MotorParam::r)*MotorParam::GEAR_RATIO*60/(2*M_PI); - return rotation_speed; + // w_R = v/r + (tread/2r)*w + // v: 線形速度[m/s]、r:タイヤ半径[m]、tread: トレッド幅[m], 2:車体の角速度[rad/s] + // float vel = calculated_linear_vel*0.001 + left_or_right*(MotorParam::TREAD_WIDTH/2)*calculated_angular_vel; // [mm/s] + // float rotation_speed = (vel/MotorParam::r)*MotorParam::GEAR_RATIO*60/(2*M_PI); + float rotation_speed = (calculated_linear_vel*0.001/MotorParam::r) + left_or_right*(MotorParam::TREAD_WIDTH/(2*MotorParam::r))*calculated_angular_vel; + // ギア比を考慮 + return rotation_speed*MotorParam::GEAR_RATIO; } float Motor::linearVelocityPIDControl(float target_linear_vel, float current_linear_vel, float &pid_error_sum){ @@ -86,19 +91,28 @@ float Motor::linearVelocityPIDControl(float target_linear_vel, float current_lin if(pid_error_sum > LinearVelocityPID::MAX_PID_ERROR_SUM) { pid_error_sum = LinearVelocityPID::MAX_PID_ERROR_SUM; } + if(LinearVelocityPID::MAX_SPEED < abs(target_linear_vel + pid_error)){ + return (target_linear_vel+pid_error) > 0 ? RobotControllerParam::MAX_SPEED : 0.0; + } + if(target_linear_vel + pid_error <0) return 0.0; return target_linear_vel + pid_error; } float Motor::angularVelocityPIDControl(float target_angular_vel, float current_angular_vel, float &pid_error_sum){ float pid_error = AngularVelocityPID::Kp*(target_angular_vel - current_angular_vel)+ AngularVelocityPID::Ki*pid_error_sum; pid_error_sum += target_angular_vel - current_angular_vel; - if(pid_error_sum > AngularVelocityPID::MAX_PID_ERROR_SUM) { - pid_error_sum = AngularVelocityPID::MAX_PID_ERROR_SUM; + if(abs(pid_error_sum) > AngularVelocityPID::MAX_PID_ERROR_SUM) { + pid_error_sum = (pid_error_sum > 0 ? AngularVelocityPID::MAX_PID_ERROR_SUM : -AngularVelocityPID::MAX_PID_ERROR_SUM); + } + if(RobotControllerParam::MAX_OMEGA < abs(target_angular_vel + pid_error)){ + return target_angular_vel > 0 ? RobotControllerParam::MAX_OMEGA : -RobotControllerParam::MAX_OMEGA; } return target_angular_vel + pid_error; } int Motor::calcDuty(float torque) { + // shishikawaさんのmotor.jsより((R*tau)/Kt + Ke*omega)/Vbat + // https://github.com/meganetaaan/M5Mouse/blob/master/mouse/motor.js return 100*(MotorParam::R*torque/MotorParam::Kt + MotorParam::Ke*this->rotation_speed)/Battery::adc_bat; } diff --git a/Core/Src/robot_controller.cpp b/Core/Src/robot_controller.cpp index 90ca6d9..ff84a00 100644 --- a/Core/Src/robot_controller.cpp +++ b/Core/Src/robot_controller.cpp @@ -7,6 +7,7 @@ extern Motor motor_r; extern Log vel_log; RobotController::RobotController(){ + this->is_running = true; }; void RobotController::motorControl(float target_linear_vel, float target_angular_vel) { @@ -20,6 +21,7 @@ void RobotController::allMotorStop() { AngularVelocityPID::target_angular_vel = 0.0; motor_r.Stop(); motor_l.Stop(); + this->is_running = false; return; } @@ -47,6 +49,7 @@ void RobotController::straight(float target_distance) { // [mm] unsigned long prev_count = 0; float total_time = 0.0; this->allMotorStop(); + this->is_running = true; while (true) { unsigned long current_count = __HAL_TIM_GET_COUNTER(&htim9); @@ -54,22 +57,28 @@ void RobotController::straight(float target_distance) { // [mm] total_time += delta_time; prev_count = current_count; float diff = target_distance - LinearVelocityPID::current_distance; + AngularVelocityPID::target_angular_vel = 0.0; + AngularVelocityPID::w_pid_error_sum = 0.0; if(diff < RobotControllerParam::MIN_DISTANCE_TO_RUN) break; // 加速区間 if(LinearVelocityPID::current_distance < AL){ LinearVelocityPID::target_linear_vel += accel*delta_time; - printf("accel, diff: %lf, target_vel: %lf, delta_time: %lf\n\r", diff, LinearVelocityPID::target_linear_vel, delta_time); + // printf("accel, diff: %lf, target_vel: %lf, delta_time: %lf\n\r", diff, LinearVelocityPID::target_linear_vel, delta_time); } // 定速区間 else if (LinearVelocityPID::current_distance >= AL && LinearVelocityPID::current_distance <= (AL+CL)) { LinearVelocityPID::target_linear_vel = v_max; - printf("const, diff: %lf, target_vel: %lf, delta_time: %lf\n\r", diff, v_max, delta_time); + // printf("const, diff: %lf, target_vel: %lf, delta_time: %lf\n\r", diff, v_max, delta_time); } // 減速区間 else if (LinearVelocityPID::current_distance > (AL+CL) && LinearVelocityPID::current_distance <= (AL+CL+DL)) { LinearVelocityPID::target_linear_vel -= decel*delta_time; + if(LinearVelocityPID::target_linear_vel < LinearVelocityPID::MIN_SPEED) { + LinearVelocityPID::target_linear_vel = LinearVelocityPID::MIN_SPEED; + } printf("decel, diff: %lf, target_vel: %lf, delta_time: %lf\n\r", diff, LinearVelocityPID::target_linear_vel, delta_time); } + printf("adc_bat: %lf, calculated_linear_vel: %lf, calculated_angular_vel: %lf, motor_r.rotation_speed: %lf, motor_l.rotation_speed: %lf, motor_r.duty: %d, motor_l.duty: %d, target_angular_vel: %lf, current_angular_vel: %lf, current_deg: %d, diff: %lf\n\r", Battery::adc_bat, LinearVelocityPID::calculated_linear_vel, AngularVelocityPID::calculated_angular_vel, motor_r.rotation_speed, motor_l.rotation_speed, motor_r.duty, motor_l.duty, AngularVelocityPID::target_angular_vel, AngularVelocityPID::current_angular_vel, radToDeg(AngularVelocityPID::current_angle), diff); if(LinearVelocityPID::target_linear_vel > v_max) LinearVelocityPID::target_linear_vel = v_max; if(LinearVelocityPID::target_linear_vel < 0) LinearVelocityPID::target_linear_vel = 0.0; } @@ -79,18 +88,30 @@ void RobotController::straight(float target_distance) { // [mm] } void RobotController::turn_right(uint16_t target_deg) { + AngularVelocityPID::target_angular_vel = degToRad(-150); // [rad/s] + // LinearVelocityPID::target_linear_vel = 10; //[mm/s] float target_rad = degToRad(target_deg); float diff = 9999.0; while(diff > 0) { - diff = target_rad - AngularVelocityPID::current_angle; - printf("target_deg: %d, current_deg: %d, diff: %lf\n\r", target_deg, radToDeg(AngularVelocityPID::current_angle), diff); + diff = target_rad - abs(AngularVelocityPID::current_angle); + printf("adc_bat: %lf, calculated_linear_vel: %lf, calculated_angular_vel: %lf, motor_r.rotation_speed: %lf, motor_l.rotation_speed: %lf, motor_r.duty: %d, motor_l.duty: %d, target_deg: %d, target_angular_vel: %lf, current_angular_vel: %lf, current_deg: %d, diff: %lf\n\r", Battery::adc_bat, LinearVelocityPID::calculated_linear_vel, AngularVelocityPID::calculated_angular_vel, motor_r.rotation_speed, motor_l.rotation_speed, motor_r.duty, motor_l.duty, target_deg, AngularVelocityPID::target_angular_vel, AngularVelocityPID::current_angular_vel, radToDeg(AngularVelocityPID::current_angle), diff); } printf("Finished: %lf\n\r", diff); - // this->allMotorStop(); + this->allMotorStop(); return; } void RobotController::turn_left(uint16_t target_deg) { + AngularVelocityPID::target_angular_vel = degToRad(150); // [rad/s] + // LinearVelocityPID::target_linear_vel = 10; //[mm/s] + float target_rad = degToRad(target_deg); + float diff = 9999.0; + while(diff > 0) { + diff = target_rad - AngularVelocityPID::current_angle; + printf("adc_bat: %lf, calculated_linear_vel: %lf, calculated_angular_vel: %lf, motor_r.rotation_speed: %lf, motor_l.rotation_speed: %lf, motor_r.duty: %d, motor_l.duty: %d, target_deg: %d, target_angular_vel: %lf, current_angular_vel: %lf, current_deg: %d, diff: %lf\n\r", Battery::adc_bat, LinearVelocityPID::calculated_linear_vel, AngularVelocityPID::calculated_angular_vel, motor_r.rotation_speed, motor_l.rotation_speed, motor_r.duty, motor_l.duty, target_deg, AngularVelocityPID::target_angular_vel, AngularVelocityPID::current_angular_vel, radToDeg(AngularVelocityPID::current_angle), diff); + } + printf("Finished: %lf\n\r", diff); + this->allMotorStop(); return; } @@ -130,8 +151,10 @@ void RobotController::mainControl(){ // printf("duty_r %d, duty_l %d\n\r", motor_r.duty, motor_l.duty); // printf("cur_LinearVelocityPIDvel %lf tar_vel %lf\n\r", LinearVelocityPID::current_linear_vel, LinearVelocityPID::target_linear_vel); // printf("current_distance: %lf angle: %lf\n\r", LinearVelocityPID::current_distance, AngularVelocityPID::current_angle); - // this->straight(540); - this->turn_right(90); + if(this-is_running) this->straight(180); + // if(this->is_running) this->turn_left(360*5); + // if(this->is_running) this->turn_right(360*5); + this->is_running = false; // 目標速度のみ与える // float target_distance = 1000;