2020年03月


以前作っていた、バランスロボットをついに立たせることが出来ました。




S__43638798

S__43638800

S__43638801


ハードは、下記のサイトを参考にしました。
M5StickC バランスロボ

ソフトは、下記のサイトを参考にしました。
実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」

M5StickCはMadgwickフィルタで倒立振子の夢を見るか?


■スケッチ

#include <M5StickC.h>

//PIDパラメータ調整
#define OFFSET             90.0                    // 重心 後方-←90°→+前方
#define COEF_P             35.0                    // Propotional
#define COEF_I             1.5                     // Integral
#define COEF_D             50.0                    // Differential

//Motor
#define LED_PIN            10                      // 内蔵LED

#define MOTOR_R 0x65 //A0=low, A1=open
#define MOTOR_L 0x63 //A0=high, A1=open


//MPU6886 https://github.com/m5stack/M5StickC/blob/master/src/utility/MPU6886.h
#define MPU6886_AFS        M5.MPU6886.AFS_2G       // Ascale [g]      (±2,4,8,16)
#define MPU6886_GFS        M5.MPU6886.GFS_1000DPS  // Gscale [deg/s]  (±250,500,1000,200)
#define DPS                1000                    // Gscale [deg/s]
#define CALIBRATION_COUNT  1000

//Function
#define min(a,b)           ((a) < (b) ? (a) : (b))
#define max(a,b)           ((a) > (b) ? (a) : (b))

//キャリブレ結果
double offsetX;              //Gyro X
float  base_angle;           //Accl X
//ローテーション結果
float dpsX;                  //Gyro X
float angleX;                //Accl X
//積分
double gyro_angle_x = 0;
float  preInterval  = 0;

// I2Cドライバー
void drvMotor(byte adr, int16_t pwr) { //pwr -255 to 255
  byte dir, st;
  if (pwr < 0) dir = 2;
  else dir =1;
  byte ipower=(byte) (abs(pwr)/4); //
  if (ipower == 0) dir=3; //brake

  //constrain(x, a, b) xがa以上b以下のときはxがそのまま返ります。xがaより小さいときはa、bより大きいときはbが返ります。
  ipower = constrain (ipower, 0, 63);
  st = drv8830read(adr);
  if (st & 1) drv8830write(adr, 0, 0);
  drv8830write(adr, ipower, dir);
}

void drv8830write(byte adr, byte pwm, byte ctrl) {
  Wire.beginTransmission(adr);
  Wire.write(0);
  Wire.write(pwm*4+ctrl);
  Wire.endTransmission(true);
}

int drv8830read(byte adr) {
  Wire.beginTransmission(adr);
  Wire.write(1);
  Wire.endTransmission(false);
  Wire.requestFrom((int)adr, (int)1, (int)1);
  return Wire.read();
}


void setup() {
  //初期化
  M5.begin();
  M5.Lcd.setRotation(1);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("<B-Robot v1>");
  //Motor設定
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

// I2C設定
  Wire.begin(0, 26, 400000); //SDA, SCL
//  Wire.setClock(50000);
  drvMotor(MOTOR_R,0);
  drvMotor(MOTOR_L,0);

  //MPU設定
  M5.MPU6886.Init();
  M5.MPU6886.SetGyroFsr(MPU6886_GFS);
  M5.MPU6886.SetAccelFsr(MPU6886_AFS);
  delay(1000);

  //キャリブレーション
  calibration();
//  base_angle-=1.5;
//  offsetX+=0.6;

  M5.Lcd.setRotation(2);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 140);
  M5.Lcd.printf("BT%3.1fV\n", M5.Axp.GetBatVoltage());
  face();
}

void loop() {
  static  int   Duty;
  float         power, powerP, powerD;
  static float  powerI = 0;
  double        angle;

  //MPUローテーション
  calcRotation();
  //モーター制御
  angle = angleX;
  M5.Lcd.setCursor(0, 0);
  if ((base_angle>=0)&&(base_angle<=1.0)) {
    M5.Lcd.setTextColor(GREEN);
  } else {
    M5.Lcd.setTextColor(RED);
  }
  M5.Lcd.printf(">%3.1f", base_angle);

  if ((angle - OFFSET > -20) && (angle - OFFSET < 20)) {    // 30度以下だけモーター駆動
    powerP = (angle - OFFSET ) / 90;                        // angle=0~180  → -1~1
    powerI += powerP;
    powerD = dpsX / DPS;                                    // 角速度 → -1~1
    power = powerP * COEF_P + powerI * COEF_I + powerD * COEF_D;
    power = max(-1, min(1, power));                         // -1~1
    //モーター駆動
    Duty = (int)(255* power );
    drvMotor(MOTOR_R,Duty);
    drvMotor(MOTOR_L,Duty);
    digitalWrite(LED_PIN, HIGH);
  } else {                                                  // 転倒したら停止
    drvMotor(MOTOR_R,0);
    drvMotor(MOTOR_L,0);
    powerI = 0;
    digitalWrite(LED_PIN, LOW);
  }
  M5.update();
  if (M5.BtnA.wasReleased()) {
    M5.Lcd.fillScreen(GREEN);
    ESP.restart();
    while (true) ;
  }
}

