1. ã¯ããã«
æè¿ãå€å
žå¶åŸ¡çè«ãéäžçã«å匷ããæ©äŒããã£ãã®ã§ãããã©ãã©ã¹å€æãªã©ã®çè«ããããå匷ããŠããå¶åŸ¡ã®å®éãããã£ãæ°ã«ã¯ããŸããªããªãã®ã§ããããæãåãããŠäœéšã§ãããããªå¶åŸ¡ã®ææã欲ãããšæã£ãŠããŸããã
ãããªãªããããã€ãäœããããšæã£ãŠæè²·ã£ãŠããã
ãã©ã³ãžã¹ã¿æè¡ 2019幎7æå·ãã§ç¹éãããŠãããã«ã«ãã³åç«æ¯åããæãåºãããããäœã£ãŠã¿ãããšã«ããŸããã
äžå³ã®ãããªãã®ã§ãã
ãåç«æ¯åããšã¯ãæ¯ç¹ãããéå¿ãé«ãäœçœ®ã«ããæ¯ãåã®ããšãèšããäžå³ã§èšãã°ã¿ã€ã€ã®ã·ã£ãããæ¯ç¹ãæ¿å
šäœãæ¯ãåã§ããããã®æ¿ãåããªãããã«ã¿ã€ã€ã®å転ãå¶åŸ¡ããã®ãç®æšã§ãã
äžè¿°ã®ãã©ã³ãžã¹ã¿æè¡ã®äœäŸã§ã¯ãã«ã«ãã³ãã£ã«ã¿ã®æè¡ã«ããæ¯ãåã®è§åºŠ Ξ ãš æ¯ç¹ã®äœçœ® x ã®èªã¿åããå®å®ãããçŸä»£å¶åŸ¡çè«ã䜿ã£ãŠåç«æ¯åãçŽç«ããç¶ããæš¡åã®å·¥äœæ¹æ³ã解説ãããŠããŸãã
äœäŸã§ã¯ãã€ã³ã³ãšããŠã
STM32 Nucleo Board STM32F401ãã䜿ãããŠããã®ã§ãããå
šãåãã§ã¯é¢çœããªãããªãšæããRaspberry Pi Pico ã䜿ã£ãŠã¿ãããšã«ããŸããã
Raspberry Pi Pico ã«æçãããã䜿çšçµéšãå¢ããããã£ããšããçç±ããããŸããæšããã€ã³ã³ããŒãã£ãŠãã€ã§ãããäœãäžèª¿ã¹ããã«æ±ºããããã«è²ã
ãšèŠåŽããããšã«ãªããŸãããïŒæµ®åå°æ°ç¹ãŠããã(FPU)ããªããã¿ã€ããŒãäžã€ãããªãããªã©ïŒãæçµçã«åããã®ã§çµæãªãŒã©ã€ã§ãïŒãããïŒïŒã
å®éã«åäœããŠããæ§åã瀺ããåç»ã¯ãã¡ãã§ãã
ã¡ãªã¿ã«ããã®äœäŸã¯ããªããªã«çŸä»£å¶åŸ¡çè«ã䜿ã£ãŠããã®ã§ãå€å
žå¶åŸ¡çè«ã®ææã欲ãããšããæ¬æ¥ã®ç®çã¯éæã§ããŠããªãã®ã§ããããŸããã®ç¹ã¯ãã®ãã¡ãªããšãããŸãããã
ãããªããã§ããã®ãã©ã³ãžã¹ã¿æè¡ 2019幎7æã®ã«ã«ãã³åç«æ¯åã® Raspberry Pi Pico ãžã®ç§»æ€çãäœãããã§ã®ã¡ã¢ãæ¬ããŒãžã«æ®ããŸãã
2. 泚æ
æ¬ããŒãžãèªãããã§ããã€ã泚æããããŸãã®ã§ãé ã«è¿°ã¹ãŠãããŸãã
ããã©ã³ãžã¹ã¿æè¡ 2019幎7æå·ããå¿
é
ã«ã«ãã³åç«æ¯åã¯ã
ãã©ã³ãžã¹ã¿æè¡ 2019幎7æå·ãã®ç¹éãæçéžè¹ã¢ããã«åŠã¶ç¢ºççµ±èšã³ã³ãã¥ãŒã¿ãã§ç¹éãããŠããã®ã§ãããããã¯å
šäœã§ 150 ããŒãžã«ãåã¶å€§ç¹éã§ãããã®å
容å
šãŠã解説ããããšã¯ã§ããªãã®ã§ãåããããªåç«æ¯åãèªåã§ãäœã£ãŠã¿ãããšæã£ãå Žåãã®æžç±ã¯å¿
é ã§ããã
ãã©ã³ãžã¹ã¿æè¡ 2019幎7æå· (é»åç)ãã® PDF ã¯ä»ã§ãå
¥æå¯èœã§ãã®ã§ããã¡ãã¯ããã»ã©åé¡ã«ã¯ãªããªãã§ãããã
ããã©ã³ãžã¹ã¿æè¡ 2019幎7æå·ãã®ä»é² DVD ã«å«ãŸããããã°ã©ã ã®ãœãŒã¹ã³ãŒããå¿
é
ãããäžçªããã¯ã«ãªããšæãã®ã§ãããäžè¿°ã®é»åçã«ã¯ããã°ã©ã ã®ãœãŒã¹ã³ãŒããããããäžéšããå«ãŸããŠããŸãããå
šãŠã®ãœãŒã¹ã³ãŒããå
¥æããã«ã¯ä»é² DVD ãå¿
é ãªã®ã§ããã倿¬ãªã©ã§ã¯ DVD ãä»å±ããªãããšãå€ãã§ããããç§ãåæã«å
¬éããããã«ã¯ãããªãã®ã§ããªããšãããŠå
¥æããå¿
èŠããããŸãã峿žé€šãªã©ã§ä»é² DVD ãäžç·ã«è²žãåºããŠãããšãããæ¢ãã®ãè¯ãã§ãããããã¡ãªã¿ã«ãRaspberry Pi Pico çšã®ãœãŒã¹ã³ãŒãã¯å
ã®ãœãŒã¹ã³ãŒããžã®ããããšããåœ¢ã§æäŸããŸãããã«ãæžã®ãã€ããªãã¡ã€ã«ãæäŸããŸãã®ã§ããã詊ãããšã¯ã§ããŸããããã©ã¡ãŒã¿ã倿Žããçã®ããã«ã¯ãœãŒã¹ãå¿
èŠãšãªããŸãã
ã¿ãã€ã®ãŠãããŒãµã«ãã¬ãŒãL ãå
¥æãã«ãã
ããã¯ææã«ãããšæãã®ã§ãããå·çæã¯ã
ã¿ã〠楜ããå·¥äœã·ãªãŒãº No.172 ãŠãããŒãµã«ãã¬ãŒãL 210Ã160mmããå
¥æãã«ããç¶æ
ãç¶ããŠããŸããã¿ãã€ã«åãåããããšãããçç£çµäºç¢ºå®ã§ã¯ãªããæ¬¡åçç£ææã¯æªå®ã ããã§ããããã«ã€ããŠã¯ãåããµã€ãºã® ABS æš¹èçã賌å
¥ããå¿
èŠãªåæã«èªåã§ãã³ãã€ã¹ã§ 3mm ã®ç©Žããããããšã«ããŸãããäžã®åçã®çœãæ¿ãããã§ãã
ã¢ãŒã¿ãŒãã©ã€ã TA7291P ãå
¥æãã«ãã
é·ããé»åå·¥äœã§æçšãããŠãã TA7291P ã¯æ¢ã«çç£çµäºãšãªããçŸåšå
¥æããã«ããã§ãïŒamazon ã§ã¯è¶³ãçããè¶³ã«ã¯ãã ãæ®ã£ãŠãããªã©ãããã«ãäžå€ãšããèŠãç®ã®è£œåã売ãããŠããŸããïŒãå¥ã®å
¥æãããããã®ã䜿ããããšãèããã®ã§ããããããããšã¢ãã«åã®æéãå¢ããã®ã§ãæå
ã«è€æ°ãã£ã TA7291P ããã®ãŸãŸäœ¿ãããšã«ããŸããã
ããŒã¿ãªãŒãšã³ã³ãŒããé«äŸ¡
åç«æ¯åã®äœçœ® x ãèšæž¬ããããã«ã
ããŒã¿ãªãŒãšã³ã³ãŒã EC202A100Aããçšããã®ã§ããã6500 å以äžãšãªããªãã«é«äŸ¡ã§ãããããã®äŸ¡æ ŒãèŠããšããããã®åç«æ¯åã®äœæã«æããããããã«ãªã£ãç¬éã§ãããã©ã³ã¯ãäžããããå°ãå®äŸ¡ãªãã®ã¯äœ¿ããªãããšãèããã®ã§ããããã©ãã«ãé¿ããããã«ãã€ã³ã³ä»¥å€ã¯ãªãã¹ãåããã®ãçšããããšã«ããŸããã
Raspberry Pi Pico ã®ããã°ã©ã ã C èšèªã§æžã
Raspberry Pi Pico ã䜿ããšæ±ºãããšãã¯ãåœç¶ããã°ã©ã 㯠Python ã§æžãã§ããããšæã£ãŠãããéäžãŸã§ã¯ããããŠããã®ã§ããããåäœé床ãè¶³ããªããããå®å®æ§ãè¶³ããªãããªã©åé¡ãå€çºããããããããªã C èšèªãçšããããšã«ããŸãããRaspberry Pi Pico ã C èšèªã§éçºããããã«ã¯ãéçºç°å¢ãšã㊠Raspberry Pi äžã§ cmake ã䜿ãã®ãäžçªç°¡åã ãšæããŸããä»ã® Linux ã Windows ã§ãã§ãããããããŸãããæªæ€èšŒã§ãã
ã¯ãã ä»ããè¶
倧å€
ãŠãããŒãµã«åºæ¿ãçšããåè·¯ã®äœæã¯è¶
ä¹
ãã¶ãã ã£ãã®ã§ãããæããã倧å€ã§æ³£ãããã§ãããã¯ãã ä»ããããé¢ã¯ãšãŠã人ã«ã¯èŠããããŸããã
3. å¿
èŠãªãã®
å¿
èŠãªãã®ããªã¹ãã¢ãããããšä»¥äžã®ããã«ãªããŸãã
4. Raspberrry Pi Pico ã®éçºç°å¢ã®èšå®
ããŠãããããå
ã¯åç«æ¯åã®äœæã«å
¥ã£ãŠããããã§ããããããªãè»äœã®çµã¿ç«ãŠã«å
¥ãããã§ã¯ãããŸããããŸãã¯ããã¬ããããŒããçšããã9軞ã»ã³ãµãŒBMX055ãšã«ã«ãã³ãã£ã«ã¿ãçšããè§åºŠã®æšå®ããã話ãé²ããŠãããŸãã
æžç±ã§ãããšãp.41 ããå§ãŸãã第3ç¯ åŸæèšã®ãœãããŠã§ã¢éçºãããã³ p.49 ã® Inclinometer.cpp ã®åäœã®éšåã§ãã
ãã®ããã«ããŸã㯠Raspberrry Pi Pico ã®éçºç°å¢ã®èšå®ããå§ããŠãããŸãããã
C èšèªã§éçºãè¡ãããã®åºæ¬çãªæ
å ±ã¯ã
The C/C++ SDKãã«æžãããŠããŸãã
ãŸããéåžžã® Raspberry Pi ã®ãã¹ã¯ãããç°å¢ãçšæããRaspberrry Pi Pico ã®éçºã«å¿
èŠãªããŒã«ã®ã€ã³ã¹ããŒã«ããå§ããŸããããäžèšã®2ã€ã®ã³ãã³ããé ã«è¡ããŸããæåŸã®ãminicomãã¯ã¿ãŒããã«äžã§ã·ãªã¢ã«éä¿¡ãè¡ãããã®ãã®ã§ãã
sudo apt update
sudo apt install cmake gcc-arm-none-eabi libnewlib-arm-none-eabi libstdc++-arm-none-eabi-newlib minicom
ã€ã³ã¹ããŒã«ãçµãã£ãããã䜿ãã®ãŠãŒã¶ãŒã®ããŒã ãã£ã¬ã¯ããªã«ãRaspberry Pi Pico çšã® SDK ãããŠã³ããŒãããŸããããäžã€ç®ã®ã³ãã³ãã¯ããŒã ãã£ã¬ã¯ããªã«ç§»åããããã®ãã®ã§ãã
cd
git clone https://github.com/raspberrypi/pico-sdk.git
次ã«ãããŠã³ããŒããçµãã pico-sdk ã®å Žæãç°å¢å€æ° PICO_SDK_PATH ã«ã»ããããŸãããããã®ããã«ã¯ããã¡ã€ã« .bashrc ã®æ«å°Ÿã«èšå®ã远å ããå¿
èŠããããŸãã
ãŸãã¯ä»¥äžã®ã³ãã³ãã§ .bashrc ãç·šéçšã«éããŸãããã
mousepad .bashrc
ãããŠãéãããã¡ã€ã«ã®æ«å°Ÿã«ç§»åãã以äžã®3è¡ã远èšããŠãã¡ã€ã«ãä¿åããŠéããŸããäžã€ç®ã§ ç°å¢å€æ° PICO_SDK_PATH ã«ããŒã ãã£ã¬ã¯ããªã«ãã pico-sdk ãæå®ããŠããŸãã
äºã€ç®ã¯ãminicom -b 115200 -o -D /dev/ttyACM0ããšããé·ãã³ãã³ãããminiacmããšããçãã³ãã³ãïŒãšã€ãªã¢ã¹ïŒã§å®è¡ããããã®ãã®ã§ãã
äºã€ç®ã¯åç«æ¯åã®å®æåã«çšãããšã€ãªã¢ã¹ã§ãäžã€ç®ã¯åç«æ¯åã®å®æåŸã«ïŒäººã«ãã£ãŠã¯ïŒçšãããããã°çšã®ãšã€ãªã¢ã¹ã§ãã
export PICO_SDK_PATH=/home/$USER/pico-sdk
alias miniacm="minicom -b 115200 -o -D /dev/ttyACM0"
alias miniusb="minicom -b 115200 -o -D /dev/ttyUSB0"
远èšãçµãã£ããããã®ã¿ãŒããã«ã§äžèšã³ãã³ããå®è¡ããã°èšå®ãåæ ãããŸãã
source .bashrc
ãªããæ°ãã«èµ·åããã¿ãŒããã«ã§ã¯è¿œèšããèšå®ã¯èªåçã«èªã¿èŸŒãŸããã®ã§å床èªã¿èŸŒãå¿
èŠã¯ãããŸããã
以äžã§ Raspberry Pi Pico çšã®ã©ã€ãã©ãªã®èšå®ãçµãããŸããããã¥ãŒããªã¢ã«ãµã€ããåèã«ç°¡åãªäŸã詊ããŠã¿ãŸããããPico ã§ã"Hello, world!ããšåºåãã printf åœä»€ãå®è¡ãã
ããã Rapsberry Pi ã®ã¿ãŒããã«ã§åãåã£ãŠè¡šç€ºããããšãããã®ã§ãã
ãŸããhello ãã£ã¬ã¯ããªãäœæããããã« SDK ãã pico_sdk_import.cmake ãšãããã¡ã€ã«ãã³ããŒããŠããŸãããã
mkdir hello
cd hello
cp ~/pico-sdk/external/pico_sdk_import.cmake .
次ã«ããã«ãçšã®èšå®ãã¡ã€ã« CMakeLists.txt ãã¡ã€ã«ãäœæããŸããããhello ãã£ã¬ã¯ããªã«ããã¿ãŒããã«ã§ãã®ãŸãŸä»¥äžã®ã³ãã³ããå®è¡ããŸãã
mousepad CMakeLists.txt
空㮠mousepad ãéãããã以äžã®å
容ãèšè¿°ããŸãããã
cmake_minimum_required(VERSION 3.13)
# initialize the SDK based on PICO_SDK_PATH
# note: this must happen before project()
include(pico_sdk_import.cmake)
project(my_project)
# initialize the Raspberry Pi Pico SDK
pico_sdk_init()
# rest of your project
add_executable(hello_world
hello_world.c
)
# Add pico_stdlib library which aggregates commonly used features
target_link_libraries(hello_world pico_stdlib)
pico_enable_stdio_usb(hello_world 1)
pico_enable_stdio_uart(hello_world 0)
# create map/bin/hex/uf2 file in addition to ELF.
pico_add_extra_outputs(hello_world)
èšè¿°ãçµãã£ãããä¿åããŠãã®ãã¡ã€ã«ãéããŸãããã®ãã¡ã€ã«ã¯ãæžããããã°ã©ã ããã«ãããããã®èšå®ãã¡ã€ã«ãšãªããŸãã
次ã«ãCèšèªããã°ã©ã hello_world.c ãèšè¿°ããŸããããhello ãã£ã¬ã¯ããªã«ããã¿ãŒããã«ã§ãã®ãŸãŸä»¥äžã®ã³ãã³ããå®è¡ããŠ
空㮠hello_world.c ãã¡ã€ã«ãéããŸãã
mousepad hello_world.c
éããããäžèšã®å
容ãèšè¿°ããŸãããããã®ãã¡ã€ã«ã¯ã1ç§ããã« "Hello, world!"ãšããæåã衚瀺ãããšãããã®ã§ããæ£ç¢ºã«ã¯ã1ç§ããã«"Hello, world!"ãšããæååãã·ãªã¢ã«éä¿¡ã§éä¿¡ããããšããå
容ã§ãéä¿¡å
㯠USB ã®æ¥ç¶å
ãšããèšå®ã«ãªã£ãŠããŸãã
#include <stdio.h>
#include "pico/stdlib.h"
int main() {
stdio_init_all();
while(1){
printf("Hello, world!\n");
sleep_ms(1000);
}
return 0;
}
èšè¿°ãçµãã£ããä¿åããŠãã¡ã€ã«ãéããŸãã
ããŠãCèšèªããã°ã©ã ãæžããããããã«ãããããã®èšå®ãã¡ã€ã«ãçšæã§ããã®ã§æ¬¡ã«ãã«ããè¡ããŸããhello ãã£ã¬ã¯ããªã«ããã¿ãŒããã«ã§ä»¥äžã®ã³ãã³ããå®è¡ããŠãã«ããè¡ããŸãããã
ãã«ãçšã®ãã£ã¬ã¯ã㪠build ãäœæããŠããããã®ãã£ã¬ã¯ããªå
ã§ãcmake ..ãããmakeããšããäºã€ã®ã³ãã³ããå®è¡ããŠããŸãã
mkdir build
cd build
cmake ..
make
å®è¡ãçµãããšãäžèšã®ãããªè¡šç€ºã«ãªã£ãŠããã®ã§ã¯ãªãã§ããããã
(äžç¥)
[100%] Built target pioasm
[ 98%] No install step for 'PioasmBuild'
[100%] Completed 'PioasmBuild'
[100%] Built target PioasmBuild
ãããŠããã® build ãã£ã¬ã¯ããªå
ã« hello_world.uf2 ãšãããã¡ã€ã«ãã§ããŠããã®ã§ã¯ãªãããšæããŸãããã®ãã¡ã€ã«ããPico ã«ã³ããŒããŠå®è¡ãã¹ããã¡ã€ã«ãšãªããŸãã
Pico ã« hello_world.uf2 ãã³ããŒãããããPico ã® BOOTSEL ãã¿ã³ãæŒããªãã Raspberry Pi ã« USB æ¥ç¶ããŸããããPico ããã¡ã€ã«ãããŒãžã£ã§éããŸãã®ã§ãã®äžïŒRPI-RP2ïŒã« hello/build ãã£ã¬ã¯ããªã«ãããhello_world.uf2 ãã³ããŒããŸãããããã¡ã€ã«ãããŒãžã£ã«ãããã©ãã°ã¢ã³ãããããã§æ§ããŸããã
ã³ããŒãçµãããš Pico ãåèµ·åãããå
ã»ã©ã® C èšèªããã°ã©ã ãèªåçã«åäœãéå§ããŸãã
Raspberry Pi ã®ã¿ãŒããã«äžã§ãšã€ãªã¢ã¹ miniacm ãå®è¡ããŸãããã
miniacm
ãã®ãšã€ãªã¢ã¹ã¯ãminicom -b 115200 -o -D /dev/ttyACM0ããšããã³ãã³ããå®è¡ããã®ãšåã广ãããã®ã§ããã
ããã¯ãã/dev/ttyACM0 ãšããŠèªèãããŠãã Pico ãšã·ãªã¢ã«éä¿¡ãããããšããæå³ã«ãªããŸãã
ããŠãminiacm ãå®è¡ããã¿ãŒããã«ã§ã¯ãPico ãåºåããã Hello, world! ããšããæååã1ç§ããã«è¡šç€ºãããŠããã®ã§ã¯ãªãã§ããããïŒããªãã¡ãã·ãªã¢ã«éä¿¡ã«ãããPico ããã®æååã®éä¿¡ã Raspberry Pi ã§åä¿¡ã§ããããšã«ãªããŸãã
以äžã§åäœç¢ºèªçµäºã§ãããŸããminiacm ã§å®è¡ãã minicom ãçµäºããŸããããã Hello, world! ããšããæååã衚瀺ãããŠããã¿ãŒããã«äžã§ãããŒããŒãã§ãCtrl-AãâãQãâãEnterããšé ã«å
¥åããŸããããminicom ãçµäºããŸãããããŠãPico ãšã® USB æ¥ç¶ãåãé¢ãããšã§ Pico ã®é»æºãåããŸãããã
5. åç«æ¯åçšããã°ã©ã ã®æºå
ããã§ã¯ãPico çšã®ããã°ã©ã ã®æºåã«å
¥ããŸããããRaspberry Pi ã®ããŒã ãã£ã¬ã¯ããªã«ç§»åããäžèšã®ã³ãã³ãã§å¿
èŠãªãã¡ã€ã«ãããŠã³ããŒãããŸããããäžã€ç®ã®ã³ãã³ããããŒã ãã£ã¬ã¯ããªãžã®ç§»åã衚ããŸãã
cd
git clone https://github.com/neuralassembly/pico-inverted-pendulum
ããŠã³ããŒããçµãã£ãããpico-inverted-pendulum ãã£ã¬ã¯ããªã«ç§»åããŸãã
cd pico-inverted-pendulum
ãã®äžã«ã¯ïŒã€ã®ãã£ã¬ã¯ããªããããŸãã
- KalmanAngle: Inclinometor.cpp ããã«ãããããã®ãã£ã¬ã¯ããª
- KalmanFinal: Inverted_Pendulum_Kalman.cpp ããã«ãããããã®ãã£ã¬ã¯ããª
- Binaries: ãã«ãæžãã¡ã€ã« Inclinometor.uf2 ãš Inverted_Pendulum_Kalman.uf2 ãæ ŒçŽãããã£ã¬ã¯ããª
ããããå
ã¯ãInclinometor.cpp ãš Inverted_Pendulum_Kalman.cpp ãRaspberry Pi Pico åãã«ãã«ãããæ¹æ³ãèšããŠãããŸãããœãŒã¹ã³ãŒããå
¥æã§ããªãæ¹ã¯ãBinarie ãã©ã«ãã«æ ŒçŽããããã«ãæžãã¡ã€ã«ã詊ãããšãã§ããŸãã
ããŠãKalmanAngle ãš KalmanFinal ã®äºã€ã®ãã£ã¬ã¯ããªã«ãpico_sdk_import.cmake ãã¡ã€ã«ã KalmanAngle ãã£ã¬ã¯ããªãš KalmanFinal ãã£ã¬ã¯ããªã«ã³ããŒããŸããå
ã»ã©ã® Hello, world! ããã°ã©ã ã§ãåçã®äœæ¥ãè¡ããŸãããã
cp ~/pico-sdk/external/pico_sdk_import.cmake KalmanAngle
cp ~/pico-sdk/external/pico_sdk_import.cmake KalmanFinal
äºã€ã®ãã£ã¬ã¯ããªã®ãã¡ã KalmanAngle ã¯æžç±ã® p.41 ããå§ãŸãã第3ç¯ åŸæèšã®ãœãããŠã§ã¢éçºãããã³ p.49 ã® Inclinometer.cpp ãå®è¡ããããã®ãã®ã§ãã
ãŸããKanlanFinal ã¯è»äœã宿ããããšã«åäœãããããã°ã©ã ãæ ŒçŽããããã£ã¬ã¯ããªã§ãã
以äžãèžãŸãããã©ã³ãžã¹ã¿æè¡2019幎7æå· ä»é²DVD ã«å«ãŸãããªãªãžãã«ã®ããã°ã©ã ã®ãã¡ã3ã€ã®ãã¡ã€ã«ãäžèšã®å Žæã«ã³ããŒããŸãã
Inclinometer.cpp ã pico-inverted-pendulum/KalmanAngle ãã£ã¬ã¯ããªã«ã³ããŒ
SolveRiccatiEquation.py ãš Inverted_Pendulum_Kalman.cpp ã pico-inverted-pendulum/KalmanFinal ãã£ã¬ã¯ããªã«ã³ããŒ
ãããæžãã ããpico-inverted-pendulum ãã£ã¬ã¯ããªã«ããã¿ãŒããã«ã§ä»¥äžã®ã³ãã³ããå®è¡ããRaspberry Pi Pico çšã®ãã¡ã€ã«ã«æŽæ°ããŸãã
patch -p0 -i pico-ip.patch
ãã®ã³ãã³ãã«ãããInclinometer.cpp ãSolveRiccatiEquation.py ãInverted_Pendulum_Kalman.cpp ã Raspberry Pi Pico çšã®ããã°ã©ã ã«æŽæ°ãããŸããã
ãã¡ãããããã3ãã¡ã€ã«ããããããé©åãªå Žæã«ãªããšæŽæ°ã¯å€±æããŸãã
倱æããããåãã£ã¬ã¯ããªã«ããäžãã¡ã€ã«ãäžæŠæ¶ããããäžåºŠã³ããŒããããçŽããšããã§ãããã
ããŠãããã°ã©ã ã®æŽæ°ãæžãã ããã«ãããŠã¿ãŸããããäžèšã®ã³ãã³ããé ã«å®è¡ããŸãããŸã㯠KalmanAngle ãã£ã¬ã¯ããªã§ãã
cd ~/pico-inverted-pendulum/KalmanAngle
mkdir build
cd build
cmake ..
make
ãããçµãã£ãã KalmanFinal ãã£ã¬ã¯ããªã§ãã
cd ~/pico-inverted-pendulum/KalmanFinal
mkdir build
cd build
cmake ..
make
ã©ã¡ãããšã©ãŒãªããã«ãã§ããããããããã® build ãã£ã¬ã¯ããªã« uf2 ãã§ããŠããã¯ãã§ããå
ã«é²ã¿ãŸãããã
6. 9軞ã»ã³ãµãŒBMX055ãšã«ã«ãã³ãã£ã«ã¿ãçšããè§åºŠã®æšå®
ã§ã¯ããŸãã¯ãã¬ããããŒããçšããã9軞ã»ã³ãµãŒBMX055ãšã«ã«ãã³ãã£ã«ã¿ãçšããè§åºŠã®æšå®ããè¡ã£ãŠã¿ãŸãããã
æžç±ã§ãããšãp.41 ããå§ãŸãã第3ç¯ åŸæèšã®ãœãããŠã§ã¢éçºãããã³ p.49 ã® Inclinometer.cpp ã®åäœã®éšåãªã®ã§ããã
ãŸãã9軞ã»ã³ãµãŒBMX055 ãã¯ãã ä»ãããªããã°ãªããŸããã黿ºãšä¿¡å·ã¬ãã«ããšãã« 3.3V ãªã®ã§ãèª¬ææžã«ããããã« JP7 ã®éšåã«ã¯ãã ãçããVCC ãš 3.3V ã®äž¡æ¹ã«é»æºãæ¥ç¶ããããã«ããŸãã
ãã¬ããããŒãäžã§äœæããåè·¯ã¯ä»¥äžã®éãã§ãã
åè·¯ãçµããããå
ã»ã©ãã«ãããŠåŸããã pico-inverted-pendulum/KalmanAngle/build/Inclinometer.uf2 ã Pico ã«ã³ããŒããŸããããBOOTSELãã¿ã³ãæŒããªãã Pico ã USB çµç±ã§ Raspberry Pi ã«æ¥ç¶ãããã¡ã€ã«ãããŒãžã£ãŒã§ãã¡ã€ã«ãã³ããŒããã°è¯ãã®ã§ããããã³ããŒãçµãããš Pico ãåèµ·åããããã°ã©ã ãåãå§ããŠããŸãã
ãããããšããã«ã«ãã³ãã£ã«ã¿ãéããè§åºŠã®ããŒã¿ããšããã»ã³ãµããçŽæ¥èšç®ããè§åºŠã®ããŒã¿ãã空çœã§åºåãã㊠0.1 ç§ããšã«éãããŠããŸãã
Raspberry Pi ã®ã¿ãŒããã«ã§ miniacm ãšã€ãªã¢ã¹ã§ minicom ãå®è¡ããŠã¿ãŸãããã
miniacm
ãããšãã¿ãŒããã«äžã«è§åºŠã衚瀺ãããã¯ãã§ãããªããåè·¯å³ã«èšããããã«ãPico ãžã®é»æºæå
¥æã¯ãUSBæ¥ç¶ç«¯åãçäžãåããç¶æ
ïŒè§åºŠ Ξ=0ïŒã§æ¥ç¶ããåæãšãªã£ãŠããŸãã
äžåºŠ Pico ãžã® USB æ¥ç¶ãåããåããåãããç¶æ
ã§ USB æ¥ç¶ããããã«ããŠã¿ãŸããããè§åºŠ 0 ä»è¿ãã衚瀺ãå§ãŸããåŸããåãã«ãã£ãŠæ£ãŸãã¯è² ã®è§åºŠãåŸãããã¯ãã§ãã
åç«æ¯åã¯ãã®ããã«è§åºŠ 0 床ä»è¿ã§åäœããŸãã
ãŸããè§åºŠããªãã¹ãäžå®ã«ä¿ã£ããŸãŸããã¬ããããŒããå°ãæ¿ããã«åãããŠã¿ãŸãããïŒäŸãã°ããŒãã«äžã§ãã¬ããããŒãã®è§åºŠãä¿ã£ããŸãŸãã¬ããããŒããããŒãã«äžã§ååŸã«æ»ãããããªã©ïŒããã«ã«ãã³ãã£ã«ã¿ãéããè§åºŠã®ããŒã¿ãã¯ããŸãå€åããªãã®ã«å¯Ÿãããã»ã³ãµããçŽæ¥èšç®ããè§åºŠã®ããŒã¿ãã¯å€§ããå€åããã®ããããã¯ãã§ããäŸãã°ä»¥äžã®ãããªåºåãåŸãããŸãã
(äžç¥)
60.476841 60.588188
60.568378 60.581692
59.464199 46.408962
57.363106 60.888397
61.548885 82.362831
64.96154 78.891701
63.586735 44.846802
59.855469 51.911228
59.990036 61.171368
60.115009 60.16819
ããã¯ãè§åºŠ60床ããªãã¹ãä¿ã¡ã€ã€ãã»ã³ãµãåããããšãã®æ§åã§ããã«ã«ãã³ãã£ã«ã¿ã®åºåã§ããå·ŠåŽã®æ°å㯠60 床ä»è¿ã®å€ã«ä¿ãããŠããŸãããã»ã³ãµããçŽæ¥æ±ããè§åºŠã§ããå³åŽã®æ°å€ã¯ã倧ããå€åããŠããŸãããããäžå³ã®ããã«ã°ã©ãã«ãããšããã¯ã£ããããŸãã5ç§éã§3åã»ã³ãµãæ¿ããåããããšãã®æ§åã§ãã
ã«ã«ãã³ãã£ã«ã¿ã®åºåã®å€åãå°ãããªã£ãŠããããšããããã§ãããããããã«ã«ãã³ãã£ã«ã¿ã®å¹æã§ãã
ããŠããã®ã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå®ã§ãããèšç®ã«ã©ããããã®æéããããã®ã§ãããããã«ã«ãã³ãã£ã«ã¿ã®èšç®ã¯æµ®åå°æ°æŒç®ãçšããè¡åèšç®ãå€çšããã®ã§ãèšç®ã³ã¹ãã¯é«ãã¯ãã§ãã
ãªãªãžãã«ã® Inclinometer.cpp ãèŠããšã颿°ãvoid update_theta()ãã®åé ã«ã//It takes 650 usec. (NUCLEO-F401RE 84MHz, BMX055)ããšã³ã¡ã³ããæžããŠãããŸãã
äžæ¹ãç§ã® Raspbery Pi Pico ããŒãžã§ã³ã§èšæž¬ãè¡ã£ãŠã¿ããšãããããã 800 ÎŒs ã§ããã
ãã®èšæž¬æ¹æ³ã§ããã以äžã®äºã€ã®æ¹æ³ãæãã€ããŸãã
- ãªã·ãã¹ã³ãŒããæã£ãŠããå ŽåïŒupdate_theta ã®åé ã§ã©ããã® GPIO ã 1 ã«ããupdate_theta çµäºæã«ãã® GPIO ã 0 ã«ããããã®ä¿¡å·ããªã·ãã¹ã³ãŒãã§èŠ³æž¬ãããããã°ã©ã ã®ä¿®æ£ãæäœéã§æžãã®ãã¡ãªãã
- ãªã·ãã¹ã³ãŒããæã£ãŠããªãå ŽåïŒupdate_theta ã倿°åç¹°ãè¿ãããã°ã©ã ãæžããéå§æãšçµäºæã®æå»ã®å·®ã printf ããã800ÎŒs ã ãš10,000 åã§ 8 ç§ãããã«ãªããèšæž¬å°çšã®ããã°ã©ã ãæžããªããã°ãªããªãã®ãããé¢å
ããŠãRaspberry Pi Pico ã§ã¯ãªãªãžãã«ã® NUCLEO-F401RE ã«æ¯ã¹ãŠåäœãé
ããªã£ãŠããŸããŸãããRaspberry Pi Pico ã®ã¯ããã¯åšæ³¢æ°ã¯ 125MHz ã§ NUCLEO-F401RE ã® 84MHz ãããéãã®ã«ãªãã ãããšäžç¬èããŠããŸããŸããããã¯ã NUCLEO-F401RE ã«æèŒãããŠãã MPU STM32F401RE ã«ã¯æµ®åå°æ°ç¹ãŠããã (FPU) ãæèŒãããŠããã®ã«å¯ŸããRaspberry Pi Pico ã«æèŒãããŠãã RP2040 ã«ã¯ããããªãããã ãšèããããŸãã
ãRaspberry Pi Pico FPUãã§æ€çŽ¢ãããšããInterface 2021幎8æå·ãã®å®®ç°è³¢äžããã«ããèšäºãMicroPythonÃPicoã®å®åæ€èšŒãããããããŸãã
ç¡æã§èªãã1ããŒãžç®ã«ãFPUãæã€Cortex-M4ç³»ããŒããšPicoã§ã¯çŽ1.5åã®å·®ãåºãŸããããšæžããŠããããããããã®èšäºãåæ ããçµæãšèšãããšæããŸãã
ã«ã«ãã³ãã£ã«ã¿ãä»äºãªã©ã§æ¬æ Œçã«äœ¿ãæ¹ãªãã°ãFPU æèŒã®ããŒããéžã¶ã¹ããªã®ã§ããããããã¯ãããšããŠåãå
¥ããäžã§ãæ¬ããŒãžã§ã¯ Raspberry Pi Pico ã§ã®åç«æ¯åã®å®çŸãç®æããŸãã
äœè«ã§ãããCèšèªã§ 800ÎŒsããã£ãã«ã«ãã³ãã£ã«ã¿ã®èšç®ã MicroPython ã§å®è£
ããå Žåã 4.2ms ãš 5 åãããã®æéãããããŸãããããã«å ããI2C çµç±ã§ã®ã»ã³ãµã®å€ã®ååŸãææã©ã³ãã ã«ãšã©ãŒãè¿ãåé¡ããããPython ã®å©çšãæå¿µããŸããã
ã¡ãªã¿ã«ãC èšèªã«ãããã«ã«ãã³ãã£ã«ã¿ã®èšç®ã®ã¹ã±ãžã¥ãŒã«ãå³ç€ºãããšäžå³ã®ããã«ãªããŸãã
ã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®èšç®ã2.5msããšãããªãã¡ 400Hz ã®åšæ³¢æ°ã§è¡ãããã®ã¯ãããã°ã©ã äžã®äžèšã®éšåã«ãããŸããå®éã«ããã°ã©ã äžã§äœ¿ãããã®ã¯åšæã«ãã theta_update_interval ã®æ¹ã§ãããã®å€ãã¿ã€ãã«æž¡ããã2.5ms ããšã®åŠçãå®çŸããŠããŸãã
const float theta_update_freq = 400; //Hz
const float theta_update_interval = 1.0/theta_update_freq;
2.5ms ãã 0.8ms ãåŒãã 1.7ms ã®æéã§æ®ãã®å¶åŸ¡ãªã©ãè¡ããªããã°ãããªãããã§ãããè¥å¹²ã¹ã±ãžã¥ãŒã«ã窮å±ãªæ°ãããŸããããã®éšåãã©ããªããæ°ã«ãã€ã€ãå
ã«é²ã¿ãŸãããã
7. è»äœã®çµã¿ç«ãŠ
ããŠãã«ã«ãã³ãã£ã«ã¿ãéããè§åºŠã®ååŸã«æåããããããããè»äœã®äœæã§ããããããå
㯠Pico ã®äœ¿ãæ¹ã倧ããå€ãããŸãã®ã§ãè»äœäœæåã«æ³šæããŠãããŸãããã
ãããŸã§ã¯ãPico ãžã®é»æºã USB 端åã«ããäŸçµŠããŠããŸããããããããããã㯠Pico äžã® USB 端åãçšããŸãããPico ãžã®é»åã¯ãVSYS 端åãžåäžå
黿± 3æ¬ã®åºåãå ããããšã§äŸçµŠããŸãã
ãŸããåç«æ¯åãåäœããŠãããšã㯠USB 端åãçšããŸãããããUSB 端åãéã㊠Raspberry Pi ãšã·ãªã¢ã«éä¿¡ããããšã¯ã§ããŸãããäžã®åç»ã®ããã«åç«æ¯åãæ£åžžåäœããŠãããšãã¯ã·ãªã¢ã«éä¿¡ã¯äžèŠã§ãããããã§å€§ããªåé¡ã¯ãªãã®ã§ããããããã°æãªã©ã« Raspberry Pi äžã§ Pico ããã®åºåãèªã¿ãããšãããããããããŸããããã®ãããªå Žåã¯ãPico ã® UART0 TX ãš UART0 RX ã®ãã³ãçšããŠã·ãªã¢ã«éä¿¡ãããããšã«ãªããŸããéä¿¡çžæãšããŠã¯ã
FTDI USBã·ãªã¢ã«å€æã¢ããã¿ãŒ Rev.2ãã 3.3V ã¢ãŒãã§çšããã®ãç°¡åã§ãããããRXâTXãããTXâRXãããGND-GNDãã®çµã¿åãããããªã3æ¬ã§ Pico ãšã·ãªã¢ã«å€æã¢ããã¿ãæ¥ç¶ããã·ãªã¢ã«å€æã¢ããã¿ãš USB æ¥ç¶ãã Raspberry Pi ã§ miniusb ãšã€ãªã¢ã¹ã«ãã minicom ãèµ·åãã/dev/ttyUSB0 ãšéä¿¡ããã°è¯ãã®ã§ãããã®èŸºãã¯ãªãã·ã§ã³ã§ããå¿
é ã§ã¯ãããŸããã
以äžã®æ³šæãèžãŸããŠè»äœãäœæããŠãããŸãã
è¶³åã
ãŸããæžç±ã«èšãããŠããéããã€ãã¯ãŒã®ã¢ãŒããã¯ã¹ HE ã¯ãã®ã¢æ¯ 64.8:1 ã§2ã€äœæããããšã«ãªããŸããããããã«ããã·ã£ããã®åºå®äœçœ®ã¯ãè»äœãžã®åãä»ãããããŒã¿ãªãŒãšã³ã³ãŒãã®åãä»ãã®éã«å€ããå¯èœæ§ããããŸãã®ã§ãäœçœ®ã確å®ãããŸã§ã¯ããããã¯è»œãã«ç· ããŠãããŸãããã
ã®ã¢ãŒããã¯ã¹ã宿ããåŸãã¢ãŒã¿ãŒã®ç«¯åãæ©æž¡ãããããã« 0.1ÎŒF ã®ã³ã³ãã³ãµãã¯ãã ä»ãããå¿
èŠããããŸããããã¯ãæžç±å³14ã®åè·¯å³ã«ãããŠãMã§è¡šãããã¢ãŒã¿ãŒã®è¿ãã«é
眮ãããŠããã³ã³ãã³ãµã®ããšã§ãã
å°ããããã«ããã§ãããäžå³ã«æ°Žè²ã®ã³ã³ãã³ãµãèŠããŸããã
ããŠãã¢ãŒã¿ãŒã«ã³ã³ãã³ãµãåãä»ãããã€ãã¯ãŒã®ã¢ãŒããã¯ã¹ HE ã¯ãã¿ãã€ã®ãŠãããŒãµã«ãã¬ãŒãã«ããã§åºå®ããŸãããABS æš¹èæ¿ã賌å
¥ããå Žåã¯èªåã§ç©Žã空ããå¿
èŠããããŸããéèªã§ã®ãŠãããŒãµã«ãã¬ãŒãã®å©ç𿹿³ã«åããããªããäžå³ã®ããã« 3mm ã®ç©Žã4ã€ãã³ãã€ã¹ã§éããããšã«ãªãã§ãããã
以äžã®ããã«ããŠãABSæš¹èæ¿ã«ãã€ãã¯ãŒã®ã¢ãŒããã¯ã¹ HE ãåºå®ããæ§åãäžå³ã«ãªããŸãããã®èŸºãã¯ãŸã ãŸã 楜ããäœæ¥ãã§ããæ®µéã§ãã
ããŒã¿ãªãŒãšã³ã³ãŒã
ããŒã¿ãªãŒãšã³ã³ãŒãã®åãä»ãã¯ã@Kosuke_Matsui(æŸäº èä»)ããã«ããqiitaã®èšäºã
ãã©ã³ãžã¹ã¿æè¡ïŒæå·ãåç«æ¯åãã®å¶äœèšé²ããåèã«ããŸãããã®ã¢ããã¯ã¹ã®è£œäœéçšãåçãè±å¯ã§ãã®ã§åèã«ãªããšæããŸãã
ç§ãã«ãããªã³ã°ãšããŒã¿ãªãŒãšã³ã³ãŒããåºå®ããæ§åã瀺ããã®ãäžå³ã§ããããŒã¿ãªãŒãšã³ã³ãŒãã¯ABSæš¹èæ¿ããæµ®ããŠããã®ã§ãã¿ãã€ã®ãŠãããŒãµã«ã¢ãŒã ã®åã端ãšã1mmçšåºŠã®åãã®äž¡é¢ããŒããæã¿ãABSæš¹èæ¿ã«ç©Žããããããã§ã
ãããã£ããã§åºå®ããŠããŸããéãªå·¥äœã§ãããäžå¿ã¯æ©èœããŠããŸãã
åè·¯ã®äœæ
åè·¯ã®äœæã¯ãæžç± p.54 ã®å³14ãšã»ãŒåãã§ããNUCLEO-F401RE ã Raspberry Pi Pico ã«çœ®ãæããããã§ãããããã®éãã®éšåã®ã¿ãå³ç€ºãããšäžå³ã®ããã«ãªããŸãã
åè·¯ããŠãããŒãµã«åºæ¿äžã«å®çŸããæ§åãäžå³ã§ãããã³ãããããã³ãœã±ãããçšããŠããŒããã±ãŒãã«ãçè±å¯èœã«ããŠããŸãããªããã¢ãŒã¿ãŒãã©ã€ãã® OUT1ãOUT2 ã®ä¿¡å·ã§ãããOUT1 ã«ã¯ã¢ãŒã¿ãŒã®éã®ã±ãŒãã«ãOUT2 ã«ã¯ã¢ãŒã¿ãŒã®èµ€ã®ã±ãŒãã«ãæ¥ç¶ããŸãã
ã¯ãã ä»ãã«é¢ããŠã¯æ ¹æ°ããè¡ããããããŸãããæ³šæãã¹ãç¹ã¯ãã¯ãã ãä»ãããããŒããååç±ããããšã§ããããã
åè·¯ã«ã€ããŠããäžç¹æ³šæãã¹ããªã®ã¯ãã¢ãŒã¿ãŒãã©ã€ãã®ãã³ã®åœ¢ç¶ãããç¹æ®ãªã®ã§ããã³ãœã±ãããšæ¥è§ŠãæªãããšããŸãã«ããããšããããšã§ãã
åè·¯ã宿ãããåŸãçæ¹ã®ã¿ã€ã€ãå
šãå転ãããæ¥ç¶ãæªãã®ããããŒããæªãã®ããããŒãã®é
çœ®ãæªãã®ããªã©ãã¯ãã ãã€ãããå€ãããäœæéã詊è¡é¯èª€ããŸããã
çµå±ãã¢ãŒã¿ãŒãã©ã€ããšãã³ãœã±ããã®æ¥è§Šãæªãã£ãããšãåå ãšãããããã³ãœã±ãããåãå€ããæ°ããªãã³ãœã±ãããåãåºããŠãŸãã¯ãã ä»ããçŽããŠè§£æ±ºããŸããã
ãã®ãããã®è©Šè¡é¯èª€ã§ã¯ã
ã¯ãã åžãåãç·ããå¿
é ã§ããã
è»äœã®äœæ
æçµçã«å
šãŠã®éšåãè»äœã«åãä»ããæ§åãäžå³ã§ãã
ã®ã¢ããã¯ã¹ãåãä»ããéã¯ãæžç±ã«åãããããšæ
éã«è¡ããŸãããããã®ãããã«ãªããšåå°œããŠããã®ã§ãããã®èŸºããªïŒããšæã£ãäœçœ®ã«ãµã€ã³ãã³ã§å°ãã€ããããã«ç©ŽããããŠåãä»ããŠããŸããŸãããå³äžã«èšããããã«ãæžç±ãšæ¯ã¹ããšéå¿ãäžã«æ¥ãããã«åãä»ããŠããŸã£ãããã§ãã
ãã®ããã«éããéå¿äœçœ®ãå€ãããšãåç«æ¯åã®åŸ®åæ¹çšåŒãå€ãããããã«äŒŽãç¶æ
æ¹çšåŒããã£ãŒãããã¯ã²ã€ã³ãå€ãã£ãŠããŸãïŒããããã¿ãã€ã®ãŠãããŒãµã«ãã¬ãŒãã§ã¯ãªãABSæš¹èæ¿ã䜿ã£ãŠããæç¹ã§å€§ããªå€åã§ãïŒããã®å€åãããã°ã©ã ã«åæ ãããããã®ããŒã«ãæžç±ã§çšæãããŠãããããã SolveRiccaciEquation.py ã§ãããã®äœ¿ãæ¹ã¯åŸè¿°ããŸãã
ããã«ãéããéå¿äœçœ®ãå€å°éã£ãŠãããã°ã©ã ã¯åäœããããšãå€ããšæããŸãã
ã§ãã®ã§ãåè·¯ã黿± ããã¯ã¹ã®åãä»ãäœçœ®ã«ã€ããŠã¯ããã»ã©ç¥çµè³ªã«ãªãå¿
èŠã¯ãããŸããã
8. åç«æ¯åã®èµ·å
ããŠãåç«æ¯åã宿ããããPico ã«ããã°ã©ã ãæžãããã§åäœãããŠã¿ãŸããããã5. åç«æ¯åçšããã°ã©ã ã®æºåãã§è¡ã£ããã«ãã«ãã ~/pico-inverted-pendulum/KalmanFinal/build ãã£ã¬ã¯ããªã« Inverted_Pendulum_Kalman.uf2 ãã§ããŠããŸãã®ã§ããããæžã蟌ã¿ãŸãã
ãã®éã«æ³šæãããã€ããããŸãã
ãŸããPico ã USB æ¥ç¶ãããŸãã«ãå
黿± ã«ãã黿ºãåã£ãŠãããŸãããã黿ºã®ãœã±ããããã³ããæããŠããŸãã®ã確å®ã§ãã
ãªããããã¯ç°å¢ã«ããã®ãããããŸããããç§ã®å Žå Pico ãåºæ¿ã«ãšãã€ãããŸãŸã ãšãBOOTSEL ãæŒããªãã Raspberry Pi ã«å·®ããŠãèªèããªããšããåé¡ããããŸããããã㯠USB ã«æµãã黿µéãäžè¶³ããŠãããããããããŸããã
ãã®ãããªåé¡ããã£ãã®ã§ãç§ã¯ Pico ãåºæ¿ããæã㊠Raspberry Pi ã«åãä»ããããã«ããŸããããªããPico ãåºæ¿ã«æ·±ãå·®ããŠãããšæãã®ã¯ãªããªãé£ããã§ãããã³ãœã±ãããš Pico ã®ééã«ãã€ãã¹ãã©ã€ããå·®ã蟌ã¿ãééãåºããªããå°ããã€æãããã«ããŸãããåä»»ãã«æãããšãããšãã£ããäœã£ãåºæ¿ãç ŽæããæãããããŸãã®ã§æ³šæããŸããããã§ããããPico ãäœåºŠãæãå·®ãããå¯èœæ§ããããšãã¯ãåºæ¿ã«ããŸãæ·±ãå·®ã蟌ãŸãªãããšããæ³šæããããšããã§ãããã
ãŸããRaspberry Pi ã«å¯ŸããŠã§ã¯ãªããWindows ãã·ã³ã«æ¥ç¶ãããšãPico ãåºæ¿ã«å·®ãããŸãŸã§ãèªèãããããšããããŸããããã®å Žåããã«ãã«ããã§ãã uf2 ãã¡ã€ã«ãããããã Windows ã«ç§»åããŠãããŠãWindows ãã Pico ã«ã³ããŒããããšããã®ãæã§ãïŒããã¯ããã§é¢åã§ããïŒã
ãããã«ãããPico ã« uf2 ãã¡ã€ã«ãã³ããŒãããšåèµ·åãããããåç«æ¯åã®åè·¯ãåäœãå§ããŸããããããUSB ãã黿ºãäŸçµŠãããŠããç¶æ³ã§ã¯ã¢ãŒã¿ãŒã«é»åãäŸçµŠãããŸããã®ã§åç«æ¯åã¯åããŸãããUSB ãã Pico ãåãé¢ããæ¹ããŠå
黿± ãæ¥ç¶ããŠåäœãéå§ãããŸãããã
ãã¬ããããŒãã§è¡ã£ãããã«ã黿ºæå
¥æã¯åç«æ¯åãçŽç«ãããç¶æ
ãããªã㡠Ξ=0 ä»è¿ã®ç¶æ
ãç¶æããŸããé»è²ã®LED ãç¹ç¯ããŠããéã¯ãæ§ã
ãªåæåãè¡ãããŠããŸãã®ã§ãå¯èœãªéã Ξ=0 ãç¶æããããã«ããŸããããé»è²ã®LED ãæ¶ãããšåç«æ¯åã¯åäœãéå§ããŸããã¿ã€ã€ã®å転ã®åãã«ãã£ãŠç·ãšèµ€ã®LEDã®ã©ã¡ãããç¹ç¯ããåç«æ¯åã®å®å®æ§ãç¶æãããŸãã
黿ºæå
¥æãæãå®å®æ§ã厩ããããã¿ã€ãã³ã°ã§ãããæäœã«ã¯ããçšåºŠã®æ
£ããå¿
èŠã§ããäœåºŠãç·Žç¿ããŠæ
£ããŠã¿ãŸãããã
ããŸãåäœããªããšããå Žåã¯ãåè·¯ãªã©ã®èŠçŽããããããšã«ãªããŸãã
9. åç«æ¯åã®ãã©ã¡ãŒã¿ã®å€æŽ
ããŠãç§ãšäŒŒãããŒãã䜿ã£ãŠåç«æ¯åãäœãéããããã°ã©ã ã倿Žããå¿
èŠã¯ããŸããªãã¯ãã§ããåºæ¿ã黿± ããã¯ã¹ã®åãä»ãäœçœ®ãå€å°éã£ãŠããããã°ã©ã ã¯ããããåäœããã®ã§ã¯ãªãããšæããŸãã
ã§ãããåèã®ããã«ãã©ã¡ãŒã¿ïŒéããéå¿äœçœ®ãªã©ïŒã倿Žããæ¹æ³ãèšããŠãããŸãã
pico-inverted-pendulum/KalmanFinal ãã£ã¬ã¯ããªã« SolveRiccatiEquation.py ãšãã Python ããã°ã©ã ããããŸãã
ããã¯ãåç«æ¯åã®ãã©ã¡ãŒã¿ããåç«æ¯åã®ç¶æ
æ¹çšåŒããã£ãŒãããã²ã€ã³ãèšç®ããŠããããã®ã§ãã
ããã¹ããšãã£ã¿ mousepad ã Python éçºç°å¢ Thonny ãªã©ã§ãã®ãã¡ã€ã«ãéããŠã¿ãŠã¿ããšããªãªãžãã«ã®ãã¡ã€ã«ããç§ã倿Žããéšåãããã€ãã¿ã€ãããŸãã
以äžã®ãããªéšåã§ãã
# ã¿ãã€ãŠãããŒãµã«ãã¬ãŒããã ABS æš¹èæ¿ã«å€ããã®ã§éããå€ãã£ãéšå
#mass (kg)
#m_plate = 0.080 /2
m_plate = 0.108 /2
# ã·ã£ããããããããªãŒã®éå¿äœçœ®ã 6.5cm ãã 8.5cm ã«å€ãã£ãéšå
#The length between the center of gravity and the axis (m)
#d_battery = 0.065
d_battery = 0.085
# åºæ¿ã®éãã倧ãããã·ã£ããããã®è·é¢ãå€ãã£ãéšå
#mass (kg)
#m_circuit = 0.100 /2
m_circuit = 0.0406 /2
#length (m)
x_circuit = 0.010
#y_circuit = 0.095
y_circuit = 0.070
#The length between the center of gravity and the axis (m)
#d_circuit = 0.140
d_circuit = 0.165
ãã以å€ã§ã¯äžèšã®éšåãéèŠã§ãã
ããã¯ããšããšã®ããã°ã©ã ã§ã¯ T=0.01 ãšèšãããŠãããã¢ãŒã¿ãŒãžã®å¶åŸ¡ã 10ms ããšã«è¡ãããããšã決ããŠããéšåã§ãã
ããããåŸè¿°ããããã« Pico ã§ã¯é床ããããã远ãã€ããªãã®ã§ãã¢ãŒã¿ãŒãžã®å¶åŸ¡ã14ms ããšã«å€æŽããŠããŸãã
ãã®å€æŽã«ãããç¶æ
æ¹çšåŒã圱é¿ãåããŸãã
#sampling rate of the discrete time system
T = 0.014 #sec
以äžã®äžèº«ã確èªããããã§ãSolveRiccatiEquation.py ãå®è¡ããŠã¿ãŸãããããã®åã«å¿
èŠãªã©ã€ãã©ãªã以äžã®ã³ãã³ãã§ã€ã³ã¹ããŒã«ããŠããå¿
èŠããããŸãã
sudo apt update
sudo apt install python3-numpy python3-matplotlib
sudo pip3 install control --break-system-packages
ã€ã³ã¹ããŒã«ãçµãããå®è¡ããŠã¿ãŸããããpico-inverted-pendulum/KalmanFinal ãã£ã¬ã¯ããªã§ä»¥äžãå®è¡ããŸãã
python3 SolveRiccatiEquation.py
æžç± p.68 å³27 ã«é¡äŒŒããã·ãã¥ã¬ãŒã·ã§ã³çµæã®ã°ã©ããçŸããŸãããããã§éèŠãªã®ã¯ã°ã©ãã§ã¯ãªããã³ã³ãœãŒã«ã«è¡šç€ºãããäžèšã®éšåã§ãã
ïŒäžç¥ïŒ
sampling rate = 0.014 sec
matrix Ax (discrete time)
[[ 1.00447563e+00 1.40208911e-02 0.00000000e+00 9.73784303e-05]
[ 6.39181504e-01 1.00447563e+00 0.00000000e+00 1.37291716e-02]
[ -2.00253350e-03 -9.40752945e-06 1.00000000e+00 1.34265323e-02]
[ -2.82332812e-01 -2.00253350e-03 0.00000000e+00 9.19205414e-01]]
matrix Bx (discrete time)
[[-0.00062615]
[-0.08827914]
[ 0.00368742]
[ 0.51951251]]
ïŒäžç¥ïŒ
Gain (calculated)
[[ 28.38403323 4.29034848 0.09009136 0.36309152]]
ãããã®æ°å€ããå¶åŸ¡ããã°ã©ã Inverted_Pendulum_Kalman.cpp ã«åæ ãããã°ãªããŸããããªããäžã§èŠãéããéå¿äœçœ®ãªã©ã®æ°å€ãå€ããŠããªãå Žåã¯ã
åºåãããæ°å€ã¯æ¢ã«ç§ãåæ æžã§ãã
Inverted_Pendulum_Kalman.cpp ãéãã°ãäžèšã®ãããªéšåãã¿ã€ããã¯ãã§ãã
ïŒäžç¥ïŒ
float A_x[4][4] = {
{1.00447563e+00, 1.40208911e-02, 0.00000000e+00, 9.73784303e-05},
{6.39181504e-01, 1.00447563e+00, 0.00000000e+00, 1.37291716e-02},
{-2.00253350e-03, -9.40752945e-06, 1.00000000e+00, 1.34265323e-02},
{-2.82332812e-01, -2.00253350e-03, 0.00000000e+00, 9.19205414e-01}
};
ïŒäžç¥ïŒ
float B_x[4][1] = {
{-0.00062615},
{-0.08827914},
{0.00368742},
{0.51951251}
};
ïŒäžç¥ïŒ
float Gain[4] = {28.38403323, 4.29034848, 0.09009136, 0.36309152};
çãããå¿
èŠã«å¿ããŠãããæžãæããã°ããããã§ãããªããæžãæããããpico-inverted-pendulum/KalmanFinal/build ãã£ã¬ã¯ããªã«ç§»åã㊠make ã³ãã³ããå®è¡ããã°ãuf2 ãã¡ã€ã«ãæŽæ°ãããŸãã
10. åç«æ¯åã®å¶åŸ¡ã®ã¿ã€ã ã¹ã±ãžã¥ãŒã«
ããŠãåç«æ¯åã®å¶åŸ¡ã®ããã«ã¯ã以äžã®3ã€ã宿çã«å®è¡ãããå¿
èŠããããæžç±ã§ã¯ä»¥äžã®åšæãšå®çŸæ¹æ³ã«ãªã£ãŠããŸããã
ã¿ã¹ã¯ | åšæ | æžç±ã§ã®å®çŸæ¹æ³ | Pico ã§ã®å®çŸæ¹æ³ |
ããŒã¿ãªãŒãšã³ã³ãŒãã®èªã¿åã | 25 ÎŒs | ã¿ã€ã1 | 2ã€ç®ã®CPUã³ã¢ã§ while + sleep |
ã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå® | 2.5 ms | ã¿ã€ã2 | ã¿ã€ã |
ã¢ãŒã¿ãžã®å¶åŸ¡ | 10 ms | whileæ + sleep | 1ã€ç®ã®CPUã³ã¢ã§ while + sleep |
ããã Pico ã«ç§»æ€ããå ŽåããŸãã¿ã€ãã1ã€ãããªãã®ãã©ãããã®ããåé¡ã§ããã
äžèšã®ã¿ã¹ã¯ã®ãã¡ããã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå®ããšãã¢ãŒã¿ãŒãžã®å¶åŸ¡ãã¯æéã«å¯ŸããŠæ£ç¢ºã«å®è¡ããªããã°ãªããŸããã
ãªãããšãããšãã«ã«ãã³ãã£ã«ã¿ã®èšç®åŒãç¶æ
æ¹çšåŒã«æééé Ît ãå
¥ã£ãŠããããããã® Ît ã®éãã«èšç®ãå®è¡ããªããã°ãªããªãããã§ãã
äžæ¹ãããŒã¿ãªãŒãšã³ã³ãŒãã®èªã¿åãã¯ããã»ã©æéã«æ£ç¢ºã§ããå¿
èŠã¯ãããŸããããå€ã®å€åãåãããŒããªãçšåºŠã«éãããšããæ¡ä»¶ãæºããããã°è¯ãã®ã§ãã
以äžãèžãŸããäžèšã®ããã«2ã€ã®CPUã³ã¢ã«èšç®ãå²ãæ¯ãããšã«ããŸããã
ãããããšã1ã€ç®ã®CPUã³ã¢ã§ã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå®ãšã¢ãŒã¿ãŒãžã®å¶åŸ¡ã®2ã€ã®ã¿ã¹ã¯ãããŸãããªããã°ãªããŸããã
çµè«ããèšããšãå¶åŸ¡åšæ 10ms 㯠Pico ã«ãšã£ãŠã¯éãããŸããããªã·ãã¹ã³ãŒãã®åºåããäœã£ãäžèšã®ã°ã©ããã芧ãã ããã
ã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå®ã 2.5ms ã§è¡ãããŠããããã®åéã« 4 åã® 10ms åšæã§å¶åŸ¡ãè¡ãããã®ã§ããã
è§åºŠã®æšå®ãè¡ã£ãŠããªãæéã«å¯ŸããŠãå¶åŸ¡ã«ãããæéãã®ãªã®ãªåãŸãçšåºŠã§é·ãããã®ã§ãããã®ã°ã©ãã§ã¯åé¡ãªãããã«èŠããã®ã§ãããã¢ãŒã¿ãŒãå®éã«åããšããã®ã°ã©ãã¯å€§ããä¹±ããŠããŸããŸãã
ãã®ãããªããã§ãããå°ãæéã«äœè£ãæãããå¶åŸ¡ã®æ¹ãè¯ãã ãããšèããå¶åŸ¡ã®åšæã 14msãã«ã«ãã³ãã£ã«ã¿ã«ããè§åºŠã®æšå®ã¯ãã® 1/4 ã®åšæ 3.5ms ã«ããŠã¿ãŸãããå®éã«å¶åŸ¡æã«ãªã·ãã¹ã³ãŒãã®åºåããäœæããã°ã©ããäžå³ã§ãã
å¶åŸ¡ã«æéã®äœè£ãããããšãèŠãŠåããŸãã
å®éã«ã¯ã10ms åšæã§ã 14ms åšæã§ãåç«æ¯åã¯å®å®ãããããã®ã§ãããå¿æã¡ 14ms åšæã®æ¹ãå®å®ããŠãããïŒãšããæ°ãããŸãã
以äžã®å
容ã¯ãããã°ã©ã äžã®äžèšã®éšåã®å€æŽã«ããå®çŸããŠããŸãã
//const float theta_update_freq = 400; //Hz (Not Used)
const float theta_update_interval = 0.0035;
(äžç¥ïŒ
float feedback_interval_sec = 0.014; //sec
float feedback_interval_sec_wait = 0.0122; //sec
æåŸã® feedback_interval_sec_wait ã® 0.0122 ãšããæ°å€ã¯ãå¶åŸ¡ãè¡ã while ã«ãŒãã® sleep ã«äœ¿ãããã®ã§ãããé©åãªå€ã¯ãªã·ãã¹ã³ãŒãã§æ³¢åœ¢ãäžã®ãããªæ³¢åœ¢ãèŠãªãã調ç¯ãããããªãã®ãåä»ã§ããã
11. ãããã«
ãããã§ãããïŒ
åç«æ¯åã®äœæã«ãèŠåŽããŸããããããã°ãæžãã®ãéåžžã«å€§å€ã§ããã
ããšããããåãããšããç¶æ
ãªãã°å²ãšæ©ããã¡ããå®çŸã§ããŠããŸãããå®éãæžç±ã®ãã©ã¡ãŒã¿ã®ãŸãŸã§ãããªããšãåç«æ¯åãå®å®ãããããšãã§ããã»ã©ã§ãã
ããããããã°ãæžãããã«çްãã調ã¹ãŠãããšãã«ã«ãã³ãã£ã«ã¿ãå¶åŸ¡ãäžå®åšæã§åããŠããªããªã©åé¡ãããããèŠã€ããããããäžã€äžã€æœ°ããŠãããªããã°ãªããªãããšãã£ãå
·åã§ãã
åé¡ãã€ã¶ããŠãããšãå°ããã€ã§ã¯ãããŸãããå¶åŸ¡ã®å®å®æ§ãå¢ããŠããã®ã坿ããšããã§ãã
ãããªæãã«èŠåŽããã ããã£ãŠã宿ããåç«æ¯åãžã¯ããªãã®æçããããŠããŸããç¹ã«å¿
èŠããªãã®ã«æ¯æ¥ã¡ããã¡ãã黿ºãå
¥ããŠåç«ãããŠããŸãã
çãããäžå®¶ã«äžå°ãåç«æ¯åãäœæããŠã¿ãŠã¯ãããã§ããããã