//TWIPRoboBaseSteper1ICM42670BLE.ino
//library: ICM42670P by TDK/Invensense
//library: Madgwick by Arduino
//library: vl53l1x-arduino by Pololu
//Assigning Command Strings
// 'cal': execute calibration.
// 'att': measure attitude.
// 'dst": measure distance.
// 'mot': test stepper motor.
// 'stp': stop everything.
// 'oal': execute overall operation.
#include <AttitudeDetectorICM42670Coarse.h>
#include <Handy_BLE_Uart.h>
#include <StepperMotorESP32.h>
#include <VL53L1X.h>
#include <BuiltinLED.h>
#define BLE_DEVICENAME "CartBase"
const uint32_t operatingFrequency = 200; //[Hz] Base operating frequency 200[Hz]
const uint32_t operatingPeriod = 1000000 / operatingFrequency; //[us] Base operating period 5000[us]=5[ms]
const uint32_t MinPulseperiod = 20; //[us] Minimum period of the step pulse for stepper motors
AttitudeDetectorICM42670Coarse Attitude_Detector(Wire, true);//default true (ICM42670's address 0x69)
Handy_BLE_Uart bleuart;
StepperMotorESP32 stepperR, stepperL; //Right and Left stepper motor
VL53L1X vl53l1x;
BuiltinLED builtinled; //class BuiltinLED のインスタンス
//Assigning pins
const uint8_t Pin_R_DIR = D0; // Pin number used for the right motor's "dir" signal
const uint8_t Pin_R_STEP = D1; // Pin number used for the right motor's "step" signal
const uint8_t Pin_L_DIR = D2; // Pin number used for the left motor's "dir" signal
const uint8_t Pin_L_STEP = D3; // Pin number used for the left motor's "step" signal
const uint8_t Pin_Enable = D6; // Pin number used for the motor driver's "Enable" (negative logic)
const uint8_t POWER_MONITORING_PIN = D10; //LiPo Battery Voltage Monitoring
enum class ERequest {
non,
cal,
att,
dst,
mot,
oal
};
unsigned long microsPrevious;
ERequest request = ERequest::non;
ERequest previousrequest = ERequest::non;
void setup() {
int ret;
Serial.begin(115200);
delay(1000);
//Serial.println("operatingPeriod " + String(operatingPeriod));
//Wire.begin();
//Wire.setClock(10000); //I2C clockの設定 [Hz] defaultは400000 I2C不調なら10000 or 10000
//*** initialize Attitude_Detector ***
Attitude_Detector.start(operatingFrequency, -65, 6, -89);
//*** initialize TOF Distance Sensor VL53L1X ***
Serial.println("vl53l1x initialization started");
vl53l1x.setTimeout(500); //[msec]
if (!vl53l1x.init()) {
Serial.println("Failed to detect and initialize vl53l1x!");
while (1);
}
//set long distance range
vl53l1x.setDistanceMode(VL53L1X::Short);
//set timing budget: 20000usec
vl53l1x.setMeasurementTimingBudget(20000);
//set measurement interval in Continuous Measurement Mode: 20msec
vl53l1x.startContinuous(20);
Serial.println("vl53l1x initialization completed");
//*** initialize BLE ***
builtinled.startBlinking();
bleuart.setInitialMessage("ICM42670_VL53L1X_Stepper_BLE\n Send me 'cal', 'att', 'dst', 'mot', 'oal', or 'stp'");
//bleuart.begin(BLE_DEVICENAME, 123456); //uses passkey 123456
bleuart.begin(BLE_DEVICENAME); //without passkey
while (!bleuart.isConnected()) delay(100);
builtinled.stopBlinking();
delay(1000);
//*** initialize stepper motors ***
stepperR.setPins(Pin_R_DIR, Pin_R_STEP, Pin_Enable);
stepperR.disableDriver();
stepperR.setControlValue(operatingFrequency, MinPulseperiod); //200[Hz] 40[us]
stepperR.enableDriver();
stepperL.setPins(Pin_L_DIR, Pin_L_STEP, Pin_Enable);
stepperL.disableDriver();
stepperL.setControlValue(operatingFrequency, MinPulseperiod); //50[msec] 40[us]
stepperL.enableDriver();
microsPrevious = micros() - operatingPeriod ;
Serial.println("end of setup");
bleuart.transferString("end of setup");
}
void loop() {
float ax, ay, az; //[mm/s2] acceleration xyz
float gx, gy, gz; //[deg/sec] angular velocity xyz
float pitch, roll, yaw; //[deg]Euler angle
static int distance = 0; //[mm]
static VL53L1X::RangeStatus diststatus = VL53L1X::RangeValid;
unsigned long microsNow, microsDifference;
String message;
const double Motor_Drive_dutyRatios[] = {-0.8, -0.6, -0.4, -0.2, 0., 0.2, 0.4, 0.6, 0.8};
const static int nRatios = sizeof(Motor_Drive_dutyRatios)/sizeof(double);
static int increment = 1; //デューティー比番号dutyRatioIndex変更用変数
static int dutyRatioIndex = (nRatios>>1);
static int cntr = 0;
powermonitoring(); //リポバッテリ電圧監視 LED点滅で電圧低下を知らせてくる
if (!bleuart.isConnected()) {
request = ERequest::non;
return;
}
if (bleuart.ReceivedStringReady()) { //Polling
processReceivedString(bleuart.getReceivedString());
}
if (request == ERequest::non) {
stepperR.drive(0.);
stepperL.drive(0.);
return;
}
if (request == ERequest::cal) {
stepperR.drive(0.);
stepperL.drive(0.);
request = ERequest::non;
executeCalibration();
return;
}
microsNow = micros();
microsDifference = microsNow - microsPrevious;
if (microsDifference >= operatingPeriod ) {
if (microsDifference > operatingPeriod * 1.5) {
microsPrevious = microsNow;
} else {
microsPrevious = microsPrevious + operatingPeriod ;
}
//Must be executed at the operatingFrequency.
Attitude_Detector.getAttitude(ax, ay, az, gx, gy, gz, pitch, roll, yaw);
if (vl53l1x.dataReady()) {
distance = vl53l1x.read(false);
diststatus = vl53l1x.ranging_data.range_status;
}
cntr++;
if (cntr < operatingFrequency) return;
cntr = 0;
if (request == ERequest::att
|| request == ERequest::oal) {
Serial.println("request == ERequest::att");
message = "acc: " + String(ax) + ", " + String(ay) + ", " + String(az);
bleuart.transferString(message);
message = "angvel: " + String(gx) + ", " + String(gy) + ", " + String(gz);
bleuart.transferString(message);
message = "Eulang: " + String(pitch) + ", " + String(roll) + ", " + String(yaw);
bleuart.transferString(message);
}
if (request == ERequest::dst
|| request == ERequest::oal) {
Serial.println("request == ERequest::dst");
message = "distance: " + String(distance) + " status: " +String(diststatus);
bleuart.transferString(message);
}
if (request == ERequest::mot
|| request == ERequest::oal) {
Serial.println("request == ERequest::mot");
double dutyRatio_Value = Motor_Drive_dutyRatios[dutyRatioIndex];
stepperR.drive(dutyRatio_Value);
int stepsL = stepperL.drive(dutyRatio_Value);
message = "motor duty: " + String(dutyRatio_Value) + ", " + String(stepsL);
bleuart.transferString(message);
dutyRatioIndex += increment;
if (nRatios-1 <= dutyRatioIndex) increment = -1;
if (dutyRatioIndex < 1) increment = 1;
}
}
}
//Command Parser
void processReceivedString(String str) {
Serial.println("RX_Value = " + str);
//開発初期の確認用エコーバック 不要になったらコメントアウトして送信を減らす
bleuart.transferString(str);
previousrequest = request;
if (str.indexOf("cal") == 0) request = ERequest::cal;
else if (str.indexOf("att") == 0) request = ERequest::att;
else if (str.indexOf("dst") == 0) request = ERequest::dst;
else if (str.indexOf("mot") == 0) request = ERequest::mot;
else if (str.indexOf("oal") == 0) request = ERequest::oal;
else if (str.indexOf("stp") == 0) request = ERequest::non;
}
//Calibration of Attitude_Detector Gyro
void executeCalibration()
{
Serial.println("Received your calibration request.");
Serial.println("Keep the sensor motionless.");
bleuart.transferString("Received your calibration request.");
bleuart.transferString("Keep the sensor motionless.");
Attitude_Detector.calibrateGyro();
int16_t gxc, gyc, gzc;
Attitude_Detector.getGyrOffset(gxc, gyc, gzc);
String result = "calibration value " + String(gxc) + ", " + String(gyc) + ", " + String(gzc);
bleuart.transferString(result);
Serial.println("Calibration has done.");
bleuart.transferString("Calibration has done.");
}
void powermonitoring() {
static int ledind = 0;
//power monitoring begin
const double ADCfactor = (30. + 4.7) / 4.7;
const int thresholdVoltage = 11000; //[mV]
int analogVolts = analogReadMilliVolts(POWER_MONITORING_PIN); //[mV]
int EstimatedPowerSupplyVoltage = analogVolts * ADCfactor;
if (EstimatedPowerSupplyVoltage <= thresholdVoltage && ledind == 0) {
builtinled.startBlinking2();
ledind = 1;
} else if (thresholdVoltage <EstimatedPowerSupplyVoltage && ledind == 1) {
builtinled.stopBlinking();
ledind = 0;
}
}
16:39:28: >connecting CartBase ..
16:39:30: >connected
16:39:31: ICM42670_VL53L1X_Stepper_BLE
Send me 'cal', 'att', 'dst', 'mot', 'oal', or 'stp'
16:39:36: $$cal
16:39:36: cal
16:39:36: Received your calibration request.
16:39:36: Keep the sensor motionless.
16:39:37: calibration value -108, -1, -120
16:39:37: Calibration has done.
16:39:41: $$att
16:39:41: att
16:39:42: acc: 0.76, 0.12, 9.85
16:39:42: angvel: 0.00, 0.02, 0.02
16:39:42: Eulang: -4.39, 0.74, -0.02
16:39:43: acc: 0.74, 0.14, 9.86
16:39:43: angvel: -0.08, -0.07, 0.02
16:39:43: Eulang: -4.33, 0.76, -0.02
16:39:44: acc: 0.75, 0.14, 9.85
16:39:44: angvel: 0.08, -0.04, 0.03
16:39:44: Eulang: -4.39, 0.80, -0.03
16:39:45: acc: 0.76, 0.12, 9.86
16:39:45: angvel: -0.14, 0.04, -0.14
16:39:45: Eulang: -4.40, 0.68, -0.02
16:39:45: $$stp
16:39:45: stp
16:39:48: $$dst
16:39:48: dst
16:39:48: distance: 1064 status: 0
16:39:49: distance: 1070 status: 0
16:39:50: distance: 1066 status: 0
16:39:51: distance: 1066 status: 0
16:39:52: distance: 1077 status: 0
16:39:53: $$stp
16:39:53: stp
16:39:55: $$mot
16:39:55: mot
16:39:55: motor duty: 0.00, 0
16:39:56: motor duty: 0.20, 50
16:39:57: motor duty: 0.40, 100
16:39:58: motor duty: 0.60, 150
16:39:59: motor duty: 0.80, 200
16:40:00: motor duty: 0.60, 150
16:40:01: motor duty: 0.40, 100
16:40:01: $$stp
16:40:01: stp
16:40:04: >disconnected
Xiao ESP32S3を使って，BLE-UART通信が可能で，６軸センサICM42670P，TOF距離センサVL53L1Xを搭載し，
2つのステッパモータで移動可能なカートを想定したデモプログラムを作成しました。
★重要 このデモプログラムではステッパモータ向けのパルス信号をESP32のAPI Ledcを用いて生成しています。
「７ 考察」の問題点を考慮して，頻繁なPWMパルス周波数変更があるような用途にAPI ledcを使用した本ページ記述の方法は避けるべきと考えます。
このデモプログラムは，動作としては意味を持ちませんが，
BLE-UARTでの無線リモコンで操作できるカートのひな型となっています。
姿勢データや距離データをもとにモータ指令値を生成し，
デモプログラムのように5msecごとにステッパモータドライバに与えるようにすれば，
自律動作を行うカートも製作できると思います。
コマンド文字列の割り当ては，ここのプログラム例にとらわれることなく，
動作に即したものになるよう自由に決めればよいと思います。
このデモプログラムをひな型にして，改造するときは，各要素のプログラミングに慣れる必要があります。
そのときには，「１ はじめに」で紹介している，各要素の使い方ページを参照するとよいと思います。