//キャリブレーション関数
void calibration() {
  float raw_gyro_x, raw_gyro_y, raw_gyro_z;
  float raw_acc_x, raw_acc_y, raw_acc_z;
  float acc_x = 0, acc_y = 0, acc_z = 0;
  offsetX = 0;
  for (int i = 0; i < CALIBRATION_COUNT; i++) {
    M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z);
    M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z);
    //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換含む
    offsetX += -raw_gyro_x;
    acc_x   += -raw_acc_x;
    acc_y   +=  raw_acc_y;
    acc_z   += -raw_acc_z;
  }
  //gyro offset
  offsetX /= CALIBRATION_COUNT;
  //加速度から角度を算出
  acc_x   /= CALIBRATION_COUNT;
  acc_y   /= CALIBRATION_COUNT;
  acc_z   /= CALIBRATION_COUNT;
  base_angle = (atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI) + 180;
}

//MPUローテーション関数
void calcRotation() {
  float raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
  float acc_angle_x;
  M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z);
  M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z);
  //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換
  raw_gyro_x *= -1;
  raw_acc_x  *= -1;
  raw_acc_z  *= -1;
  //
  acc_angle_x = (atan2(raw_acc_y, raw_acc_z + abs(raw_acc_x)) * 360 / 2.0 / PI) + 180;
  dpsX = raw_gyro_x - offsetX;
  //積分
  float now_time = millis();
  float interval = now_time - preInterval;  //前回からの経過時間
  preInterval = now_time;
  gyro_angle_x += dpsX * (interval * 0.000999999);
  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x) + base_angle;
}

void face(){
 int x0=10;
 int y0=40;
 int w0=25;
 int h0=10;
 int col=TFT_ORANGE;
 M5.Lcd.fillRect(x0,y0,w0,h0,col);
 M5.Lcd.fillRect(x0+35,y0,w0,h0,col);
 M5.Lcd.fillCircle(x0+12,y0+25, 7,col);
 M5.Lcd.fillCircle(x0+12,y0+25, 5,BLACK);
 M5.Lcd.fillCircle(x0+48,y0+25, 7,col);
 M5.Lcd.fillCircle(x0+48,y0+25, 5,BLACK);
 M5.Lcd.fillTriangle(x0+30,y0+25, 30,90,50,90,col);
 M5.Lcd.fillRect(20,y0+60,40,15,col);
}


・最近の新型コロナの影響で非接触型の温度計を作ってみました。
 毎日の体温の目安になるのではと思っています。

■画面の説明
上から
①周囲温度
②ターゲット温度
③MIN,MAX温度
④釦Aを押したときのターゲット温度(Hold)

*釦Bを押すとリスタートし、データをクリア出来ます。


S__43614232


S__43614231


mlx90615


■使用部品
①M5StickC
③ピンヘッダ、ピンソケット(手持ちの部品)


■ライブラリは、下記のものを使いましたが、そのままではエラーが発生しましたので
ヘッダーファイルを編集しました。


下記の三つのファイルを同じフォルダに保存してコンパイルしました。
mlx90615.ino
mlx90615.h
mlx90615.cpp

■スケッチ

//mlx90615.ino

// 2020/03/06 Yoshito Hashiguchi

#include <M5StickC.h>
#include <Wire.h>
#include "mlx90615.h"

MLX90615 mlx;

float maxtemp=0.0;
float mintemp=100.0;
float hold=0.0;
float objtemp=0.0;
float ambitemp=0.0;

void setup() {

  Serial.begin(115200);
  M5.begin();
  M5.Lcd.setRotation(0);
  M5.Lcd.fillScreen(BLACK);

  Wire.begin(0, 26, 400000); //SDA, SCL
  Serial.println("Melexis MLX90615 infra-red temperature sensor");
  Serial.print("Sensor ID number = ");
  Serial.println(mlx.get_id(), HEX);
}

int ct=0;

