※なにか気になる点がありましたらコメント欄にご記入ください。
※この記事は以下のリンクから貼られており、倒立振子という創作物に関するコードの一部を載せています。詳しくはリンク元をご覧ください。
倒立振子に挑む - Tanuki_Bayashin’s diary
使用しているマイコンは Raspberry Pi Pico と呼ばれるものです。
記述はC言語を用いています。（pico－SDK C/C++ という環境を使用しています）
以下にコードの頭の部分を載せます。
ヘッダファイルの読み込み、マクロの定義、外部変数の定義 が続きます。
変数やマクロの種類は、
- Picoの入出力に関連するもの
- 倒立振子の動き（状態）に関するもの
- カルマンフィルタと呼ばれる処理に関するもの
- モーターに与える電圧を計算し出力するもの
- その他使い勝手に関するもの
に分けられます。
ある程度の回数で使用している変数は、外部変数としているため、ほとんどが外部変数です。
またコードの内容に関して、詳しく説明を付けると煩雑になると思うので省きます。ご容赦ください。
使用されていない変数や、バグにつながる書き方もあるかと思います。ご了承ください。
気付いた点等ありましたら、ご指摘くださるとありがたいです。
リスト１ ヘッダファイル、マクロの定義、外部変数の定義など
//========================================================= // File Name: InvertedPndulum05.c // MPU board: Raspberry Pi Pico // Accelerometer + Gyro sensor: MPU6050 // UART Device : FT231X(Akizuki) // Rotary Encoder: EC202A100A(Iwatsu Manufacturing) // Motor Driver : TA7291A(TOSHIBA) // GPIO Devices : 3 LEDs, 2 tactile switches // A/D Converter is used (on Raspberry Pi Pico) // // 2023/01/31 written by @Tanuki_Bayashin // // This Program is to Control the Inverted Pendulum System. // version 5.0 // //========================================================= #include <stdio.h> #include <string.h> #include <math.h> #include <stdlib.h> #include "pico/stdlib.h" #include "hardware/uart.h" #include "pico/binary_info.h" #include "hardware/gpio.h" #include "hardware/pwm.h" #include "hardware/adc.h" #include "hardware/i2c.h" #include "hardware/timer.h" //========================================================= // Port Setting // DigitalOut; // I2C i2c(PB_9, PB_8); //Gyro + Accelerometer (SDA, SCLK) // Serial uart_usb(USBTX, USBRX); //UART (over USB) //========================================================= #define UART_ID uart1 #define BAUD_RATE 115200 // 115200 / 8[bit] / (1/0.01[sec]) = 144 characters #define UART_TX_PIN 8 //(GPIO8 #11) #define UART_RX_PIN 9 //(GPIO9 #12) #define PI 3.1415926535898 #define ONE_G 16383 // 2^14 - 1 = 16383 #define BUTTON_BLACK 18 // 黒のタクトsw Push -> ON #define BUTTON_YELLOW 19 // 黄色のタクトsw Push -> ON #define LED_RED 20 // RED LED #define LED_GREEN 21 // GREEN LED #define LED_YELLOW 22 // YELLOW LED #define LED_PICO 25 // LED on the Pico // By default these devices are on bus address 0x68 static int addr = 0x68; // detect rotary encorder pulse #define rotary_encorder_phaseA 14 // GP14(#19 Pin) #define rotary_encorder_phaseB 15 // GP15(#20 Pin) // PWM Modules #define PIN_PWM0 12 //(GPIO12 #16Pin PWM_A[6]) // for Motor Driver IC (TA7291A TOSHIBA) #define Motor_IN1 10 //(GPIO10 #14Pin) #define Motor_IN2 11 //(GPIO11 #15Pin) //========================================================= //Accelerometer and gyro statistical data //========================================================= int16_t acceleration[3], gyro[3]; int16_t* temperature; int sample_num = 1000; float theta_mean; float theta_variance; float theta_dot_mean; float theta_dot_variance; float P_angle, P_angle_dot, P_angle_dot_ave; float P_angle_dot_data[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; //========================================================= //Accelerometer and gyro offset //========================================================= // variables for calculate offset // for offset of accelaration float acc_Y_offset = 0.0; float acc_Z_offset = 0.0; // for offset of angular velocity float gyro_X_offset = 0.0; //========================================================= //Rotary encoder variables //========================================================= int rotary_encoder_update_rate = 25; //usec int rotary_encoder_resolution = 100; int phaseA, phaseB; int code = 0; int table[16] = { 0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0 }; float pre_theta2 = 0; // for calcurate W_angle_dot (interval 10ms (not 100ms)) int encoder_value = 0; float W_angle[10]; float W_angle_dot; //========================================================= // PWM to Mortor Voltage // DATAs from a experiment //========================================================= //float a_m = 349.94, b_m = -43.1673; // 最小二乗法による近似。プラスとマイナス //float a_p = 343.34, b_p = 65.893; float a_m = 323.17, b_m = -56.55; // 最小二乗法による近似。プラスとマイナス float a_p = 334.15, b_p = 28.48; int Motor_Mode = 0; // モーターが正転(2)か逆転(1)かSTOP(0)か static pwm_config pwm0_slice_config; // not used uint pwm0_slice_num; float V_offset_m = -0.15; float V_offset_p = 0.2; float volts; int pwm_value; //========================================================= // PWM to Mortor Voltage // DATAs from a experiment //========================================================= //feed back gain float K[4] = { 28.37445212, 4.36987912, 0.13194284, 0.36487902 }; int count_esti = 0, count_encoder = 0, count_control = 0; //========================================================= //Gain vector for the state feedback //(R=1000, Q = diag(1, 1, 10, 10), f=100Hz) //float Gain[4] = { 29.87522919, 4.59857246, 0.09293, 0.37006248 }; float Gain[4] = { 32.17318922, 4.96845674, 0.29363568, 0.4401379 }; //========================================================= //Kalman filter (for P_angle estimation) variables //========================================================= //Update rate int theta_update_freq = 400; //Hz float theta_update_interval = 0.0025; // (1.0 / (float)theta_update_freq //State vector //[[theta(degree)], [offset of theta_dot(degree/sec)]] //float theta_data_predict[2][1]; float theta_data_predict[2][1] = { {1},{1} }; float theta_data[2][1]; //Covariance matrix float P_theta_predict[2][2]; float P_theta[2][2]; //"A" of the state equation float A_theta[2][2] = { {1, -0.0025}, {0, 1} }; // 0.0025: theta_update_interval //"B" of the state equation float B_theta[2][1] = { {0.0025}, {0} }; // 0.0025: theta_update_interval //"C" of the state equation float C_theta[1][2] = { {1, 0} }; //========================================================= //Kalman filter (for all system estimation) variables //State vector //[[theta1(rad)], [theta1_dot(rad/s)], [theta2(rad)]. [theta2_dot(rad/s)]] float x_data_predict[4][1]; float x_data[4][1]; //Covariance matrix float P_x_predict[4][4]; float P_x[4][4]; //"A" of the state equation (update freq = 100 Hz) // for @Tanuki_Bayashin version float A_x[4][4] = { { 1.00221, 1.00074e-02, 0.0, 4.74452e-05}, { 4.42472e-01, 1.00221, 0.0, 9.40068e-03}, {-1.20179e-03,-4.02488e-06, 1.0, 9.71225e-03}, {-2.38120e-01,-1.20179e-03, 0.0, 9.43005e-01} }; //"B" of the state equation (update freq = 100 Hz) // for @Tanuki_Bayashin version float B_x[4][1] = { {-3.05075e-04}, {-6.04468e-02}, { 1.85024e-03}, { 3.66477e-01} }; //"C" of the state equation (update freq = 100 Hz) float C_x[4][4] = { {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1} }; //measurement noise float measure_variance_mat[4][4]; //System noise #define voltage_error 0.01 float voltage_variance = voltage_error * voltage_error; //========================================================= //Motor control variables float feedback_rate = 0.01; //sec struct repeating_timer timer_esti; struct repeating_timer timer_encorder; struct repeating_timer timer_control; bool cancelled_esti; bool cancelled_encorder; bool cancelled_control; //static uint32_t t_start = 0, t_end = 0; static uint32_t t_start = 0, t_end = 0; //========================================================= //variable for measurement data //Kalman filter (all system) variables //========================================================= float y_input[4][1]; float deg_rad_coeff = (3.14 * 3.14) / (180 * 180); float encoder_error = 0.1 * 2 * 3.14 / 400.0; float encoder_rate_error = 0.1 * 2 * 3.14 / 400.0 / 0.01;
