※なにか気になる点がありましたらコメント欄にご記入ください。
※この記事は以下のリンクから貼られており、倒立振子という創作物に関するコードの一部を載せています。詳しくはリンク元をご覧ください。
倒立振子に挑む - Tanuki_Bayashin’s diary
使用しているマイコンは Raspberry Pi Pico と呼ばれるものです。
記述はC言語を用いています。（pico－SDK C/C++ という環境を使用しています）
ここでは、main() 関数内の処理について説明をします。
初めは、各種機能の初期化を行っています。
手順が進んで、制御のメインループに入ります。
メインループの手前で、ロータリーエンコーダーの処理（25μｓ置きに呼び出される）と、カルマンフィルタの推定の処理（２．５ｍｓ置きに呼び出される）をタイマー割り込みに設定しています。
メインループ内では、センサーから値を取り込み、処理を行い、ＰＷＭによりモーターへ所定の電圧を与えています。
リスト６ main() 関数内の処理
//========================================================= // Main //========================================================= void main() { static char s[100]; int i, j, count; bool start_control = false; int led_yellow = 0; int count_led_yellow = 0; int flag = 0; ///////////////////////// // UART 初期設定 // ///////////////////////// // process something important ? stdio_init_all(); // Set up our UART with the required speed. uart_init(UART_ID, BAUD_RATE); // Set the TX and RX pins by using the function select on the GPIO // Set datasheet for more information on function select gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART); gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART); // Use some the various UART functions to send out data // In a default system, printf will also output via the default UART ///////////////////////////// // 加速度センサー 初期設定 // ///////////////////////////// #if !defined(i2c_default) || !defined(PICO_DEFAULT_I2C_SDA_PIN) || !defined(PICO_DEFAULT_I2C_SCL_PIN) #warning i2c / mpu6050_i2c example requires a board with I2C pins //uart_puts(UART_ID, "Default I2C pins were not defined"); #else //uart_puts(UART_ID, "Hello, MPU6050! Reading raw data from registers..."); // This example will use I2C0 on the default SDA and SCL pins (4, 5 on a Pico) i2c_init(i2c_default, 400 * 1000); gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C); gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C); gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN); gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN); // Make the I2C pins available to picotool bi_decl(bi_2pins_with_func(PICO_DEFAULT_I2C_SDA_PIN, PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C)); mpu6050_reset(); #endif ///////////////////////////////////////// // GPIO(for LEDs and BUTTONs) initialize ///////////////////////////////////////// gpio_init(LED_RED); gpio_init(LED_GREEN); gpio_init(LED_YELLOW); gpio_set_dir(LED_RED, GPIO_OUT); gpio_set_dir(LED_GREEN, GPIO_OUT); gpio_set_dir(LED_YELLOW, GPIO_OUT); gpio_init(LED_PICO); gpio_set_dir(LED_PICO, GPIO_OUT); gpio_init(BUTTON_BLACK); gpio_set_dir(BUTTON_BLACK, GPIO_IN); //gpio_pull_up(BUTTON_BLACK); gpio_pull_down(BUTTON_BLACK); gpio_init(BUTTON_YELLOW); gpio_set_dir(BUTTON_YELLOW, GPIO_IN); //gpio_pull_up(BUTTON_YELLOW); gpio_pull_down(BUTTON_YELLOW); // add this statement (2022/12/25) sleep_ms(1); /////////////////////////////////// // Calibration Start /////////////////////////////////// gpio_put(LED_YELLOW, 1); // YELLOW LED is ON sleep_ms(500); gpio_put(LED_YELLOW, 0); // YELLOW LED is OFF sleep_ms(500); gpio_put(LED_YELLOW, 1); // YELLOW LED is ON Calibration(); snprintf(s, sizeof(s), "%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e,%1.3e\n", acc_Y_offset, acc_Z_offset, gyro_X_offset, theta_mean, theta_variance, theta_dot_mean, theta_dot_variance); uart_puts(UART_ID, s); gpio_put(LED_YELLOW, 0); // YELLOW LED is OFF sleep_ms(500); count = 0; led_yellow = 0; count_led_yellow = 0; // While start_control is false,this while loop is running. // and pushing Yellow Button for 1 minute continueously, // start_control turned to true and get out from this loop! /* while (start_control == false) { if ((gpio_get(BUTTON_YELLOW) == 1) || (gpio_get(BUTTON_BLACK) == 1)) { gpio_put(LED_GREEN, 1); gpio_put(LED_RED, 0); sleep_ms(1); } else { gpio_put(LED_GREEN, 0); gpio_put(LED_RED, 1); sleep_ms(1); } } */ while (start_control == false) { if (gpio_get(BUTTON_YELLOW) == 1) { sleep_ms(1); count++; gpio_put(LED_GREEN, 1); gpio_put(LED_RED, 0); if (count > 1000) { start_control = true; gpio_put(LED_GREEN, 0); gpio_put(LED_RED, 0); break; } } else { count = 0; gpio_put(LED_GREEN, 0); gpio_put(LED_RED, 1); } if (count_led_yellow % 250 == 0) { if (led_yellow == 0) { gpio_put(LED_YELLOW, 1); led_yellow = 1; } else { gpio_put(LED_YELLOW, 0); led_yellow = 0; } count_led_yellow = 0; } count_led_yellow++; } sleep_ms(1); // Set the phaseA and phaseB pins to GPIO gpio_init(rotary_encorder_phaseA); gpio_init(rotary_encorder_phaseB); gpio_set_dir(rotary_encorder_phaseA, GPIO_IN); gpio_set_dir(rotary_encorder_phaseB, GPIO_IN); encoder_value = 0; ////////////////////////////////// // start controlling the machine // after send "a" 3 times. ////////////////////////////////// // YELLOW LED is ON for 3 [sec] send_a_3times(); ///////////////////////// // A/D 変換の処理 // ///////////////////////// A/D 変換はお休み // ADC module initialize //uart_puts(UART_ID, "ADC Example, measuring GPIO26\n"); //adc_init(); // Make sure GPIO is high-impedance, no pullups etc //adc_gpio_init(26); // Select ADC input 0 (GPIO26) //adc_select_input(0); ///////////////////////// // PWM の前処理 // ///////////////////////// /* GPIOにPWMを割り当て */ gpio_set_function(PIN_PWM0, GPIO_FUNC_PWM); pwm0_slice_num = pwm_gpio_to_slice_num(PIN_PWM0); /* clkdiv と wrap を指定 */ pwm_set_clkdiv(pwm0_slice_num, 12.1875); pwm_set_wrap(pwm0_slice_num, 1023); /* レベル値(デューティカウント値)を設定(ここでは0) */ pwm_set_chan_level(pwm0_slice_num, PWM_CHAN_A, 0); /* pwm0 start */ pwm_set_enabled(pwm0_slice_num, true); // pwm_set_enabled(pwm0_slice_num, false); /* END of PWM Module Setting */ //------------------------------------------- //Motor driver intialization //------------------------------------------- // Set the IN0 and IN1 pins to GPIO gpio_init(Motor_IN1); gpio_init(Motor_IN2); gpio_set_dir(Motor_IN1, GPIO_OUT); gpio_set_dir(Motor_IN2, GPIO_OUT); gpio_put(Motor_IN1, 0); //motor STOP gpio_put(Motor_IN2, 0); //motor STOP //Motor driver intialization END //------------------------------------------- //Kalman filter (angle) initialization //------------------------------------------- //initial value of theta_data_predict theta_data_predict[0][0] = 0; theta_data_predict[1][0] = theta_dot_mean; //initial value of P_theta_predict P_theta_predict[0][0] = 1; P_theta_predict[0][1] = 0; P_theta_predict[1][0] = 0; P_theta_predict[1][1] = theta_dot_variance; //------------------------------------------- //Kalman filter (all system) initialization //------------------------------------------- //initial value of x_data_predict for(int i=0; i<4; i++) { x_data_predict[i][0] = 0; } //initial value of P_x_predict for(int i=0; i<4; i++) { for(int j=0; j<4; j++) { P_x_predict[i][j] = 0; } } for(int i=0; i<4; i++) { P_x_predict[i][i] = 1e-4; } //measurement noise matrix for(int i=0; i<4; i++) { for(int j=0; j<4; j++) { measure_variance_mat[i][j] = 0; } } measure_variance_mat[0][0] = theta_variance * deg_rad_coeff; measure_variance_mat[1][1] = theta_dot_variance * deg_rad_coeff; measure_variance_mat[2][2] = encoder_error * encoder_error; measure_variance_mat[3][3] = encoder_rate_error * encoder_rate_error; ///////////////////////////////////////////// // Timer ///////////////////////////////////////////// // set P_angle to theta_data[0][0] theta_data[0][0] = get_P_angle(); // start rotary encorder process (25 us) add_repeating_timer_us(-25, rotary_encoder_check, NULL, &timer_encorder); sleep_ms(1); // start estimation process (2.5 ms) add_repeating_timer_us(-2500, theta_estimation, NULL, &timer_esti); sleep_ms(1); // calculate W_angle [degree] as initial values W_angle[0] = (float)encoder_value / 1074.0 * 360.0; for (int i = 1; i <= 9; i++) { W_angle[i] = W_angle[0]; } // calculate P_angle_dot [deg/s] as initial values P_angle_dot_data[0] = get_P_angle_dot(); for (int i = 0; i < 9; i++) { P_angle_dot_data[i + 1] = P_angle_dot_data[0]; } count_control = 250; // start main control process (10.0 ms) //add_repeating_timer_ms(-10, Control_Pendulum, NULL, &timer_control); //sleep_ms(1); uart_puts(UART_ID, "Start!!\n"); sleep_ms(10); //=========================================== //Main loop //it takes 10 msec (calculation) //=========================================== Motor_Mode = 0; count = 0; //t_start = time_us_64(); while(1) { t_start = time_us_32(); Control_Pendulum(); count++; if (count >= 50) { count = 0; if (flag == 0) { gpio_put(LED_PICO, 1); flag = 1; }else { gpio_put(LED_PICO, 0); flag = 0; } } if (count % 5 == 0) { uart_var(); } else { sleep_ms(1); } t_end = time_us_32() - t_start; } //=========================================== //Main loop (end) //=========================================== cancelled_esti = cancel_repeating_timer(&timer_control); //uart_puts(UART_ID, "cancelled... _control"); snprintf(s, sizeof(s), "ctl:%d\n", cancelled_control); uart_puts(UART_ID, s); uart_puts(UART_ID, "\0"); sleep_ms(1); cancelled_esti = cancel_repeating_timer(&timer_esti); //uart_puts(UART_ID, "cancelled... _esti"); snprintf(s, sizeof(s), "esti:%d\n", cancelled_esti); uart_puts(UART_ID, s); uart_puts(UART_ID, "\0"); sleep_ms(1); cancelled_encorder = cancel_repeating_timer(&timer_encorder); //uart_puts(UART_ID, "cancelled... _encorder"); snprintf(s, sizeof(s), "enco:%d\n", cancelled_encorder); uart_puts(UART_ID, s); uart_puts(UART_ID, "\0"); sleep_ms(1); return; }
