※なにか気になる点がありましたらコメント欄にご記入ください。
※この記事は以下のリンクから貼られており、倒立振子という創作物に関するコードの一部を載せています。詳しくはリンク元をご覧ください。
倒立振子に挑む - Tanuki_Bayashin’s diary
使用しているマイコンは Raspberry Pi Pico と呼ばれるものです。
記述はC言語を用いています。（pico－SDK C/C++ という環境を使用しています）
【注】バグが見つかりました。修正済みです。（2023/3/31 追記）
リスト5内の35行と58行にsleep_us(100); という記述があります。モータードライバーの極性が変わるときに、回路を保護するために必要な処理ですが（デッドタイムと言う）、タイマー割り込み中にsleep_us() を使用すると、Picoがフリーズしてしまうことにたどり着きました。
ここでは、何もしない命令を入れるて100μｓ稼ぐこともできますが、考えてみたところ、このブランチに入った場合は、１0ｍｓの間 出力が0Ｖとなり、わざわざ100μｓ 休ませる必要はないので、単に削除してよいと判断しました。
ここでは、モーターに与える電圧値を求め、モーターを動かしています。その働きにより、倒立振子を倒立させようとしています。（まだ実現できていませんが・・）
手順としては、
- 最適制御の理論をもとに、倒立振子の４つの状態変数にかける係数をオフラインで計算する。
- カルマンフィルターで推定された４つの状態変数である、倒立振子本体の傾斜角、車輪の回転角、およびそれらの時間微分（角速度）の値を求める（この段階ではすでに求まっています）
- 上の２項より、モーターへ与える電圧値を計算する
- ラズベリーパイPicoにて、モータへの電圧値を、PWMの機能を用いてモータードライバへ送る。
- モーターへはモータードライバより既定の電圧値が与えられる。
ハードの面ではこのような経過を経てモーターが制御されています。
リスト５内の処理：
Cal_PWM_Command_Value() ：
モーターへの電圧値を計算し、PWMの機能を利用してモーターへ所定の電圧を出力しています。
Control_Pendulum()：
制御に関する処理をまとめて行っています。カルマンフィルタの処理と上のモーターへ電圧値を出力する処理を行っています。
send_a_3times()：
PCへuartの機能を用いて、変数の値を送信していますが、この処理ではデータの送信を開始する手順（’a’を３回送信し、その都度、赤のＬＥＤを点滅させる）を実行しています。
uart_var() ：
実際にuartの機能を用いてPCに信号を送信しています。処理の内容を変更すれば、送信するデータを変更できます。
リスト５ モーターへの電圧の出力と使い勝手に関する処理
void Cal_PWM_Command_Value() { calculate_W_angle_dot(); //reset volts = 0.0; //calculate Vin volts = K[0] * theta_data[0][0] + K[1] * P_angle_dot_ave + K[2] * W_angle[0] + K[3] * W_angle_dot; //offset if (volts > 0) { volts += V_offset_p; // 0.2 } if (volts < 0) { volts += V_offset_m; // -0.15 } //calculate PWM pulse width pwm_value = (int)(volts * 1023.0 / 3.3); //drive the motor in forward if (pwm_value > 0) { //over voltage if (pwm_value > 1023) { pwm_value = 1023; } //to protect TA7291P if (Motor_Mode == 2) { gpio_put(Motor_IN1, 0); //motor STOP gpio_put(Motor_IN2, 0); //motor STOP //sleep_us(100); //STOP MODE more than 100[μsec] } //forward gpio_put(Motor_IN1, 1); //motor CCW gpio_put(Motor_IN2, 0); //motor CCW gpio_put(LED_GREEN, 1); // LED_GREEN ON gpio_put(LED_RED, 0); Motor_Mode = 1; } //drive the motor in reverse else if (pwm_value < 0) { //calculate the absolute value pwm_value = -1 * pwm_value; //over voltage if (pwm_value > 1023) { pwm_value = 1023; } //to protect TA7291P if (Motor_Mode == 1) { gpio_put(Motor_IN1, 0); //motor STOP gpio_put(Motor_IN2, 0); //motor STOP //sleep_us(100); //STOP MODE more than 100[μsec] } gpio_put(Motor_IN1, 0); //motor CW gpio_put(Motor_IN2, 1); //motor CW gpio_put(LED_GREEN, 0); // LED_RED ON gpio_put(LED_RED, 1); Motor_Mode = 2; } else { gpio_put(LED_GREEN, 0); // LEDs OFF gpio_put(LED_RED, 0); gpio_put(Motor_IN1, 0); //motor STOP gpio_put(Motor_IN2, 0); //motor STOP Motor_Mode = 0; } //pwm width from 0 to 1023 pwm_set_chan_level(pwm0_slice_num, PWM_CHAN_A, pwm_value); //count_control++; return; } //========================================================= // calcurate the input voltage of the motor for // inverted pendulum. // (2022/12/17) //========================================================= void Control_Pendulum() { //stop theta update process //cancelled_esti = cancel_repeating_timer(&timer_esti); //--------------------------------------- //Kalman Filter (all system) //--------------------------------------- x_estimation(); // 2022/12/16 //--------------------------------------- //Motor control //--------------------------------------- //calculate Vin Cal_PWM_Command_Value(); // 2022/12/01 // prepare for the next calculation of theta2_dot pre_theta2 = y_input[2][0]; // start estimation process (2.5 ms) //add_repeating_timer_us(-2500, theta_estimation, NULL, &timer_esti); return; } void send_a_3times() { sleep_ms(100); gpio_put(LED_YELLOW, 1); // YELLOW LED is ON uart_puts(UART_ID, "a\n"); // 1st send sleep_ms(1); gpio_put(LED_RED, 1); // RED LED is ON sleep_ms(499); gpio_put(LED_RED, 0); // RED LED is OFF sleep_ms(500); uart_puts(UART_ID, "a\n"); // 2nd send sleep_ms(1); gpio_put(LED_RED, 1); // RED LED is ON sleep_ms(499); gpio_put(LED_RED, 0); // RED LED is OFF sleep_ms(500); uart_puts(UART_ID, "a\n"); // 3rd send sleep_ms(1); gpio_put(LED_RED, 1); // RED LED is ON sleep_ms(499); gpio_put(LED_RED, 0); // RED LED is OFF sleep_ms(500); // YELLOW LED is ON for 3 [sec] gpio_put(LED_YELLOW, 0); // YELLOW LED is OFF return; } void uart_var() { static char s[100]; snprintf(s, sizeof(s), "%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e\n", (float)t_end, P_angle, P_angle_dot, W_angle[0], W_angle_dot, uart_puts(UART_ID, s); return; }
