今回は、姿勢制御に引き続き、ToF距離計測しつつ自律走行させてみた。
備忘録として、コードを掲載する。
Atom_liteの電源は、スマホUSB電源で動いた。

距離計測自律走行....まだ本気出てないけど...#mini_pupper pic.twitter.com/wIcjcnJUuU
— ウワン (@MuAuan) March 19, 2022

ToF距離計測

スケッチ例は、以下の通り
事前にarduinoIDEのライブラリ管理で VL53L0X.h をインストールする。

#include <Wire.h>
#include <VL53L0X.h> // by Pololu http://librarymanager/All#vl53l0x-arduino https://github.com/pololu/vl53l0x-arduino
VL53L0X sensor;
void setup()
{
  Serial.begin(115200);
  Wire.begin(26,32);  //Atom  (32, 33); // M5StickC
  sensor.setAddress(0x29);
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {}
  }
  sensor.startContinuous();
}
void loop()
{
  Serial.print(sensor.readRangeContinuousMillimeters());
  if (sensor.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }
  Serial.println();
}

距離計測しつつ自律歩行

#include <M5Atom.h>
#include <Wire.h> 
#include <Adafruit_PWMServoDriver.h>
#include <VL53L0X.h> // by Pololu http://librarymanager/All#vl53l0x-arduino https://github.com/pololu/vl53l0x-arduino

VL53L0X sensor;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//Servoのマップ関数の下限と上限の角度に相当するパルス幅相当digitの値
#define SERVOMIN 102
#define SERVOMAX 492

int Distance_ToF;

//時間の経過を示す変数
int sk =0;
float kx=0,ky=0, lx=0, ly=0;
float pk=1.; //pk=1 ; 歩幅初期値
int st=36; //st=36　；繰り返し初期値
float st2=st*0.5; //繰り返しの折り返し
float st4=st*0.25; //位相初期値
float D10=50; //delay

//MPU params
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw; //, Temperature;
float acc_x0=0, acc_y0=0, acc_z0=0;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float rawX, rawY, rawZ;
float pitch, roll;

float target_r = 0; 
float target_p = 0;   
float P_r, I_r, D_r, preP_r, dt;
float P_p, I_p, D_p, preP_p;
float preTime;

//ジーグラー・ニコルズの手法で決定後Kd大きめに設定
float Ku = 0.2;  //0.55;  //7;
float Td = 400;
float Kp = Ku*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
float Ki = Ku*1.2/0.12;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd = 3*Ku*0.12/Td;  //point3 // Dゲイン 激しく動いたときのブレーキの役割をする

float Ku_ = 0.7; //1.4; //2.8; //1.4;  //7;
float Tu_ = 0.200;
float Kp_ = Ku_*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
float Ki_ = 1.2*Ku_/Tu_;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd_ = 3*Ku_*Tu_/40;  //point3 // Dゲイン 激しく動いたときのブレーキの役割をする

#include <Ps3Controller.h>
int player = 0;
int battery = 0;

void servo_write_func(float p0,float p1,float p2,float p3,float p4,float p5,float p6,float p7,float p8,float p9,float p10,float p11){
  servo_write(0, p0);
  servo_write(1, p1);
  servo_write(2, p2);
  servo_write(3, p3);
  servo_write(4, p4);
  servo_write(5, p5);
  servo_write(6, p6);
  servo_write(7, p7);
  servo_write(8, p8);
  servo_write(9, p9);
  servo_write(10, p10);
  servo_write(11, p11);
}

