※なにか気になる点がありましたらコメント欄にご記入ください。
※この記事は以下のリンクから貼られており、倒立振子という創作物に関するコードの一部を載せています。詳しくはリンク元をご覧ください。
倒立振子に挑む - Tanuki_Bayashin’s diary
使用しているマイコンは Raspberry Pi Pico と呼ばれるものです。
記述はC言語を用いています。（pico－SDK C/C++ という環境を使用しています）
ここでは４つの変数、本体の傾斜角[rad]、その角速度[rad/s]、車輪の回転角[rad]、その角速度[rad/s] を測定します。
本体の傾斜角の測定には加速度センサー（MPU6050）を用います。傾斜角の測定に関しては、以下のリンクを参照してください。（大元はトラ技2019年７月号の特集に依っています）
またMPU6050に関する処理は、運よくPico-SDKのサンプルプログラムのなかに載っていました。
傾斜計の試作機の製作 - Tanuki_Bayashin’s diary
本体の傾斜角の角速度は、同じく加速度センサー（MPU6050）を用いています。
車輪の回転角の測定は、車輪に取り付けてあるロータリーエンコーダーからの値から測定しました。A相とB相という２種類の信号をうまく使うことにより、0.9°までの精度で測定することができます。
（こちらも大元はトラ技2019年７月号の特集に依っています）
車輪の回転角の角速度は、制御周期ごとにサンプリングしたデータから、引き算を行い求めました。
リスト３ 状態変数の測定と平均、分散の測定
//========================================================= // Accelerometer (MPU6050) // Gyroscope //========================================================= // int16_t acceleration[3], gyro[3]; // int16_t* temperature; // //get data //float theta_mean; //float theta_variance; //float theta_dot_mean; //float theta_dot_variance; // 初期化しています #ifdef i2c_default static void mpu6050_reset() { // Two byte reset. First byte register, second byte data // There are a load more options to set up the device in different ways that could be added here uint8_t buf[] = { 0x6B, 0x00 }; i2c_write_blocking(i2c_default, addr, buf, 2, false); // set P_angle_dot ±500[degree/sec] buf[0] = 0x1B; buf[1] = 0x08; // 0x01 => 0x08 changed(2022/08/11) i2c_write_blocking(i2c_default, addr, buf, 2, false); // set accelaration ±2G buf[0] = 0x1C; buf[1] = 0x00; i2c_write_blocking(i2c_default, addr, buf, 2, false); // set LPF BW 260Hz(0.0ms delay) for Accel, // 256Hz(0.98ms delay) for Gyro buf[0] = 0x1A; buf[1] = 0x00; i2c_write_blocking(i2c_default, addr, buf, 2, false); } // 本体の傾斜角を測定しています float get_P_angle() { uint8_t buffer[4]; // Start reading acceleration registers from register 0x3B for 6 bytes uint8_t val = 0x3D; // true to keep master control of bus i2c_write_blocking(i2c_default, addr, &val, 1, true); i2c_read_blocking(i2c_default, addr, buffer, 4, false); acceleration[1] = (buffer[0] << 8) | buffer[1]; acceleration[2] = (buffer[2] << 8) | buffer[3]; // 86.03 is the value of compensation for initial posture P_angle = 90.0 - atan2((double)acceleration[1] - acc_Y_offset, (double)acceleration[2] - acc_Z_offset) * 180.0 / PI; return P_angle; // [degree] } // 傾斜角の角速度を測定しています float get_P_angle_dot() { uint8_t buffer[2]; float sum; //float P_angle_dot; // Now gyro data from reg 0x43 for 6 bytes uint8_t val = 0x43; i2c_write_blocking(i2c_default, addr, &val, 1, true); i2c_read_blocking(i2c_default, addr, buffer, 2, false); // False - finished with bus gyro[0] = (buffer[0] << 8) | buffer[1]; // 2^15 - 1 = 32767, ±500 [degree/s] for Full range // -500を掛けているのは座標系を合わせるため P_angle_dot = (gyro[0] - gyro_X_offset) * (-500.0) / 32767.0 - theta_data[1][0]; // [degree/s] for (int i = 0; i < 9; i++) { P_angle_dot_data[9 - i] = P_angle_dot_data[8 - i]; } P_angle_dot_data[0] = P_angle_dot; sum = 0.0; for (int i = 0; i < 10; i++) { sum += P_angle_dot_data[i]; } // 角速度の平均を求めています P_angle_dot_ave = sum / 10.0; return P_angle_dot; } #endif // 各値にはオフセットが含まれています。その値を測定します。 // また、各角度や角速度の平均や分散を求めています。 // これらはカルマンフィルタを用いる上で必要となってきます。 void Calibration() { //// Calculate these variables // acc_Y_offset; // acc_Z_offset; // gyro_X_offset; // // theta_mean; // theta_variance; // theta_dot_mean; // theta_dot_variance; //float P_angle, P_angle_dot; float acc_Y_sum = 0.0; float acc_Z_sum = 0.0; float gyro_X_sum = 0.0; float P_angle_sum = 0.0; float P_angle_dot_sum = 0.0; float temp_std; float ONE_G_sin, ONE_G_cos, P_angle_0; //P_angle_0 = 86.03; P_angle_0 = 78.0; //P_angle_0 = 90.0; ONE_G_sin = ONE_G * sin((90.0 - P_angle_0) * PI / 180.0); ONE_G_cos = ONE_G * cos((90.0 - P_angle_0) * PI / 180.0); /////////////////////////////////////////////// // Calculate offset of acc and gyro raw data // /////////////////////////////////////////////// for (int i = 0; i < sample_num; i++) { P_angle = get_P_angle(); P_angle_dot = get_P_angle_dot(); // #define ONE_G 16383 = 2^14 - 1 acc_Y_sum += acceleration[1] - ONE_G_sin; acc_Z_sum += acceleration[2] - ONE_G_cos; gyro_X_sum += gyro[0]; // delay 100[μs] sleep_us(500); } // results of calculation of mean values acc_Y_offset = acc_Y_sum / sample_num; acc_Z_offset = acc_Z_sum / sample_num; gyro_X_offset = gyro_X_sum / sample_num; /////////////////////////////////////// // Calculate mean value of variables // /////////////////////////////////////// P_angle_sum = 0.0; P_angle_dot_sum = 0.0; for (int i = 0; i < sample_num; i++) { P_angle_sum += get_P_angle() - P_angle_0; P_angle_dot_sum += get_P_angle_dot(); // delay 100[μs] sleep_us(500); } theta_mean = P_angle_sum / sample_num; // [degree] theta_dot_mean = P_angle_dot_sum / sample_num; // [degree/s] /////////////////////////////////////////// // Calculate variance value of variables // /////////////////////////////////////////// P_angle_sum = 0.0; P_angle_dot_sum = 0.0; for (int i = 0; i < sample_num; i++) { temp_std = get_P_angle() - P_angle_0 - theta_mean; P_angle_sum += temp_std * temp_std; temp_std = get_P_angle_dot() - theta_dot_mean; P_angle_dot_sum += temp_std * temp_std; // delay 100[μs] sleep_us(500); } theta_variance = P_angle_sum / sample_num; // [degree]^2 theta_dot_variance = P_angle_dot_sum / sample_num; // [degree/s]^2 return; } // ロータリーエンコーダーに関する処理です //========================================================= //Rotary encoder polling function //It takes 4usec. (NUCLEO-F401RE 84MHz) // under 1usec. (Raspberry Pi Pico 125MHz) //========================================================= int read_encoder() { phaseA = gpio_get(rotary_encorder_phaseA); phaseB = gpio_get(rotary_encorder_phaseB); return (phaseB << 1) + phaseA; } bool rotary_encoder_check(struct repeating_timer* t) { //t_start = time_us_32(); int temp; temp = read_encoder(); //check the movement code = ((code << 2) + temp) & 0xf; //update the encoder value int value = 1 * table[code]; encoder_value += value; //encoder_value = 0; //count_encoder++; //t_end = time_us_32() - t_start; return true; } // 車輪の角度の値から、角速度を求めています void calculate_W_angle_dot() // angular velocity of wheels { float temp; // calculate W_angle [degree] temp = (float)encoder_value / 1074.0 * 360.0; // calculate W_angle_dot [degree / s] W_angle_dot = (temp - W_angle[9]) * 10.0; // 掛け算の方が早い( / 0.1[sec]) W_angle[9] = W_angle[8]; // next 10 step old W_angle[8] = W_angle[7]; // next 9 step old W_angle[7] = W_angle[6]; // next 8 step old W_angle[6] = W_angle[5]; // next 7 step old W_angle[5] = W_angle[4]; // next 6 step old W_angle[4] = W_angle[3]; // next 5 step old W_angle[3] = W_angle[2]; // next 4 step old W_angle[2] = W_angle[1]; // next 3 step old W_angle[1] = W_angle[0]; // next 2 step old W_angle[0] = temp; // next 1 step old return; }