void loop() {

  M5.update();
  if ( M5.BtnA.wasPressed() ) {
    hold=mlx.get_object_temp();
  }
  if ( M5.BtnB.wasPressed() ) {
    esp_restart();
  }
  ct++;
  if (ct>100) {
    ambitemp=mlx.get_ambient_temp();
    objtemp=mlx.get_object_temp();
    if (mintemp>objtemp) mintemp=objtemp;
    if (maxtemp<objtemp) maxtemp=objtemp;
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(0,0);
    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(GREEN); 
    M5.Lcd.printf("ambient temp\n");
    M5.Lcd.setTextSize(3);M5.Lcd.setTextColor(WHITE);   
    M5.Lcd.printf("%3.1f\n",ambitemp);
    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(GREEN); 
    M5.Lcd.printf("\nterget  temp\n");
    M5.Lcd.setTextSize(3);M5.Lcd.setTextColor(WHITE);   
    M5.Lcd.printf("%3.1f\n",objtemp);
    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(CYAN);  
    M5.Lcd.printf("\nBtnB=Restart\n");
    M5.Lcd.setTextColor(GREEN); 
    M5.Lcd.printf("Battery:%3.1fV\n", M5.Axp.GetBatVoltage());

    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(GREEN);  
    M5.Lcd.printf("\n min   max\n"); 
    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(WHITE);   
    M5.Lcd.printf(" %3.1f  %3.1f\n",mintemp,maxtemp);
    
    M5.Lcd.setTextSize(1);M5.Lcd.setTextColor(CYAN);   
    M5.Lcd.printf("\nBtnA = hold\n");
    M5.Lcd.setTextSize(3);M5.Lcd.setTextColor(WHITE);     
    M5.Lcd.printf("%3.1f\n",hold);

 
    ct=0;
  }
  delay(10);
}

■ヘッダーファイル

//mlx90615.h

#ifndef __MLX90615_H__
#define __MLX90615_H__

#include "Wire.h"

// default MLX90615 I2C address
#define MLX90615_I2C_ADDR 0x5B

// ROM - ID number
#define MLX90615_REG_ID_LOW 0x1E
#define MLX90615_REG_ID_HIGH 0x1F
// RAM - ambient temperature register
#define MLX90615_REG_TEMP_AMBIENT 0x26
// RAM - object temperature register 
#define MLX90615_REG_TEMP_OBJECT 0x27

class MLX90615 {
 public:
  MLX90615(uint8_t i2c_addr = MLX90615_I2C_ADDR);
  void begin();
  uint32_t get_id();
  float get_object_temp();
  float get_ambient_temp();

 private:
  uint8_t i2c_addr_;
  uint16_t read_word16(uint8_t reg);
};
#endif // __MLX90615_H__

■C++ソースファイル

//mlx90615.cpp

#include "mlx90615.h"

MLX90615::MLX90615(uint8_t i2c_addr) {
  i2c_addr_ = i2c_addr;
}

void MLX90615::begin() {
  Wire.begin();
}

uint32_t MLX90615::get_id() {
uint32_t id;
id = read_word16(MLX90615_REG_ID_LOW);
id |= (uint32_t)read_word16(MLX90615_REG_ID_HIGH) << 16;
return id;
}

float MLX90615::get_ambient_temp() {
  float temp;
  temp = read_word16(MLX90615_REG_TEMP_AMBIENT) * 0.02 - 273.15;
  return temp;
}

float MLX90615::get_object_temp() {
  float temp;
  temp = read_word16(MLX90615_REG_TEMP_OBJECT) * 0.02 - 273.15;
  return temp;
}

uint16_t MLX90615::read_word16(uint8_t reg) {
  uint16_t data;
  Wire.beginTransmission(i2c_addr_);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(i2c_addr_, (uint8_t)3);
  data = Wire.read();       // read low byte
  data |= Wire.read() << 8; // read high byte
  Wire.read(); // read and discard PEC (packet error code)
  return data;
}





・M5StickC FM90 BD6211 バッテリー を使って倒立振子を作ってみました。

■最初M5StickCのバッテリーだけで実験していたのですが、直ぐに切れてしまい
 面倒だったので、バッテリーを積んでみました。









br-1


br-2


■使用部品
①M5StickC
②ギヤードモーター FM90(秋月)
③タイヤ FS90R対応用(秋月)
④フルブリッジドライバ 7V BD6211F-D2(秋月)
⑤SOP8P DIP変換基板(秋月)
⑥バッテリー3.7V 1600mAh(中古モバイルバッテリー部品取り)
 ・バッテリーは、HAT端子に接続
⑦3Dプリンタ作成ケース
⑧その他配線部品等


参考は、下記URL


この中のパラーメータ変更
//PIDパラメータ調整
#define OFFSET 86.0  //91.5=>86.0 // 重心 後方-←90°→+前方
#define COEF_D 55.0 //50  ->55   // Differential

  //キャリブレーション
  calibration();
  base_angle-=1.2; //追加調整
  offsetX+=0.6;          //追加調整

↑このページのトップヘ