Skip to content

Commit

Permalink
tunr_rightとstaright関数完成
Browse files Browse the repository at this point in the history
  • Loading branch information
@ShunjiHashimoto
ShunjiHashimoto committed Dec 21, 2024
1 parent 3df6400 commit 7f74fa2
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 28 deletions.
12 changes: 10 additions & 2 deletions Core/Inc/params.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
26 changes: 19 additions & 7 deletions Core/Inc/params.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,27 @@
#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;
const float BAT_RATIO = (25.0/10.0)*(stm32_vat/bit); // 電圧倍率(分圧)*（12bit=4096/3.3V)
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 {
Expand All @@ -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 {
Expand All @@ -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;
Expand All @@ -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;
Expand Down
1 change: 1 addition & 0 deletions Core/Inc/robot_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

class RobotController {
public:
bool is_running;
RobotController();
void mainControl();
void motorControl(float target_linear_vel, float target_angular_vel);
Expand Down
38 changes: 26 additions & 12 deletions Core/Src/motor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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){
Expand All @@ -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;
}

Expand Down
37 changes: 30 additions & 7 deletions Core/Src/robot_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -20,6 +21,7 @@ void RobotController::allMotorStop() {
AngularVelocityPID::target_angular_vel = 0.0;
motor_r.Stop();
motor_l.Stop();
this->is_running = false;
return;
}

Expand Down Expand Up @@ -47,29 +49,36 @@ 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);
float delta_time = calculateDeltaTime(current_count, prev_count, 10000); // タイマーの最大カウント値を10000と仮定
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;
}
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 7f74fa2

Please sign in to comment.