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秒あれば組立てから立たせるまでだいたいできます。
