gyroから角速度を計算、変数名の整理などのリファクタリング

Core/Inc/encoder.h

@@ -13,7 +13,7 @@ typedef struct {
     int32_t one_rotation_pulse;
     uint32_t initial_pulse_count;
     float rad_per_rotation;
-    float motor_vel;
+    float rotation_speed;
     bool forward_wise;
 } Encoder;
 

Core/Inc/gyro.h

@@ -0,0 +1,22 @@
+#ifndef GYRO_H
+#define GYRO_H
+
+#include "../../Core/Inc/main.h"
+#include "stm32f4xx_hal.h"
+#include <stdint.h>
+#include <spi.h>
+#include "usart.h"
+#include "params.h"
+
+typedef struct {
+    float ang_vel;
+    float yaw_deg;
+} Gyro;
+
+uint8_t Gyro_ReadByte(uint8_t reg);
+void Gyro_WriteByte(uint8_t reg, uint8_t data);
+void Gyro_Init(Gyro* gyro_);
+void Gyro_Update(Gyro* gyro_);
+extern Gyro gyro;
+
+#endif /* GYRO_H */

Core/Inc/main.h

@@ -35,6 +35,7 @@ extern "C" {
 #include <string.h>
 #include <stdbool.h>
 #include "tim.h"
+#include "math.h"
 /* USER CODE END Includes */
 
 /* Exported types ------------------------------------------------------------*/

Core/Inc/motor.hpp

@@ -9,7 +9,6 @@
 #define INC_DRIVERS_MOTOR_HPP_
 #include "main.h"
 #include "params.hpp"
-#include "timers.hpp"
 #include "tim.h"
 
 class Motor {
@@ -23,9 +22,9 @@ class Motor {
     */
     Motor(TIM_HandleTypeDef &htim_x, uint16_t mode_channel, GPIO_PinState mode, uint16_t direction_channel, uint16_t duty_channel, int16_t left_or_right);
 
-    float calcMotorSpeed(float linear_vel, float angular_vel);
-    static float linearVelocityPIDControl(float target_v, float current_v, float &pid_error_sum);
-    static float angularVelocityPIDControl(float target_w, float current_w, float &pid_error_sum);
+    float calcMotorSpeed(float calculated_linear_vel, float calculated_angular_vel);
+    static float linearVelocityPIDControl(float target_linear_vel, float current_linear_vel, float &pid_error_sum);
+    static float angularVelocityPIDControl(float target_angular_vel, float current_angular_vel, float &pid_error_sum);
 
     /**
      * @brief モーター制御関数

Core/Inc/params.h

@@ -0,0 +1,11 @@
+#ifndef PARAMS_H
+#define PARAMS_H
+
+#define WHO_AM_I 0x00
+#define PWR_MGMT_1 0x06
+#define GYRO_CONFIG_1 0x01
+
+#define SENSITIVITY_SCALE_FACTOR 16.4 
+#define GYRO_OFFSET = 2.558
+
+#endif

Core/Inc/params.hpp

@@ -1,14 +1,13 @@
-// motor_params.h
-#ifndef MOTOR_PARAMS_H
-#define MOTOR_PARAMS_H
+#ifndef PARAMS_HPP
+#define PARAMS_HPP
 
 namespace MotorParam {
     const float Ke = 0.207/1000.0; // 逆起電圧定数[V/min^-1]
     const float Kt = 1.98/1000.0;   // トルク定数[Nm/A]
     const float R = 1.07;         // 巻線抵抗[Ω]
     const float GEAR_RATIO = 43.0/13.0;        // ギア比
     const float m = 90.0/1000.0;
-    const float r = 11.5*0.001/2;           // タイヤ半径[m]
+    const float r = 11.0*0.001/2;           // タイヤ半径[m]
     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)
@@ -17,18 +16,18 @@ namespace MotorParam {
 }
 
 namespace LinearVelocityPID {
-    const float Kp = 0.1;
-    const float Ki = 0.1;
+    const float Kp = 1.0;
+    const float Ki = 0.8;
     const float Kd = 0.0;
     const uint16_t MAX_PID_ERROR_SUM = 10;
     const int16_t MIN_PID_ERROR_SUM = 0;
 }
 
 namespace AngularVelocityPID {
-    const float Kp = 0.1;
+    const float Kp = 1.0;
     const float Ki = 0.01;
     const float Kd = 0.0;
     const uint16_t MAX_PID_ERROR_SUM = 10;
     const int16_t MIN_PID_ERROR_SUM = 0;
 }
-#endif // MOTOR_PARAMS_H
+#endif // PARAMS_HPP

Core/Src/encoder.c

@@ -7,7 +7,7 @@ void Encoder_Init(Encoder* encoder, TIM_HandleTypeDef* htim_x, int32_t one_rotat
     encoder->forward_wise = cw;
     encoder->one_rotation_pulse = one_rotation_pulse;
     encoder->rad_per_rotation = 2*M_PI / one_rotation_pulse;
-    encoder->motor_vel = 0.0; // [rad/s]
+    encoder->rotation_speed = 0.0; // [rad/s]
     encoder->initial_pulse_count = (__HAL_TIM_GET_AUTORELOAD(htim_x)+1)/2; // 必要に応じて初期値を設定
     __HAL_TIM_SET_COUNTER(encoder->htim_x, encoder->initial_pulse_count); // タイマをリセット
     HAL_TIM_Encoder_Start_IT(encoder->htim_x, TIM_CHANNEL_ALL); // 左車輪のエンコーダのカウントスタート
@@ -16,7 +16,7 @@ void Encoder_Init(Encoder* encoder, TIM_HandleTypeDef* htim_x, int32_t one_rotat
 void Encoder_Update(Encoder* encoder, uint32_t cur_pulse) {
     encoder->delta_pulse = -encoder->initial_pulse_count + cur_pulse;
     encoder->total_pulse += (encoder->forward_wise ? -encoder->delta_pulse : encoder->delta_pulse);
-    encoder->motor_vel = (encoder->forward_wise ? 
+    encoder->rotation_speed = (encoder->forward_wise ? 
     (-encoder->delta_pulse * encoder->rad_per_rotation)/0.001: (encoder->delta_pulse * encoder->rad_per_rotation)/0.001);
 }
 

Core/Src/gyro.cpp → Core/Src/gyro.c

@@ -1,17 +1,12 @@
 /*
- * gyro.cpp
+ * gyro.c
  *
  *  Created on: 2024/04/27
  *      Author: hashimoto
  */
-#include "../../Core/Inc/gyro.hpp"
+#include "gyro.h"
 
-int __io_putchar(int ch) {
-    HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 100);
-    return ch;
-}
-
-uint8_t Gyro::readByte(uint8_t reg)
+uint8_t Gyro_ReadByte(uint8_t reg)
 {
   uint8_t rx_data[2];
   uint8_t tx_data[2];
@@ -26,7 +21,7 @@ uint8_t Gyro::readByte(uint8_t reg)
   return rx_data[1];
 }
 
-void Gyro::writeByte(uint8_t reg, uint8_t data)
+void Gyro_WriteByte(uint8_t reg, uint8_t data)
 {
   uint8_t rx_data[2];
   uint8_t tx_data[2];
@@ -39,15 +34,17 @@ void Gyro::writeByte(uint8_t reg, uint8_t data)
   HAL_GPIO_WritePin(SPI2_CS_GPIO_Port, SPI2_CS_Pin, GPIO_PIN_SET);
 }
 
-void Gyro::init() {
+void Gyro_Init(Gyro* gyro_) {
+    gyro_->ang_vel = 0.0;
+    gyro_->yaw_deg = 0.0;
     uint8_t who_am_i;
 
     HAL_Delay( 100 ); // wait start up
-    who_am_i = readByte( WHO_AM_I ); // 1. read who am i 
+    who_am_i = Gyro_ReadByte( WHO_AM_I ); // 1. read who am i 
     printf( " rn0x%xrn\n\r", who_am_i ); // 2. check who am i value, 0xE0が帰ってくる
     // 初回に失敗するときがあるので、もう一度動かしてみる
     HAL_Delay( 100 );
-    who_am_i = readByte( WHO_AM_I );  
+    who_am_i = Gyro_ReadByte( WHO_AM_I );  
     printf( " rn0x%xrn\n\r", who_am_i );
 
     // 2. error check
@@ -61,26 +58,22 @@ void Gyro::init() {
     // sleep 状態から起こす、clearだから0
     // デフォルトでは省電力モード、今回は20Mhzで動かす、0で設定する
     // device resetも1で設定する、右側がかいビット、左が上位ビット
-    writeByte(PWR_MGMT_1, 0x80); // 1000 0000 
+    Gyro_WriteByte(PWR_MGMT_1, 0x80); // 1000 0000 
     HAL_Delay(50);
-    writeByte(PWR_MGMT_1, 0x01); // 0000 0001
+    Gyro_WriteByte(PWR_MGMT_1, 0x01); // 0000 0001
     HAL_Delay(50);
     // GYRO_FS_SEL: 設定する必要あり、どこの速度まで角度取るかの設定、11（フルスケール）、旋回速度が遅いものは下げる必要
-    writeByte(GYRO_CONFIG_1, 0x06); // 0000 0110
-    HAL_Delay( 50 );
+    Gyro_WriteByte(GYRO_CONFIG_1, 0x06); // 0000 0110
+    HAL_Delay(100);
 }
 
-void Gyro::update() {
-    HAL_Delay(100);
+void Gyro_Update(Gyro* gyro_) {
+    // HAL_Delay(100);
     uint8_t zout_h, zout_l;
-    zout_h = readByte(0x37);
-    zout_l = readByte(0x38);
+    zout_h = Gyro_ReadByte(0x37);
+    zout_l = Gyro_ReadByte(0x38);
     int16_t gyro_raw = (((uint16_t)zout_h<<8)&0xff00)|zout_l;
-    float gyro_ang_vel = (float)gyro_raw / 16.4;
-    printf(" gyro raw %f\n\r", gyro_ang_vel);
-    // ジャイロのセンサはオフセットを設定する
-    // 0x37のアドレス、(指定しないまま送ると次のレジスタが送られる)
-    // 0x37 0x00 0x00
-    // Hamstarのコードも同じimuを使っているから参考に出来るはず
+    gyro_->ang_vel = GYRO_OFFSET + (float)gyro_raw / SENSITIVITY_SCALE_FACTOR;
+    gyro_->yaw_deg += gyro_->ang_vel * 0.001;
 }
 

Core/Src/main.cpp

@@ -28,11 +28,10 @@
 /* USER CODE BEGIN Includes */
 extern "C" {
   #include "encoder.h"
+  #include "gyro.h"
 }
 #include "led.hpp"
-#include "timers.hpp"
 #include "motor.hpp"
-#include "gyro.hpp"
 #include "params.hpp"
 
 /* USER CODE END Includes */
@@ -86,8 +85,7 @@ int main(void)
 {
   /* USER CODE BEGIN 1 */
   LedBlink ledBlink;
-  Gyro gyro;
-  Timers timers;
+  // Gyro gyro;
   // 参照渡しの場合は、htim1は直接渡す
   Motor motor_l(htim1, Motor_Mode_Pin, GPIO_PIN_SET, MotorL_TIM1_CH1_Pin, TIM_CHANNEL_2, -1);
   Motor motor_r(htim1, Motor_Mode_Pin, GPIO_PIN_SET, MotorR_TIM1_CH3_Pin, TIM_CHANNEL_4, 1);
@@ -122,46 +120,59 @@ int main(void)
   MX_ADC2_Init();
   /* USER CODE BEGIN 2 */
   setbuf(stdout, NULL); // std::outのバッファリングを無効にし、ログを即出力する
-  gyro.init();
+  Gyro_Init(&gyro);
   encoder_init();
-  float adc_bat = 0.0;
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
-  float target_vel = 1.0; // [m/s]
-  float target_w = 0.0;
+  uint8_t sw0_state = 1; 
+  float adc_bat = 0.0;
+  float target_linear_vel = 0.01; // [m/s]
+  float target_angular_vel = 0.5;
   float a = 18;
-  float w_r = motor_r.calcMotorSpeed(target_vel, target_w); // [rpm]
-  float w_l = motor_l.calcMotorSpeed(target_vel, target_w); // [rpm]
   float torque = (MotorParam::m*a*MotorParam::r)/MotorParam::GEAR_RATIO;
   float vel_pid_error_sum = 0.0;
   float w_pid_error_sum = 0.0;
   while (1){
+    // Lチカ
     ledBlink.toggle();
-    HAL_Delay(MotorParam::RATE);
+    // IMU
+    // gyro.update();
+    // sw状態の確認
+    if(sw0_state != 0) sw0_state = HAL_GPIO_ReadPin(SW0_GPIO_Port,SW0_Pin);
+    // バッテリー電圧の取得
     HAL_ADC_Start(&hadc2);
     HAL_ADC_PollForConversion(&hadc2, 1000);
     adc_bat = HAL_ADC_GetValue(&hadc2) * MotorParam::BAT_RATIO;
-    float current_v = (encoder_r.motor_vel*MotorParam::r + encoder_l.motor_vel*MotorParam::r)/2;
-    float current_w = (encoder_r.motor_vel*MotorParam::r + encoder_l.motor_vel*MotorParam::r)/MotorParam::TREAD_WIDTH;
-    float linear_vel = Motor::linearVelocityPIDControl(target_vel, current_v, vel_pid_error_sum);
-    float angular_vel = Motor::angularVelocityPIDControl(target_w, current_w, w_pid_error_sum);
-    printf("pid_error_sum %lf , w %lf\n\r", vel_pid_error_sum, w_pid_error_sum);
-    float duty_r = 100*(MotorParam::R*torque/MotorParam::Kt + MotorParam::Ke*w_r)/adc_bat;
-    float duty_l = 100*(MotorParam::R*torque/MotorParam::Kt + MotorParam::Ke*w_l)/adc_bat;
-
-    printf("Battery Voltage: %lf\n\r", adc_bat); 
-    printf("Encoder Value: l:%d r:%d, Rotation Num: l: %d r:%d, Rotation Vel: l: %lf r: %lf\n\r",
-            encoder_l.total_pulse, encoder_r.total_pulse, 
-            Encoder_GetRotationCount(&encoder_l), Encoder_GetRotationCount(&encoder_r),
-            encoder_l.motor_vel, encoder_r.motor_vel);
-    // printf("w_r: %lf, w_l: %lf, torque: %lf, duty: r: %lf, l: %lf\n\r", 
-    //         w_r, w_l, torque, duty_r, duty_l);
-    printf("cur_v %lf tar_v %lf   , cur_w %lf tar_w %lf\n\r", current_v, linear_vel, current_w, angular_vel);
-
-    motor_r.run(GPIO_PIN_RESET, duty_r);
-    motor_l.run(GPIO_PIN_SET, duty_l); 
+    if(sw0_state == 0) {
+      // 速度制御
+      float current_linear_vel     = (MotorParam::r*(encoder_r.rotation_speed) + MotorParam::r*(encoder_l.rotation_speed))/2.0;
+      float current_angular_vel    = gyro.ang_vel*0.09*M_PI/180;
+      float calculated_linear_vel  = Motor::linearVelocityPIDControl(target_linear_vel, current_linear_vel, vel_pid_error_sum);
+      float calculated_angular_vel = Motor::angularVelocityPIDControl(target_angular_vel, current_angular_vel, w_pid_error_sum);
+      float rotation_speed_r       = motor_r.calcMotorSpeed(calculated_linear_vel, calculated_angular_vel); // [rpm]
+      float rotation_speed_l       = motor_l.calcMotorSpeed(calculated_linear_vel, calculated_angular_vel); // [rpm]
+      float duty_r = 100*(MotorParam::R*torque/MotorParam::Kt + MotorParam::Ke*rotation_speed_r)/adc_bat;
+      float duty_l = 100*(MotorParam::R*torque/MotorParam::Kt + MotorParam::Ke*rotation_speed_l)/adc_bat;
+      motor_r.run(GPIO_PIN_RESET, duty_r);
+      motor_l.run(GPIO_PIN_SET, duty_l); 
+      // ログ
+      // printf("pid_error_sum %lf , w %lf\n\r", vel_pid_error_sum, w_pid_error_sum);
+      // printf("Battery Voltage: %lf\n\r", adc_bat); 
+      // printf("delta %d, Rotation Num: l: %d r:%d, Rotation Vel: l: %lf r: %lf\n\r",
+      //         encoder_r.delta_pulse, Encoder_GetRotationCount(&encoder_l), Encoder_GetRotationCount(&encoder_r),
+      //         encoder_l.rotation_speed, encoder_r.rotation_speed);
+      // printf("rotation_speed_r: %lf, rotation_speed_l: %lf, torque: %lf, duty: r: %lf, l: %lf\n\r", 
+      //         rotation_speed_r, rotation_speed_l, torque, duty_r, duty_l);
+      // TODO IMUを使ったPID制御の実装とリファクタリングとバッテリーケーブルの修理
+      // メモリスト、C言語とC++で参照渡しが異なる。エンコーダ、IMUのグローバル変数の見直し
+      // IMUが-2000~2000だったので、-180~180にするために0.90をかけた
+      // PID制御の+pid_error, I成分はtarget-currentで、pid_errorを足し合わせるものではない
+      printf("cur_vel %lf tar_vel %lf , cur_w %lf tar_w %lf\n\r", current_linear_vel, calculated_linear_vel, current_angular_vel, calculated_angular_vel);
+    }
+    // printf("gyro vel: %lf yaw deg: %lf\n\r", gyro.ang_vel*0.09*M_PI/180, gyro.yaw_deg*0.09);
+    HAL_Delay(MotorParam::RATE);
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */