Merge pull request #9 from ShunjiHashimoto/dev/add-turn

Dev/add turn

Core/Inc/motor.hpp

@@ -19,7 +19,6 @@
 
 namespace CommonMotorControl {
     float calcCurrentLinearVel(float rotation_speed_r, float rotation_speed_l);
-    float calcCurrentAngularVel(float angular_vel);
     float calcTorque(float target_a);
     void resetTargetVelocity();
 }

Core/Inc/params.h

@@ -7,6 +7,15 @@
 
 #define SENSITIVITY_SCALE_FACTOR 16.4 
 #define GYRO_OFFSET 2.558
-#define GYRO_GAIN 0.09 // IMUが-2000~2000だったので、-180~180にするために0.90をかけた
+#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

Core/Inc/params.hpp

@@ -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 {
@@ -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;

Core/Inc/robot_controller.hpp

@@ -8,18 +8,23 @@
 #include "log.hpp"
 #include "adc.h"
 #include "photo_trans.hpp"
+#include "utils.hpp"
 
 
 // TODO:
 // pwmcontrolをここに追加する？
 
 class RobotController {
     public:
+        bool is_running;
         RobotController();
         void mainControl();
         void motorControl(float target_linear_vel, float target_angular_vel);
-        void linearRun(float distance);
+        void allMotorStop();
         void straight(float target_distance);
+        void turn_right(uint16_t target_deg);
+        void turn_left(uint16_t target_deg);
+        float calculateDeltaTime(unsigned long current_count, unsigned long& prev_count, unsigned long timer_max_count);
         float getCalculatedLinearVel();
         float getCalculatedAngularVel();
         float getCurrentLinearVel();

Core/Inc/utils.hpp

@@ -0,0 +1,11 @@
+#ifndef UTILS_HPP
+#define UTILS_HPP
+#include <cmath> // M_PIを使用するために必要
+
+// 度をラジアンに変換する関数のプロトタイプ
+float degToRad(int degree);
+
+// ラジアンを度に変換する関数のプロトタイプ
+int radToDeg(double radian);
+
+#endif // UTILS_HPP

Core/Src/gyro.c

@@ -73,7 +73,8 @@ void updateGyro(Gyro* gyro_) {
     zout_h = Gyro_ReadByte(0x37);
     zout_l = Gyro_ReadByte(0x38);
     int16_t gyro_raw = (((uint16_t)zout_h<<8)&0xff00)|zout_l;
-    gyro_->angular_vel = GYRO_OFFSET + (float)gyro_raw / SENSITIVITY_SCALE_FACTOR;
+    float gyro_gain = (gyro_raw < 0) ? GYRO_GAIN_R : GYRO_GAIN_L;
+    gyro_->angular_vel = gyro_gain*(GYRO_OFFSET + (float)gyro_raw / SENSITIVITY_SCALE_FACTOR)*M_PI/180.0;
     gyro_->yaw_deg += gyro_->angular_vel * 0.001;
 }
 

Core/Src/motor.cpp

@@ -28,27 +28,28 @@ extern "C" {
         }
         float torque = CommonMotorControl::calcTorque(LinearVelocityPID::target_a);
         LinearVelocityPID::current_linear_vel = CommonMotorControl::calcCurrentLinearVel(encoder_r.rotation_speed, encoder_l.rotation_speed); //[mm]
-        AngularVelocityPID::current_angular_vel  = CommonMotorControl::calcCurrentAngularVel(gyro.angular_vel);
         LinearVelocityPID::current_distance += LinearVelocityPID::current_linear_vel * 0.001;; // [mm/sec]*0.001[sec]
-        AngularVelocityPID::current_angle  += AngularVelocityPID::current_angular_vel * 0.001;
+        AngularVelocityPID::current_angular_vel  = gyro.angular_vel;
+        AngularVelocityPID::current_angle = gyro.yaw_deg;
 
         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]
@@ -59,10 +60,6 @@ float CommonMotorControl::calcCurrentLinearVel(float rotation_speed_r, float rot
     return 1000*((MotorParam::r*wheel_speed_r) + (MotorParam::r*wheel_speed_l))/2.0;
 }
 
-float CommonMotorControl::calcCurrentAngularVel(float angular_vel) {
-    return angular_vel*GYRO_GAIN*M_PI/180;
-}
-
 void CommonMotorControl::resetTargetVelocity() {
     LinearVelocityPID::target_linear_vel = 0.0;
     AngularVelocityPID::target_angular_vel = 0.0;
@@ -79,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){
@@ -90,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;
 }
 

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) {
@@ -15,6 +16,15 @@ void RobotController::motorControl(float target_linear_vel, float target_angular
     return;
 }
 
+void RobotController::allMotorStop() {
+    LinearVelocityPID::target_linear_vel = 0.0;
+    AngularVelocityPID::target_angular_vel = 0.0;
+    motor_r.Stop();
+    motor_l.Stop();
+    this->is_running = false;
+    return;
+}
+
 void RobotController::straight(float target_distance) { // [mm]
    float diff = target_distance - LinearVelocityPID::current_distance;
     // float v_0 = 0.0; // [mm/sec]
@@ -38,53 +48,83 @@ void RobotController::straight(float target_distance) { // [mm]
     // 1カウントあたり20μs（TIM9）, 200msごとにオーバーフロー 
     unsigned long prev_count = 0; 
     float total_time = 0.0;
-    LinearVelocityPID::target_linear_vel = 0.0;
-    AngularVelocityPID::target_angular_vel = 0.0;
-    motor_r.Stop();
-    motor_l.Stop();
+    this->allMotorStop();
+    this->is_running = true;
 
     while (true) {
-      unsigned long current_count= __HAL_TIM_GET_COUNTER(&htim9);
-      unsigned long delta_count;
-      if (current_count >= prev_count) {
-        delta_count = current_count - prev_count;
-      } else {
-        delta_count = (10000 - prev_count) + current_count; // オーバーフロー対応、10000でオーバーフロー
-      }
-      float delta_time = delta_count * 10e-6; // マイクロ秒 → 秒, 0.0125[sec]程度
+      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;
-      // printf("cur_time: %lf, last_update_time: %lf, delta_time: %lf\n\r", current_time, last_update_time, delta_time);
       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;
     }
     printf("stop\n\r");
-    LinearVelocityPID::target_linear_vel = 0.0;
-    AngularVelocityPID::target_angular_vel = 0.0;
-    motor_r.Stop();
-    motor_l.Stop();
+    this->allMotorStop();
+    return;
+}
+
+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 - 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();
+    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;
 }
 
-void RobotController::linearRun(float distance) {
-    // 指定した距離を走行する
+// タイマーのカウント値を基にデルタ時間を計算する関数
+float RobotController::calculateDeltaTime(unsigned long current_count, unsigned long& prev_count, unsigned long timer_max_count) {
+    unsigned long delta_count;
+    if (current_count >= prev_count) {
+        delta_count = current_count - prev_count;
+    } else {
+        delta_count = (timer_max_count - prev_count) + current_count; // オーバーフロー対応
+    }
+    prev_count = current_count;
+    return delta_count * 10e-6; // マイクロ秒 -> 秒
 }
 
 float RobotController::getCalculatedLinearVel() {
@@ -111,7 +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);
+      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;
@@ -122,10 +165,10 @@ void RobotController::mainControl(){
       //   motor_l.Stop();
       //   return;
       // }
-      for(int i=0; i<4; i++){
-        auto adc_val = photo_trans_sensor.getCurrentADC(i);
+      // for(int i=0; i<4; i++){
+        // auto adc_val = photo_trans_sensor.getCurrentADC(i);
         // printf("adc_val[%d]: %d\n\r", i, adc_val);
-      }
+      // }
       // printf("Finished 1 loop\n\r");
       HAL_Delay(200);
       // モード更新し、終了