木楽らぼソースコード: 2021

M5StickCバランスロボ本体のスケッチです。

	//
	// M5StickC Balancing Robot V029
	// by Kiraku Labo, 2021
	//
	// 1. Lay the robot flat, and power on
	// 2. Wait until G.Gal (Gyro calibration) completes and Accel shows at the bottom
	// 3. Hold the robot upright and wait until A.Cal (Accel calibration) completes.
	//
	// short push M5 button: Gyro calibration
	// long push M5 button: switch between standig mode and demo (circle) mode
	//

	#include <M5StickC.h>
	#include <IRremoteESP8266.h>
	#include <IRrecv.h>
	#include <IRutils.h>
	#include <BLEDevice.h>
	#include <BLEServer.h>
	#include <BLE2902.h>

	//#define DRV8835
	#define DRV8830

	#define INVERTED //M5StickC upside down
	#define ININ 0 //0:Phase-Enable mode, 1:in-in mode
	#define BTN_A 37 //M5 button
	#define BTN_B 39
	#define PHASE_L 0 //AIN1
	#define ENABLE_L 33 //AIN2
	#define PHASE_R 26 //BIN1
	#define ENABLE_R 32 //BIN2
	#define MOTOR_R 0x65 //A0=low, A1=open
	#define MOTOR_L 0x63 //A0=high, A1=open
	#define IR_PIN_8830 32
	#define IR_PWR_PIN 33
	#define IR_PIN_8835 36

	#define OSP_PWR_TH 250.0 //power
	#define OSP_LMT_TH 6 //count
	#define OSP_STP_TH 4 //count
	#define IR_TIMEOUT 15
	#define IR_MIN_UNKNOWN 12
	#define IR_BUF_SIZE 1024

	#define SERVICE_UUID "75719c1e-6299-11e8-adc0-fa7ae01bbebc"
	#define CHRX_UUID "75719f7a-6299-11e8-adc0-fa7ae01bbebc"
	#define CHTX_UUID "7571a1e6-6299-11e8-adc0-fa7ae01bbebc"

	String ver="V029";
	boolean serialMonitor=false;
	bool irDebug=false;

	int16_t counter=0;
	uint32_t time0=0, time1=0;
	int16_t counterOverSpd=0, counterTrip;
	float aveAccX=0.0, aveAccZ=0.0;
	float power, powerR, powerL, yawPower;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0;
	float gyroYoffset, gyroZoffset, accXoffset, accZoffset;
	float gyroYdata, gyroZdata, accXdata, accZdata;
	const float cutoff=0.1; //~=2 * pi * f (Hz)
	const float clk = 0.01; // in sec,
	const uint32_t interval = 10; // in msec
	boolean calibrated=false;
	int8_t state=-1;
	float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
	int16_t maxPwr;
	float yawAng=0.0;
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float angAdj=0.0;
	int8_t jX, jY;

	int16_t motorDeadband=0;
	int16_t motorDeadbandNeg=0; //compiler does not accept formula for min(x,y)
	float mechFactorR, mechFactorL, mechLRbal, mechFact;
	int8_t motorRDir=0, motorLDir=0;
	bool spinContinuous=true;
	float spinDest, spinTarget, spinFact=1.0;;
	float spinStep = 30.0*clk;
	int16_t ipowerL=0, ipowerR=0;
	int16_t motorLdir=0, motorRdir=0; //stop 1:+ -1:-
	float gyroZdps, accXg;
	float vBatt, voltAve=3.7;
	int16_t drvOffset, punchSpd, punchSpd2, punchSpd2N, punchDur, punchCountL=0, punchCountR=0;
	byte demoMode=0;
	uint16_t imuType; //200 or 6886
	uint16_t driverType; //8830 or 8835
	int irPin;
	byte joyLX, joyLY, joyRX, joyRY, joySW;
	bool byIrRemote=true; //true=TV remote, false=BLE joystick
	float lrBalance=0.0, spinBalAdj=0.0;

	//irCode[0]:demo key, [1-9]:key1 to key9
	const uint32_t irCode[] = {0x0E90, 0x0010, 0x0810, 0x0410, 0x0C10, 0x0210, 0x0A10, 0x0610, 0x0E10, 0x0110}; //Sony format
	uint32_t irRcvTime=0;

	#ifdef DRV8835
	IRrecv irRcv(IR_PIN_8835, IR_BUF_SIZE, IR_TIMEOUT, true);
	#else
	IRrecv irRcv(IR_PIN_8830, IR_BUF_SIZE, IR_TIMEOUT, true);
	#endif
	decode_results irRes;

	BLEServer* pServer = NULL;
	BLECharacteristic* pCharTx = NULL;
	BLECharacteristic* pCharRx = NULL;

	bool deviceConnected = false;
	bool oldDeviceConnected = false;
	uint32_t value = 0;

	class MyCallbacks: public BLECharacteristicCallbacks {
	void onWrite(BLECharacteristic *pChar) {
	std::string value = pChar->getValue();
	if (value.length()>0) {
	joyLX=value[0];
	joyLY=value[1];
	joyRX=value[2];
	joyRY=value[3];
	joySW=value[4];
	jX=-(constrain(joyRX,0,200)-100)/20; //-5 to 5 = Right to Left
	jY=(constrain(joyLY,0,200)-100)/20; //-5 to 5 = Down to Up
	if (jX!=0 \|\| jY!=0) byIrRemote=false;
	if (!byIrRemote) { //joystick
	spinContinuous=true;
	spinStep=-0.3*(float)jX;
	movePower=(float)jY*20.0;
	// if (jY==0) spinBalAdj=-(float)jX*lrBalance;
	// else spinBalAdj=0.0;
	angAdj=-movePower/5000000.0;
	}
	// Serial.print("Value: ");
	// for (int i = 0; i < 5; i++) Serial.print(value[i]);
	// Serial.println();
	}
	}
	};

	class MyServerCallbacks: public BLEServerCallbacks {
	void onConnect(BLEServer* pServer) {
	deviceConnected = true;
	};
	void onDisconnect(BLEServer* pServer) {
	deviceConnected = false;
	}
	};

	void setup() {
	pinMode(BTN_A, INPUT);
	pinMode(BTN_B, INPUT);
	setupBLE();
	Serial.begin(115200);
	M5.begin();
	byte resp;
	Wire1.beginTransmission(SH200I_ADDRESS);
	resp = Wire1.endTransmission();
	if (resp==0) {
	Serial.println("SH200I found");
	imuType=200;
	}
	else {
	Wire1.beginTransmission(MPU6886_ADDRESS);
	resp = Wire1.endTransmission();
	if (resp==0) {
	Serial.println("MPU6886 found");
	imuType=6886;
	}
	else Serial.println("No IMU found");
	}
	imuInit();
	Wire.begin(0, 26); //SDA,SCL
	Wire.setClock(50000);

	#ifdef DRV8830
	Serial.println("DRV8830");
	driverType=8830;
	pinMode(IR_PIN_8830, INPUT_PULLUP);
	pinMode(IR_PWR_PIN, OUTPUT);
	digitalWrite(IR_PWR_PIN, HIGH);
	#else
	Serial.println("DRV8835");
	driverType=8835;
	ledcSetup(1, 20000, 8); //LEDC_CHANNEL, LEDC_FREQUENCY, LEDC_RESOLUTION_BITS
	ledcSetup(2, 20000, 8);
	ledcSetup(3, 20000, 8);
	ledcSetup(4, 20000, 8);
	ledcAttachPin(PHASE_L, 1);
	ledcAttachPin(ENABLE_L, 2); //PIN, LEDC_CHANNEL Left
	ledcAttachPin(PHASE_R, 3);
	ledcAttachPin(ENABLE_R, 4); //Right
	irPin=IR_PIN_8835;
	pinMode(IR_PIN_8835, INPUT);
	#endif

	irRcv.setUnknownThreshold(IR_MIN_UNKNOWN);
	irRcv.enableIRIn();
	M5.Axp.ScreenBreath(11);
	#ifdef INVERTED
	M5.Lcd.setRotation(2);
	#else
	M5.Lcd.setRotation(0);
	#endif
	M5.Lcd.setTextFont(1);
	M5.Lcd.fillScreen(BLACK);
	M5.Lcd.setCursor(0, 10);
	if (imuType==200) M5.Lcd.println(" SH200I");
	else if (imuType==6886) M5.Lcd.println(" MPU6886");
	if (driverType==8830) M5.Lcd.println(" DRV8830");
	else M5.Lcd.println(" DRV8835");
	M5.Lcd.println(" "+ver);
	delay(3000);
	M5.Lcd.setTextFont(4);
	M5.Lcd.setTextSize(1);
	M5.Lcd.setCursor(0,70);
	M5.Lcd.println(" G.Cal");
	resetMotor();
	resetPara();
	resetVar();
	delay(1000);
	getBaselineAccZ();
	getBaselineGyro();
	M5.Lcd.fillScreen(BLACK);
	dispVolt();
	}

	void loop() {
	getIR();
	checkButton();
	checkBLE();
	getGyro();
	switch (state) {
	case -1:
	dispVolt();
	M5.Lcd.setCursor(10,110);
	M5.Lcd.printf("%5.2f ", accXdata);
	M5.Lcd.setCursor(10,130);
	M5.Lcd.printf("%5.2f ", aveAbsOmg);
	if (abs(accXdata)<0.25 && aveAbsOmg<1.0) state=0;
	break;
	case 0: //baseline
	calibrate();
	state=1;
	break;
	case 1: //run
	if (abs(varAng)>30.0 \|\| abs(aveAccZ)<0.3 \|\| counterTrip>OSP_STP_TH) {
	if (serialMonitor) sendStatus();
	resetMotor();
	resetVar();
	state=-1;
	}
	else {
	calcTarget();
	drive();
	}
	break;
	}
	counter += 1;
	if (counter%10==0) {
	M5.Lcd.setCursor(15, 45);
	M5.Lcd.printf("%d ", jY);
	M5.Lcd.setCursor(50, 45);
	M5.Lcd.printf("%d ", jX);
	}
	if (counter >= 100) {
	counter = 0;
	dispVolt();
	if (serialMonitor) sendStatus();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}

	void dispVolt() {
	float vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
	M5.Lcd.setCursor(10,70);
	M5.Lcd.printf("%4.2fv ", vBatt);
	}

	void getIR() {
	if (irRcv.decode(&irRes)) {
	if (millis()-irRcvTime<300) {
	irRcv.resume();
	return;
	}
	irRcvTime=millis();
	if (serialMonitor) Serial.print(resultToHumanReadableBasic(&irRes));
	if (irDebug) Serial.println(resultToSourceCode(&irRes));
	exec();
	irRcv.resume();
	}
	}

	void exec() {
	uint32_t code=(uint32_t)irRes.value;
	if (code==irCode[0]) demoCirc();
	else if (code==irCode[1]) turnLeft();
	else if (code==irCode[2]) moveForward();
	else if (code==irCode[3]) turnRight();
	else if (code==irCode[4]) spinLeft();
	else if (code==irCode[5]) moveStop();
	else if (code==irCode[6]) spinRight();
	else if (code==irCode[7]) ; //not asigned
	else if (code==irCode[8]) moveBack();
	else if (code==irCode[9]) ; //not assigned
	}

	void turnLeft() {
	spinContinuous=false;
	spinStep=0.6;
	if (movePower>=0.0) spinDest=spinTarget+30.0;
	else spinDest=spinTarget-30.0;
	}

	void turnRight() {
	spinContinuous=false;
	spinStep=0.6;
	if (movePower>=0.0) spinDest=spinTarget-30.0;
	else spinDest=spinTarget+30.0;
	}

	void moveForward() {
	if (abs(spinStep)<0.01) movePower +=40.0;
	spinStep=0.0;
	}

	void spinLeft() {
	if (spinContinuous) spinStep +=0.4;
	else {
	spinContinuous=true;
	spinStep=0.4;
	}
	}

	void moveStop() {
	spinStep=0.0;
	movePower=0.0;
	}

	void spinRight() {
	if (spinContinuous) spinStep -= 0.4;
	else {
	spinContinuous=true;
	spinStep=-0.4;
	}
	}

	void moveBack() {
	if (abs(spinStep)<0.01) movePower -=40.0;
	spinStep=0.0;
	}

	void demoCirc() {
	spinContinuous=true;
	spinStep=0.4;
	movePower=-40.0;
	}

	void calibrate() {
	resetVar();
	drvMotorL(0);
	drvMotorR(0);
	counterOverSpd=0;
	// delay(100);
	M5.Lcd.setCursor(0,70);
	M5.Lcd.println(" A.Cal ");
	delay(500);
	getBaselineAccX();
	calibrated=true;
	M5.Lcd.fillScreen(BLACK);
	}

	void getBaselineAccX() {
	accXoffset=0.0;
	for (int i=1; i <= 30; i++){
	readGyro();
	accXoffset += accXdata;
	delay(20);
	}
	accXoffset /= 30;
	}

	void getBaselineAccZ() {
	accZoffset=0.0;
	for (int i=1; i <= 30; i++){
	readGyro();
	accZoffset += accZdata;
	delay(20);
	}
	accZoffset /= 30;
	}

	void getBaselineGyro() {
	gyroZoffset=gyroYoffset=0.0;
	for (int i=1; i <= 30; i++){
	readGyro();
	gyroZoffset += gyroZdata;
	gyroYoffset += gyroYdata;
	delay(20);
	}
	gyroYoffset /= 30;
	gyroZoffset /= 30;
	}

	void checkButton() {
	checkButtonA();
	#ifdef DEVELOP
	checkButtonB();
	#endif
	}

	void checkButtonA() {
	uint32_t msec=millis();
	if (digitalRead(BTN_A) == LOW) {
	M5.Lcd.setCursor(5, 5);
	M5.Lcd.print(" ");
	while (digitalRead(BTN_A) == LOW) {
	if (millis()-msec>=2000) break;
	}
	if (digitalRead(BTN_A) == HIGH) btnAshort();
	else btnAlong();
	}
	}

	void btnAlong() {
	M5.Lcd.setCursor(5, 5);
	if (demoMode==1) {
	demoMode=0;
	spinStep=0.0;
	movePower=0.0;
	M5.Lcd.print("Stand ");
	}
	else {
	demoMode=1;
	spinContinuous=true;
	spinStep=0.4;
	movePower=40.0;
	M5.Lcd.print("Demo ");
	}
	while (digitalRead(BTN_A) == LOW);
	}

	void btnAshort() {
	M5.Lcd.setCursor(0, 70);
	M5.Lcd.print(" G.Cal ");
	delay(1000);
	imuInit();
	getBaselineAccZ();
	getBaselineGyro();
	M5.Lcd.setCursor(0, 70);
	M5.Lcd.print(" ");
	}

	void imuInit() {
	if (imuType==200) {
	M5.Imu.Init();
	i2c1Write(SH200I_ADDRESS, SH200I_GYRO_CONFIG, 0x011); //reg0x0F, No Gyro HPF, ODR=1kHz
	delay(1);
	i2c1Write(SH200I_ADDRESS, SH200I_GYRO_DLPF, 0x02); //reg0x11, DLPF=125Hz
	delay(1);
	}
	else M5.Mpu6886.Init();
	}

	void resetPara() {
	#ifdef DRV8830
	Kang=35.0;
	Komg=0.8;
	KIang=800.0;
	Kyaw=5.0;
	Kdst=10.0;
	Kspd=2.5;
	mechFactorL=0.45;
	mechFactorR=0.45;
	punchSpd=30;
	punchDur=2;
	motorDeadband=17;
	motorDeadbandNeg=-motorDeadband;
	maxPwr=250;
	drvOffset=0;
	#else
	Kang=35.0;
	Komg=0.8;
	KIang=1000.0;
	Kyaw=4.0;
	Kdst=8.0;
	Kspd=2.5;
	mechFactorL=0.35;
	mechFactorR=0.35;
	punchSpd=25;
	punchDur=2;
	motorDeadband=20;
	motorDeadbandNeg=-motorDeadband;
	maxPwr=250; //limit slip
	drvOffset=0;
	#endif
	punchSpd2=max(punchSpd, motorDeadband);
	punchSpd2N=-punchSpd2;
	}

	void getGyro() {
	readGyro();
	varOmg = (gyroYdata - gyroYoffset); //unit:deg/sec
	if (state!=1) aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
	yawAng += (gyroZdata - gyroZoffset) * clk;
	varAng += (varOmg + ((accXdata - accXoffset) * 57.3 - varAng) * cutoff ) * clk;
	}

	void readGyro() {
	float gX, gY, gZ, aX, aY, aZ;
	if (imuType==200) {
	M5.Imu.getGyroData(&gX,&gY,&gZ);
	M5.Imu.getAccelData(&aX,&aY,&aZ);
	}
	else {
	M5.Mpu6886.getGyroData(&gX,&gY,&gZ);
	M5.Mpu6886.getAccelData(&aX,&aY,&aZ);
	}
	#ifdef INVERTED
	gyroYdata=gX;
	gyroZdata=-gY;
	accXdata=aZ;
	accZdata=aY;
	#else
	gyroYdata=-gX;
	gyroZdata=gY;
	accXdata=aZ;
	accZdata=-aY;
	#endif
	aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
	}

	void calcTarget() {
	if (spinContinuous) {
	if (abs(movePower)>1.0) spinFact=constrain((powerR+powerL)/10.0, -1.0, 1.0); //moving
	else spinFact=1.0; //standing
	spinTarget += spinStep * spinFact;
	}
	else {
	if (spinTarget < spinDest-spinStep) spinTarget += spinStep;
	if (spinTarget > spinDest+spinStep) spinTarget -= spinStep;
	}
	calcMoveTarget();
	}

	void calcMoveTarget() {
	if (movePwrTarget < movePower+spinBalAdj-movePwrStep) movePwrTarget += movePwrStep;
	else if (movePwrTarget > movePower+spinBalAdj+movePwrStep) movePwrTarget -= movePwrStep;
	else movePwrTarget = movePower+spinBalAdj;
	}

	void drive() {
	varAng += angAdj;
	varSpd += power * clk;
	varDst += Kdst * varSpd * clk;
	varIang += KIang * varAng * clk;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (absf(power) > OSP_PWR_TH) counterOverSpd += 1;
	else {
	counterOverSpd=0;
	counterTrip=0;
	}
	if (counterOverSpd>OSP_LMT_TH) {
	counterTrip += 1;
	counterOverSpd=0;
	power=0.0; //limit slip
	}
	power = constrain(power, -maxPwr, maxPwr);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerR = power + yawPower + movePwrTarget;
	powerL = power - yawPower + movePwrTarget;
	ipowerL = (int16_t) constrain(powerL * mechFactorL, -maxPwr, maxPwr);
	if (ipowerL > 0) {
	if (motorLdir == 1) {
	punchCountL++;
	punchCountL = constrain(punchCountL, 0, 100);
	}
	else punchCountL=0;
	motorLdir = 1;
	if (punchCountL<punchDur) drvMotorL(max(ipowerL, punchSpd2)+drvOffset);
	else drvMotorL(max(ipowerL, motorDeadband)+drvOffset);
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) {
	punchCountL++;
	punchCountL = constrain(punchCountL, 0, 100);
	}
	else punchCountL=0;
	motorLdir = -1;
	if (punchCountL<punchDur) drvMotorL(min(ipowerL, punchSpd2N) +drvOffset);
	else drvMotorL(min(ipowerL, motorDeadbandNeg)+drvOffset);
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}

	ipowerR = (int16_t) constrain(powerR * mechFactorR, -maxPwr, maxPwr);
	if (ipowerR > 0) {
	if (motorRdir == 1) {
	punchCountR++;
	punchCountR = constrain(punchCountL, 0, 100);
	}
	else punchCountR=0;
	motorRdir = 1;
	if (punchCountR<punchDur) drvMotorR(max(ipowerR, punchSpd2) +drvOffset);
	else drvMotorR(max(ipowerR, motorDeadband)+drvOffset);
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) {
	punchCountR++;
	punchCountR = constrain(punchCountR, 0, 100);
	}
	else punchCountR=0;
	motorRdir = -1;
	if (punchCountR<punchDur) drvMotorR(min(ipowerR, punchSpd2N)+drvOffset);
	else drvMotorR(min(ipowerR, motorDeadbandNeg)+drvOffset);
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	}

	#ifdef DRV8830
	void drvMotorL(int16_t pwr) {
	drv8830(MOTOR_L, pwr);
	}

	void drvMotorR(int16_t pwr) {
	drv8830(MOTOR_R, pwr);
	}

	void drv8830(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
	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 resetMotor() {
	drv8830(MOTOR_R, 0);
	drv8830(MOTOR_L, 0);
	counterOverSpd=0;
	counterTrip=0;
	}
	#endif

	#ifdef DRV8835
	void drvMotorL(int16_t pwr) {
	drv8835(1, 2, constrain(pwr, -255, 255));
	}

	void drvMotorR(int16_t pwr) {
	drv8835(3, 4, constrain(pwr, -255, 255));
	}

	void resetMotor() {
	drv8835(1,2,0);
	drv8835(3,4,0);
	counterOverSpd=0;
	counterTrip=0;
	}

	#if ININ==1
	void drv8835(int ch1, int ch2, int pwm) {
	if (pwm>=0) {
	ledcWrite(ch1, 0);
	ledcWrite(ch2, pwm);
	}
	else {
	ledcWrite(ch2, 0);
	ledcWrite(ch1, -pwm);
	}
	}
	#elif ININ==0
	void drv8835(int ch1, int ch2, int pwm) {
	if (pwm>=0) {
	ledcWrite(ch1, 1024);
	ledcWrite(ch2, pwm);
	}
	else {
	ledcWrite(ch1, 0);
	ledcWrite(ch2, -pwm);
	}
	}
	#endif //ININ
	#endif //DRV8835

	void resetVar() {
	power=0.0;
	movePwrTarget=0.0;
	movePower=0.0;
	spinDest=0.0;
	spinTarget=0.0;
	spinStep=0.0;
	yawAng=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	}

	void sendStatus () {
	Serial.print(millis()-time0);
	Serial.print(" state="); Serial.print(state);
	Serial.print(" accX="); Serial.print(accXdata);
	Serial.print(" accZ="); Serial.print(aveAccZ);
	Serial.print(" ang=");Serial.print(varAng);
	Serial.print(" gyroY="); Serial.print(gyroYdata);
	Serial.print(" trip=");Serial.print(counterTrip);
	Serial.print(", ");
	Serial.print(millis()-time0);
	Serial.println();
	}

	float absf(float x) {
	if (x>=0.0) return x;
	else return -x;
	}

	void i2c1Write(byte addr, byte reg, byte val) {
	Wire1.beginTransmission(addr);
	Wire1.write(reg);
	Wire1.write(val);
	Wire1.endTransmission();
	}

	void setupBLE() {
	BLEDevice::init("ESP32");
	pServer = BLEDevice::createServer();
	pServer->setCallbacks(new MyServerCallbacks());
	BLEService *pService = pServer->createService(SERVICE_UUID);

	// Create a BLE Characteristic
	pCharTx = pService->createCharacteristic(CHTX_UUID, BLECharacteristic::PROPERTY_NOTIFY);
	pCharRx = pService->createCharacteristic(CHRX_UUID, BLECharacteristic::PROPERTY_WRITE_NR);
	pCharRx ->setCallbacks(new MyCallbacks());
	pCharTx->addDescriptor(new BLE2902());
	pService->start();
	BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
	pAdvertising->addServiceUUID(SERVICE_UUID);
	pAdvertising->setScanResponse(false);
	pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
	BLEDevice::startAdvertising();
	Serial.println("Waiting a client connection to notify...");
	}

	void checkBLE() {
	// notify changed value
	if (deviceConnected) {
	pCharTx->setValue((uint8_t*)&value, 4);
	pCharTx->notify();
	}
	// disconnecting
	if (!deviceConnected && oldDeviceConnected) {
	delay(500); // give the bluetooth stack the chance to get things ready
	pServer->startAdvertising(); // restart advertising
	Serial.println("start advertising");
	oldDeviceConnected = deviceConnected;
	}
	// connecting
	if (deviceConnected && !oldDeviceConnected) {
	// do stuff here on connecting
	oldDeviceConnected = deviceConnected;
	}
	}

	void xxxx() {
	int8_t jX=(constrain(joyRX,0,200)-100)/20; //-5 to 5 = Right to Left
	int8_t jY=(constrain(joyLY,0,200)-100)/20; //-5 to 5 = Down to Up
	if (jX!=0 \|\| jY!=0) byIrRemote=false;
	if (!byIrRemote) { //joystick
	spinContinuous=true;
	spinStep=-0.3*(float)jX;
	movePower=(float)jY*20.0;
	if (jY==0) spinBalAdj=-(float)jX*lrBalance;
	else spinBalAdj=0.0;
	angAdj=-movePower/100000.0;
	}
	}

view raw M5StickV029.ino hosted with ❤ by GitHub

木楽らぼソースコード

2021年5月16日日曜日

M5StickCのバランスロボをBLEで操縦