//TWIPRoboBaseSteper2ICM42670BLE.ino
//library: ICM42670P by TDK/Invensense
//library: Madgwick by Arduino
//library: vl53l1x-arduino by Pololu
//Command assign
// 'cal': execute calibration.
// 'att': measure attitude.
// 'dst": measure distance.
// 'mot': test stepper motor.
// 'oal': execute overall operation.
// 'stp': stop everything.
#include <AttitudeDetectorICM42670Coarse.h>
#include <Handy_BLE_Uart.h>
#include <stepperMotorESP32_2s.h>
#include <VL53L1X.h>
#include <BuiltinLED.h>
#define DEVICENAME "ADMtest"
const uint32_t operatingFrequency = 200; //[Hz] 姿勢データ取得周波数 200Hz
const uint32_t operatingPeriod = 1000000 / operatingFrequency; //[us] 5000[us]=5[ms]
const uint32_t MinPulseperiod = 40; //[us] ステップパルスの最小周期
const uint32_t InterruptPeriod = 20; //[us] タイマー割込み周期
AttitudeDetectorICM42670Coarse Attitude_Detector(Wire, true);//defaultではtrue
Handy_BLE_Uart bleuart;
StepperMotorESP32_2s stepperR, stepperL;
VL53L1X vl53l1x;
BuiltinLED builtinled; //class BuiltinLED のインスタンス
//Pin assign
const uint8_t Pin_R_DIR = D0; // 右モータdir出力に使用する PIN番号
const uint8_t Pin_R_STEP = D1; // 右モータsteppulse出力に使用する PIN番号
const uint8_t Pin_L_DIR = D2; // 左モータdir出力に使用する PIN番号
const uint8_t Pin_L_STEP = D3; // 左モータsteppulse出力に使用する PIN番号
const uint8_t PinEnable = D6; // 左右モータドライブEnable（負論理）に使用する PIN番号
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は100000 I2C不調なら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(DEVICENAME, 123456); //uses passkey 123456
bleuart.begin(DEVICENAME); //without passkey
while (!bleuart.isConnected()) delay(100);
builtinled.stopBlinking();
delay(1000);
//*** initialize stepper motors ***
stepperR.initialize(Pin_R_DIR, Pin_R_STEP, PinEnable);
stepperR.disableDriver();
stepperR.setPeriods(operatingPeriod, MinPulseperiod, InterruptPeriod); //50[msec] 50[us] 20[us]
stepperR.enableDriver();
stepperL.initialize(Pin_L_DIR, Pin_L_STEP, PinEnable);
stepperL.disableDriver();
stepperL.setPeriods(operatingPeriod, MinPulseperiod, InterruptPeriod); //50[msec] 50[us] 20[us]
stepperL.enableDriver();
initializeTimerInterrupt();
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.);
delay(10);
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);
stepperL.drive(dutyRatio_Value);
message = "motor duty: " + String(dutyRatio_Value);
bleuart.transferString(message);
dutyRatioIndex += increment;
if (nRatios-1 <= dutyRatioIndex) increment = -1;
if (dutyRatioIndex < 1) increment = 1;
}
}
}
//Command Line 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;
}
}
//-- timer interrupt --
hw_timer_t * timer = NULL;
void ARDUINO_ISR_ATTR onTimer() {
//Calls two handlers.
stepperL.ISRhandler();
stepperR.ISRhandler();
}
void initializeTimerInterrupt() {
timer = timerBegin(1000000); //// Set timer frequency to 1Mhz
timerAttachInterrupt(timer, &onTimer); // Attach onTimer function to our timer.
timerAlarm(timer, InterruptPeriod, true, 0); //20カウントなので20μsがタイマー割り込み間隔
}
//-- timer interrupt --
Connecting to ADMtest ...
Connected
end of setup
ICM42670_VL53L1X_Stepper_BLE
Send me 'cal', 'att', 'dst', 'mot', 'oal', or 'stp'
cal
cal
Received your calibration request.
Keep the sensor motionless.
calibration value -118, -7, -135
Calibration has done.
att
att
acc: 0.04, 0.12, 9.88
angvel: 0.03, 0.01, -0.05
Eulang: -0.24, 0.71, -0.00
acc: 0.06, 0.12, 9.89
angvel: 0.00, 0.07, -0.04
Eulang: -0.31, 0.68, -0.00
acc: 0.02, 0.12, 9.88
angvel: -0.06, -0.05, 0.01
Eulang: -0.17, 0.71, -0.01
acc: 0.05, 0.13, 9.90
angvel: 0.03, -0.05, 0.01
Eulang: -0.29, 0.77, -0.01
stp
stp
dst
dst
distance: 467 status: 0
distance: 471 status: 0
distance: 470 status: 0
distance: 472 status: 0
distance: 469 status: 0
distance: 469 status: 0
distance: 469 status: 0
distance: 469 status: 0
stp
stp
mot
mot
motor duty: 0.00
motor duty: 0.20
motor duty: 0.40
motor duty: 0.60
motor duty: 0.80
motor duty: 0.60
motor duty: 0.40
motor duty: 0.20
stp
stp
Disconnected
Xiao ESP32S3を使って，BLE-UART通信が可能で，６軸センサICM42670P，TOF距離センサVL53L1Xを搭載し，
2つのステッパモータで移動可能なカートを想定したデモプログラムを作成しました。
★重要 このデモプログラムではステッパモータ向けのパルス信号をタイマ割り込みにて生成しています。
このデモプログラムは，動作としては意味を持ちませんが，
BLE-UARTでの無線リモコンで操作できるカートのひな型となっています。
姿勢データや距離データをもとにモータ指令値を生成し，
デモプログラムのように5msecごとにステッパモータドライバに与えるようにすれば，
自律動作を行うカートも製作できると思います。
コマンド文字列の割り当ては，ここのプログラム例にとらわれることなく，
動作に即したものになるよう自由に決めればよいと思います。
このデモプログラムをひな型にして，改造するときは，各要素のプログラミングに慣れる必要があります。
そのときには，「１ はじめに」で紹介している，各要素の使い方ページを参照するとよいと思います。