float p0_=90,p1_=90, p2_=70;
float p3_=95,p4_=90, p5_=60;
float p6_=85, p7_=100, p8_=110;
float p9_=86, p10_=100, p11_=110;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0;
      Serial.println(param);
      Serial.println("stay");
    }

    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up ){
      param += 1;
      Serial.println(param); Serial.println("\n");
      Serial.println("Change param up");
    }
    if( Ps3.event.button_down.down ){
      param += -1;
      Serial.println(param); Serial.println("\n");
      Serial.println("Change param down");
    }
    if( Ps3.event.button_down.right ){
      D10 *= 2.;
      Serial.println(D10);Serial.println("\n");
      Serial.println("Change Delay up");
    }
    if( Ps3.event.button_down.left ){
      D10 *= 0.5;
      Serial.println(D10);Serial.println("\n");
      Serial.println("Change Delay down");
    }
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
  Wire.begin(26,32); //i2cのpin宣言が重要
  Serial.begin(115200);
  sensor.setAddress(0x29);
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {}
  }
  sensor.startContinuous();
  
  setupMPU9250();
  readData();
  acc_x0=acc_x;
  acc_y0=acc_y;
  acc_z0=acc_z-1;
  
  pwm.begin();
  pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）  

  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("24:a1:60:54:2a:8a"); //PS3コントロール　Atom　Mac
  Serial.println("Ready.");

  param = 0; 
}

float func_sin(float skf){
  return sin(3.1415926535*skf/180);
}

void walk_mpu(){
  float p0_w,p1_w,p2_w,p3_w,p4_w,p5_w,p6_w,p7_w,p8_w,p9_w,p10_w,p11_w;
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  pk=0.5;
  dt = (micros()-preTime)/1000000. ;
  preTime = micros();

  P_r  = (target_r - roll) ; /// 90;      // target - roll -90~90を取るので180で割って-1.0~1.0にする
  I_r += P_r * dt;                    // 偏差を積分する
  D_r  = (P_r - preP_r) / dt;           // 偏差を微分する
  preP_r = P_r;                       // 偏差を記録する
  if (abs(Ki*I_r)>120) I_r=0;

  P_p  = -(target_p - pitch); // / 90;      // target - roll -90~90を取るので180で割って-1.0~1.0にする
  I_p += P_p * dt;                    // 偏差を積分する
  D_p  = (P_p - preP_p) / dt;           // 偏差を微分する
  preP_p = P_p;                       // 偏差を記録する
  if (abs(Ki*I_p)>120) I_p=0;

  float lx_, ly_;

  lx_ = Kp * P_r + Ki * I_r + Kd * D_r;
  lx_ = constrain(lx_, -120, 120);  // ±90に制限
  ly_ = (Kp_ * P_p + Ki_ * I_p + Kd_ * D_p);
  ly_ = constrain(ly_, -120, 120);  // ±90に制限
  
  Serial.print(" roll="); Serial.print(roll);
  Serial.print(" pitch="); Serial.print(pitch);Serial.print("\n");
  Serial.print(" lx_="); Serial.print(lx_);
  Serial.print(" ly_="); Serial.print(ly_);Serial.print("\n");
  //lx = 10*roll;
  //ly = -10*pitch;
  p0_w=p0_-10-0.5*pk*(st2-int(sk-st2)%st); p1_w=2*pk*(st2-int(sk-st2)%st)+p1_, p2_w=1*pk*(st2-int(sk-st2)%st)+p2_;
  p3_w=p3_+10-0.5*pk*(st2-int(sk-st2)%st); p4_w=2*pk*(sk%st-st2)+p4_; p5_w= 1*pk*(sk%st-st2)+p5_;
  p6_w=p6_; p7_w=-2*pk*(int(sk-st4)%st-st2)+p7_; p8_w= -1*pk*(int(sk-st4)%st-st2)+p8_;
  p9_w=p9_; p10_w=-2*pk*(st2-int(sk-st2-st4)%st)+p10_; p11_w= -1*pk*(st2-int(sk-st2-st4)%st)+p11_;
  servo_write_func(
    p0_w,p1_w-10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p2_w+10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p3_w,p4_w+10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p5_w-10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p6_w, p7_w-10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p8_w+10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)), 
    p9_w, p10_w+10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p11_w-10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)));
    
  delay(10*D10);
}
void r_turn(){
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  //pk=1;
  servo_write_func(
    p0_-10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(st2-int(sk-st2)%st)+p1_, 1*pk*(st2-int(sk-st2)%st)+p2_,
    p3_+10+0.5*pk*(st2-int(sk-st2)%st),-2*pk*(sk%st-st2)+p4_, -1*pk*(sk%st-st2)+p5_,
    p6_,-2*pk*(int(sk-st4)%st-st2)+p7_, -1*pk*(int(sk-st4)%st-st2)+p8_,
    p9_,2*pk*(st2-int(sk-st2-st4)%st)+p10_, 1*pk*(st2-int(sk-st2-st4)%st)+p11_);
  delay(D10);
}
void back(){
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  //pk=-1;
  servo_write_func(
    p0_,-2*pk*(int(sk-st4)%st-st2)+p1_, -1*pk*(int(sk-st4)%st-st2)+p2_,
    p3_,-2*pk*(st2-int(sk-st2-st4)%st)+p4_, -1*pk*(st2-int(sk-st2-st4)%st)+p5_,
    p6_+10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(st2-int(sk-st2)%st)+p7_, 1*pk*(st2-int(sk-st2)%st)+p8_,
    p9_-10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(sk%st-st2)+p10_, 1*pk*(sk%st-st2)+p11_);
  delay(D10);
}

