単３乾電池1本で、たぶん1年以上動き続けるマーブルマシンです。
なもんで、けっこうエコかなと思います。
ちょっと自慢♪は・・・とっても正確に10.0秒ごとに
1個玉を転がします... 水晶発振器で制御してます ^^;
・玉・・・BB弾 Φ６ｍｍ
・材料は100キンの時計・・・時計モジュールを取り出し、針を短くカット
・玉リフターの観覧車・・・３Dプリンタ超軽量に作る必要があります。
骨組みは1層,2列・・・厚さ0.2mm、幅0.6mm
・時計フレーム・・・３Dプリンタ いい感じに設計しないといけません。
・レールは100キンのΦ２ｍｍアルミ
・レール枕木ホルダはアルミA2017、1mm厚板から格安中華NCで削り出しです。
コースの変更は割と簡単にできます。
M5stickくんボディーケース対応の
シンプルなサンプルです。。。
BLE関係とかグラフィックとか省いたんですが
変数とか使ってないのにいっぱい残ったままのトラップがある感じです。
コーディングのお作法の悪い例です。
調整途中で継ぎ足し消したりで同じ変数使えばいいとこ定数
直接書いてたり... なんとなくやってることわかればいいということで。
こういうのは真似しないのがいいです。
M5stickくんボディーケース ＋ M5stickC Plus + FS90R の
これまでブログに書いてきた組み合わせなら、たぶん・・・
そのままのパラメータでも立つんじゃないかと思います。
★下記にあるサンプルでは
サーボの配線はこれまでのブログに載せた感じで
ほどよく余計な部分は切って配線する必要があります。
配線が長いまま束ねたりすると多分重心が
だいぶずれてパラメータが合わなくなり立たないかな。
立たせる時の要領は昨日の動画参考にしてください。
M5stick部画面を上におよそ水平にして電源ボタンを入れます。
キャリブレーションが始まります。
終わると赤LEDが消えます。
電池電圧、傾き、制御ONorOFFが表示されます。
ここで OFFの状態で
だいたいどのくらいの立たせ角度でバランスを取りそうか
目安を電源入れる前に調べておきます。
OFFの右側の数字が立ち角度です。
0度になるおよその角度を覚えておきます。
±2度範囲くらいで立たせてやればだいたい立ちます。
OFF表示の右の数値がボディーの重心に対する傾きです。
後継が＋ 前傾がー です。およそ０度の目安を掴んでおきます。
ボタンA を２秒ほど押しておくと制御開始、ONになります。
そしたらおよそ重心中央で立てて置きます。2秒猶予あります。
ジジ・・・って制御が始まったら手を放します。
転んでまた立たせる時は
およそ垂直にしたら2秒猶予以内にボディを立てて置いて・・・の繰り返しです。
ArduinoIDEにはKalmanライブラリ、M5StickCPlusを入れておきます。
スケッチはこんな感じです。
// ★注意 重要★ 連続回転サーボはFS90R用
// 連続回転サーボSG90HVではパラメータが違います。
// 経験的にデジタルのSG90HVよりアナログのFS90Rの方が調整は容易です。
#include "M5StickCPlus.h"
#include <Kalman.h>
#define MPU6886 IMU
#define MPU6886_AFS M5.MPU6886.AFS_2G
#define MPU6886_GFS M5.MPU6886.GFS_250DPS
#define MOTOR_PIN_L 0
#define MOTOR_PIN_R 26
#define BTN_A 37
#define M5_LED 10
#define OFFSET 25
float acc[3];
float accOffset[3];
float gyro[3];
float gyroOffset[3];
float kalAngle;
Kalman kalman;
long lastMs = 0;
long tick = 0;
float kspd=0.0;
float kdst=0.0;
float roll_base = -100;
float roll_offset=0;
float fil_N=10;
int motorLdir=0, motorRdir=0,L_wink_F,R_wink_F;
int16_t motor_offsetL=0,motor_offsetR=0,motordir;
float power_base;
unsigned char ms_dt=10;
float roll, pitch, yaw,roll_p,roll_i,roll_d,roll_old,power,power_calc,batt,roll_filter,roll_iLPF,roll_iHPF,data_dstLPF,data_dstHPF;
int16_t powerL,powerR,ipowerL,ipowerR,reset_wait_F,reset_wait_count,wait_count;
float data_rollp,data_rolld,data_spd,data_dst,move_rate,move_Tg,move_count,spin_rate;
unsigned char Duty,LED,motor_sw,motor_stop,i_reset;
unsigned long ms10,ms100,ms500,ms1000,ms2000;
char str_No=0,strPtr[10];
static unsigned char RX_ID;
static short RX_Val,test_No,motor_power,spin_count;
float varSpd ,varDst ,varIang ,aveAbsOmg=0.0, yawAng=0.0,varAng,yawAngx10=0.0;
float varSpd_f,varDst_f,varIang_f,varOmg,varSpdK,varAngK,varOmgK,power_LPF;
float Kang=35.0;
float Komg=0.8;
float KIang=700.0;
float Kdst=5.0;
float Kspd=1.4;
float Kyaw=1.0;
float varInteg=0.0;
float dt=0.01;
float Kyawtodeg=0.069;
float Kspin = 0.0;
float spinTarget=0;
float spin_L,spin_R;
void setup() {
M5.begin();
pinMode(BTN_A, INPUT_PULLUP);
pinMode(M5_LED, OUTPUT);
pinMode(MOTOR_PIN_L, OUTPUT);
pinMode(MOTOR_PIN_R, OUTPUT);
M5.MPU6886.Init();
M5.MPU6886.SetGyroFsr(MPU6886_GFS);
M5.MPU6886.SetAccelFsr(MPU6886_AFS);
M5.Axp.ScreenBreath(8);
M5.Lcd.setTextFont(4);
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(0, 60);
M5.Lcd.println("Calibrating");
delay(500);
calibration();
readGyro();
kalman.setAngle(getRoll()); lastMs = micros();
M5.Lcd.setRotation( 2 );
M5.Lcd.fillScreen(BLACK);
ms10 = millis();
ms100 = millis();
ms500 = millis();
ms1000 = millis();
ms2000 = millis();
digitalWrite(M5_LED, HIGH);
}
void loop() {
readGyro(); applyCalibration();
float dt = (micros() - lastMs) / 1000000.0; lastMs = micros();
float kalman_roll = getRoll();
kalAngle = kalman.getAngle(kalman_roll, gyro[0], dt)+91;
roll = kalAngle + (roll_base + roll_offset) /10.0f;
roll_filter = (roll + roll_filter*(fil_N - 1)) /fil_N;
varAng = roll_filter;
if (millis() > ms10) {
if(-30 < roll_filter && roll_filter < 30) {
wait_count += 1;
if( wait_count > 200 ) { wait_count = 201;
varSpd += power * 0.01 ;
varDst += Kdst * varSpd * 0.01 ;
varInteg += ( Kdst * varSpd + KIang * varAng ) * 0.01;
varSpdK = Kspd * varSpd;
varAngK = Kang * varAng;
varOmgK = Komg * varOmg;
power = varInteg + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
power = constrain(power, -800, 800);
if(motor_sw == 1) {
if(abs(Kspin) > 20) {yawAng=1; yawAngx10=0;}
else Kspin = 0;
powerL = -power + motor_offsetL +1450 - int16_t(Kyaw *yawAng) +Kspin +move_rate; // 東京メイカフェアで完成品購入された方は1450 ではなく1500 辺り。
powerR = power + motor_offsetR +1450 - int16_t(Kyaw *yawAng) +Kspin -move_rate; // 東京メイカフェアで完成品購入された方は1450 ではなく1500 辺り。
powerL = constrain(powerL, 800, 2200);
powerR = constrain(powerR, 800, 2200);
bool doneR=false; bool doneL=false;
uint32_t usec=micros();
digitalWrite(MOTOR_PIN_L, HIGH);
digitalWrite(MOTOR_PIN_R, HIGH);
while(!doneR || !doneL) {
uint32_t width=micros()-usec;
if (width>=powerL) {
digitalWrite(MOTOR_PIN_L, LOW);
doneL=true;
}
if (width>=powerR) {
digitalWrite(MOTOR_PIN_R, LOW);
doneR=true;
}
}
}
else{
digitalWrite(MOTOR_PIN_L, LOW);digitalWrite(MOTOR_PIN_R, LOW);
}
}
}
else{
wait_count = 0; move_rate = 0; motorLdir = 0; motorRdir = 0;
roll_p=0;roll_i=0;roll_d=0;data_spd=0;data_dst=0;i_reset=0,roll_iLPF=0;roll_iHPF=0;data_dstLPF=0;data_dstHPF=0;move_Tg=0;
power = 0; varSpd= 0; varDst= 0; varIang=0; yawAng=0.0,yawAngx10=0.0; varInteg=0;
}
ms10+=ms_dt;
}
if (millis() > ms100){
M5.Lcd.setCursor(0, 0);
if( motor_sw == 1) M5.Lcd.printf("ON ");
else M5.Lcd.printf("OFF ");
M5.Lcd.printf("%5.1f \n",varAng);
M5.Lcd.printf("%3.1fv ", batt);
M5.Lcd.printf("%4.1f ", yawAng);
ms100+=100;
}
if (millis() > ms1000){
roll_i=0;
ms1000+=1000;
}
if (millis() > ms2000){
if( digitalRead(BTN_A) == 0) motor_sw = !motor_sw;
batt = M5.Axp.GetVbatData() * 1.1 / 1000;
ms2000+=2000;
}
}
void calibration(){
float gyroSum[3];
float accSum[3];
for(int i = 0; i < 500; i++){
readGyro();
gyroSum[0] += gyro[0];
gyroSum[1] += gyro[1];
gyroSum[2] += gyro[2];
accSum[0] += acc[0];
accSum[1] += acc[1];
accSum[2] += acc[2];
delay(2);
}
gyroOffset[0] = gyroSum[0]/500;
gyroOffset[1] = gyroSum[1]/500;
gyroOffset[2] = gyroSum[2]/500;
accOffset[0] = accSum[0]/500;
accOffset[1] = accSum[1]/500;
accOffset[2] = accSum[2]/500 - 1.0;
}
void readGyro(){
M5.MPU6886.getGyroData(&gyro[0], &gyro[1], &gyro[2]);
M5.MPU6886.getAccelData(&acc[0], &acc[1], &acc[2]);
varOmg = (gyro[0]-gyroOffset[0]);
yawAngx10 += (gyro[1] - gyroOffset[1]) * dt;
yawAng = yawAngx10*Kyawtodeg;
}
void applyCalibration(){
gyro[0] -= gyroOffset[0];
gyro[1] -= gyroOffset[1];
gyro[2] -= gyroOffset[2];
acc[0] -= accOffset[0];
acc[1] -= accOffset[1];
acc[2] -= accOffset[2];
}
float getRoll(){
return atan2(acc[1], acc[2]) * RAD_TO_DEG;
}
float getPitch(){
return atan(-acc[0] / sqrt(acc[1]*acc[1] + acc[2]*acc[2])) * RAD_TO_DEG;
}
これでわかるかな。。。
サーボ中点パルス時間は仕様書には1500μsec になってるけど、
だいたい1450くらいが多いのでサンプルは1450にしてます。
★注意：東京メイカフェアで完成品で購入された方のモノは
およそ1500に一旦は調整してます。
ただ、これも多少数十μsecくらいは簡単にずれるのですが。
あと・・・
組立てて・・・・
立てるとこまでの一連の動画をここにも貼っておきます。
動画、組み立ててる最中にタイヤ（Oリング）外れちゃったんで
はめ直してます。サーボホーンに掛けてるだけなんで簡単に外れちゃいます。
でもって立たせようとしたら、、、あれ？立たないって感じになってますが
Aボタンで倒立制御をONにするの忘れちゃってました。
スイッチ入れ忘れとか、タイヤ外れとかなければ
30秒あれば組立てから立たせるまでだいたいできます。
M5stickくんのボディケースがメルカリでちょっと売れてきたので
使い方解説動画作ってみました。
サーボの配線は昨日のブログにあります。
円形サーボホーンには適当に円径より数mm小さいOリングをはめてるだけなんで
簡単に外れてます・・・すぐ付けれますが...接着剤で付けた方がいいですね。
配線が終わって、サーボホーンにOリング付けたら、後の組み立ては
1分掛からないくらいです。
正味の組み立ては途中のOリング外れがなければ10秒程度かな。。。
スナップフィットなんでネジとかもありません。
もし...サーボのはめ込みが緩い場合は輪ゴムとか掛けてください...
動画では倒立制御ONにするの忘れてOFFのまま、
一回立たせようとして、
あれ？動き出さない？ ・・・で、ONにして立たせてます・・・
動画でのプログラム、、、
にこちゃん顔グラフィック、BLE_Blynk通信、BLEシリアル通信、OTAの無線書き込みとかゴミがいっぱい入ってるのをだいたい取ったサンプル作ってます。
またここに載せます。
ぼくのプログラムはお作法でたらめで
あんまり見せれるもんじゃないですが・・・
ちょっと驚いたこと、その上記のゴミとか取ったらコンパイル、
書き込みがめちゃ早くなった・・・
そういえば最初の頃は早かったんだった。
Fritzing 電子工作ではよく回路構成の図で使われてるの見かけます。
でもって使い方もとっても簡単らしい。
M5stickくんの配線図とか書いてみようとダウンロード、インストールした
でも流石に全く説明見ずには無理があった。
なかなか思った通りに絵が描けない。配線も実はちゃんとつながってないっぽい。
Fritzing・・・使い方ちゃんと後で調べよう。。。
背面視での図です。
右サーボ（図では左）はポート26
左サーボ（図では右）はポート0
に接続。
あとはGND,５Vに電源接続 それだけです。
今まで何度も写真 載せてるのでその方がわかりやすいかな・・・？
M5stickくんケース、
３Dプリンタ持ってない人には十分500円でもうれしいと思いますよ・・・
ってことだったのでメルカリに出してたら10個の大台に乗った♪
だいたい、いまのとこ2日に1個のペース。 ありがたい。
買って頂いた人はどんな風に使ってるのかちょっと気になる。
M5stick使い慣れてる人には倒立振子はなんてことないと思うけど。。。
マイコン初心者の方は・・・
最初は無理に倒立振子にせず
ラジコンにして慣れるのもいいかもです。
BLEラジコンにするなら特にフィードバック制御とか不要です。
相補フィルタやカルマンフィルタもいらない。
タイヤもちょっと大きなの付けると割と小気味よく動くラジコンになります。
また倒立振子もタイヤが大きいと
制御もとても簡単になって
ロバスト性・・・外乱にもすごく強くなります。
デジタルのSG90-HVサーボでは自分はサーボホーンタイヤでは
ちょっと不安定に立つのが精一杯でした。
アナログのFS90Rにするとだいぶ簡単になったけど。
そのSG90サーボでもタイヤが大きくなるとかなり楽になります。
振動周期が長くなり・・・倒れるのに時間がかかるようになり、
動きはタイヤ径アップで速くなるので。
タイヤ大き目ので遊んでみるのもいいかと思います。
さらに・・・
サーボではなくただのギヤモータFM90とDRV8830とかの
モータドライバを組み合わせるともっと制御が楽になります。
応答が一桁上がる感じになって駆動の分解能も一桁上がる感じになるので。
けっこう強く押しても転ばない感じになります。
魔改造の夜 の生贄（課題）に おもちゃの部で選ばれてたちぃぺんちゃん...
あれって実はすごくよくできています。
イワヤ株式会社さんのちぃペンちゃんの動作です。
モータ1個の連続回転から、 歩行、歩行を止めて羽ばたきと鳴き,頭の揺動を交互に繰り返します。 スゴイよくできたからくりですね！
製品としての動きはイワヤさんのホームページにあります。
ちょっとこちらでは中身の動きに注目です。
申し訳ないですが毛皮を脱いで頂きました。
マイコン、サーボ等で作るなら簡単です。
自分ならどうやってモーター1個であの動作をさせる？って考えると
けっこー難しいです。
ああやってのかなぁなんて思いながら分解しました。
主要部品一式です。
動力分割機構を取り出しました。なるほどねうまく作られてます・・・すごいです。
パッと見でこの動力分割機構動作であーあれねって人は
けっこーからくり好きですね。心臓部のスライドカム機構です。
これで約20回転と約15回転の比でギヤチェンジします。
車のミッションなんかのシンクロメッシュと違うとこは
成り行きメッシュ...かな。扱う力に対してギヤ・・・見た感じ
材料はポリアセタールかな...が十分強度が高いので
適当に押し込んでも歯は欠ける心配もなくそれなりに噛み合っていきます。
すごくうまいことコンパクトに作られてます。
こういうの電子制御、マイコン、サーボでやるのは簡単ですが サイズ、コストはとうていかないません。 イワヤ株式会社さん、、、 やはり製品にまで持っていって商売にするには 知恵が詰まってます、すごいです。
まじめに電動でこの手の動作切り替えがあるおもちゃを分解してみたのは初めてだけど
よくある手法なのかな... ちょっと特許とか検索しても見当たらない。
動力分割・・・オートマチックミッションギヤモジュールです。
ミソはここ、重ねたギヤの歯数が2枚で1歯だけ数が違います。
同じピニオンで駆動されると1回転で1歯分だけ位相差が互いにずれます。
カムがくりぬき部を過ぎるとカムが重なったギヤを押し上げます。
結果、噛み合うギヤを変えることができます。
カムがくり抜き穴まで来るとバネで押し戻され・・・また繰り返しです。
理屈がわかれば多重にすればいろんな組み合わせ回転数切り替えのからくり・・・
機械で動作パターンプログラムが作れる。
似たような構成・・・波動歯車の作用の原理を応用って感じです。
波動歯車・・・というより登録商標のハーモニックドライブの方が有名。
大きなロボットなんかでよく使われます
昔高減速比でバックラッシュなし、、、試作品設計で使ったけどえらい高価...
ただ効率はそんなに良くない。。。
ギヤ規格、モジュールは0.5くらいかな？
なもんでこれってギヤのモジュールは一般の規格じゃ
作られてないカスタム品なのかな。
そんな面倒なことはせず同じモジュールで微妙に直径が違うけど
そのまま嚙み合わせに余裕持たせて使うのが現実的かな...
何かと、すぐマイコン、サーボ使いたがりなんで改めて
機械構造、機構設計考える時のいい勉強になりました。
ツィーザー・・・言葉は元々は医療用で使うピンセット？みたいですが
多機能ピンセットです。電子工作には何かと小便利です。
10年ほど前に買ったモノが電池が切れたので交換した時の中身の写真です。
一般的なテスターの機能の他、容量、インダクタンスが測れたりします。
ありがちなシーン...チップ部品の両端の電圧測るとか...
ふつうのテスターではやりにくいですが簡単です。
細く先がとがってるのでギヤモータとかのインダクタンス、抵抗ってどんなだっけ？
ってのがコネクタに挿して測れたりします。
最近のはOLEDで見やすいし、発振器が付いてたりBLEで通信できたりしますね。
USBで充電式で電池交換なんていらないし・・・
蓋を開けたところ回路モジュールとケースに分けたところ 表
蓋を開けたところ回路モジュールとケースに分けたところ 裏
FM90 ギヤモータのインダクタンス、抵抗を測定してるところ