void loop() {
  Distance_ToF=sensor.readRangeContinuousMillimeters();
  Serial.print(Distance_ToF);
  if (sensor.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }
  Serial.println();
  if(Distance_ToF<100){
    param=3;
  }
  if(Distance_ToF>200){
    param=1;
  }
  if(Distance_ToF>100&Distance_ToF<200){
    param=2;
  }
    
  readData();
  get_angle();  
  
  if(!Ps3.isConnected())
    return;
  Ps3.setPlayer(player);

  param = constrain(param,0,3);
  if(param ==0){
    calibrate();
    delay(0);
  }
  if(param ==1){
    walk_mpu();
    delay(0);
  }
  if(param ==2){
    r_turn();
    delay(0);
  }
  if(param ==3){
    back();
    delay(0);
  }          
  sk += 1;
}
//MangDang社サーボ利用の出力
void servo_write(int ch, int ang){
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); 
  pwm.setPWM(ch, 0, ang);
}

void get_angle(){
  rawX = acc_x-acc_x0;
  rawY = acc_y-acc_y0;
  rawZ = acc_z-acc_z0;
  pitch = atan2(rawX, sqrt(rawY*rawY+rawZ*rawZ))* 180.0 / PI;
  roll = atan2(rawY, rawZ)* 180.0 / PI;
}

int16_t u2s(int16_t unsigneddata){
  int16_t usd = unsigneddata;
  if (usd & (0x01 << 15)){ 
      usd = -1 * ((unsigneddata ^ 0xffff) + 1);
  }
  return usd;
}

void readData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);

  axRaw = u2s(Wire.read() << 8 | Wire.read());
  ayRaw = u2s(Wire.read() << 8 | Wire.read());
  azRaw = u2s(Wire.read() << 8 | Wire.read());
  //Temperature = u2s(Wire.read() << 8 | Wire.read());
  gxRaw = u2s(Wire.read() << 8 | Wire.read());
  gyRaw = u2s(Wire.read() << 8 | Wire.read());
  gzRaw = u2s(Wire.read() << 8 | Wire.read());

  // 加速度値を分解能で割って加速度(G)に変換する
  acc_x = (2. / 32768.)*axRaw ;  //FS_SEL_0 16,384 LSB / g
  acc_y = (2. / 32768.)*ayRaw ;
  acc_z = (2. / 32768.)*azRaw ;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  gyro_x = (250. / 32768.)*gxRaw ;  // (度/s)
  gyro_y = (250. / 32768.)*gyRaw ;
  gyro_z = (250. / 32768.)*gzRaw ;
  //temperature
  //temp= (Temperature-4040)/340. + 29.0; //室温29.0Cで校正
}

void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);  // 0; 内部クロックを設定
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);  // 
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

やはり、距離計測だけでは。。。次はコード整理・統合してPS3コントローラー制御をやろう。