M5StickCとJoyCでBLEリモコン

 バランスロボを操縦するためのBLEリモコンを作ってみました。

micro:bitバランスロボの記事はこちらです。

M5StickCバランスロボの記事はこちらです。

	//
	// JoyC Balance Robot Controller
	// by Kiraku Labo
	//
	#include <M5StickC.h>
	#include <BLEDevice.h>
	#include <Ticker.h>
	#define LED 10
	#define JOYC_ADDR 0x38
	#define JOYC_COLOR_REG 0x20
	#define JL_X16 0x50
	#define JL_Y16 0x52
	#define JR_X16 0x54
	#define JR_Y16 0x56
	#define JOYC_LEFT_X_REG 0x60
	#define JOYC_LEFT_Y_REG 0x61
	#define JOYC_RIGHT_X_REG 0x62
	#define JOYC_RIGHT_Y_REG 0x63
	#define JOYC_PRESS_REG 0x64
	#define JOYC_LEFT_ANGLE_REG 0x70
	#define JOYC_LEFT_DISTANCE_REG 0x74 //radius?
	#define JOYC_RIGHT_ANGLE_REG 0x72
	#define JOYC_RIGHT_DISTANCE_REG 0x76
	BLEUUID serviceUUID("75719c1e-6299-11e8-adc0-fa7ae01bbebc");
	BLEUUID charaUUID_RX("7571a1e6-6299-11e8-adc0-fa7ae01bbebc");
	BLEUUID charaUUID_TX("75719f7a-6299-11e8-adc0-fa7ae01bbebc");
	BLERemoteCharacteristic* pRemoteCharacteristicRX;
	BLERemoteCharacteristic* pRemoteCharacteristicTX;
	BLEAdvertisedDevice* periphDevice;
	BLEScanResults *scanCompleteCB;
	Ticker timer;
	boolean connecting = false;
	boolean scanning = false;
	bool connected = false;
	char errCode='0';
	String errStr="";
	TFT_eSprite dispBuff = TFT_eSprite(&M5.Lcd);
	TFT_eSprite voltBuff = TFT_eSprite(&M5.Lcd);
	extern const unsigned char connect_on[800];
	extern const unsigned char connect_off[800];
	byte ledColor[3]={0,100,0};
	uint8_t joyStick[5];
	float vBatt, voltAve=3.7;
	volatile uint16_t loopCounter=1, loopCounter0=0;
	void setup() {
	pinMode(LED,OUTPUT);
	digitalWrite(LED,HIGH);
	Serial.begin(115200);
	M5.begin();
	Wire.begin(0,26,10000);
	M5.Lcd.setRotation(4);
	M5.Lcd.setSwapBytes(false);
	dispBuff.createSprite(80, 160);
	dispBuff.setSwapBytes(true);
	dispBuff.fillRect(0,0,80,20, dispBuff.color565(50,50,50));
	dispBuff.setTextSize(1);
	dispBuff.setTextColor(GREEN);
	dispBuff.setCursor(55,6);
	dispBuff.pushSprite(0,0);
	setupBLE();
	i2cWriteBuff(JOYC_COLOR_REG, ledColor, 3);
	timer.attach(5.0, watch); //2 sec
	}
	void watch() {
	if (loopCounter==loopCounter0) {
	digitalWrite(LED, LOW);
	esp_restart();
	}
	loopCounter0=loopCounter;
	}
	void loop() {
	getJoystick();
	dispLcd();
	if (connecting == true) {
	Serial.println("Connecting...");
	if (connectBLE()) {
	Serial.println("Connected to the BLE Server.");
	dispBuff.fillRect(0,0,80,20, dispBuff.color565(50,50,50));
	dispBuff.setCursor(5,5);
	dispBuff.setTextSize(2);
	dispBuff.print("*");
	dispBuff.pushSprite(0,0);
	}
	else {
	Serial.println("Failed to connect BLE server.");
	dispBuff.fillRect(0,0,80,20, dispBuff.color565(50,50,50));
	dispBuff.setTextSize(1);
	dispBuff.setCursor(20,5);
	dispBuff.printf("Failed");
	dispBuff.pushSprite(0,0);
	}
	connecting = false;
	}
	if (connected) {
	pRemoteCharacteristicTX->writeValue(joyStick, 5, false); //do not expect response
	// Serial.printf("Sent:%d %d", joyStick[2], joyStick[3]);
	}
	else if (scanning) {
	dispBuff.pushImage(0,0,20,20,(uint16_t *)connect_off);
	dispBuff.fillRect(0,0,80,20, dispBuff.color565(50,50,50));
	dispBuff.setTextSize(1);
	dispBuff.setCursor(13,5);
	dispBuff.printf("scanning..");
	dispBuff.pushSprite(0,0);
	BLEDevice::getScan()->start(1, false);
	}
	M5.update();
	if (M5.BtnB.wasPressed()) {
	dispBuff.fillRect( 0,30,80,130,BLACK);
	dispBuff.setTextSize(2);
	dispBuff.setCursor(10,45);
	dispBuff.printf("RESET");
	dispBuff.pushSprite(0,0);
	delay(500);
	esp_restart();
	}
	loopCounter=(loopCounter+1)%1000;
	delay(50);
	}
	void getJoystick() {
	for (int i = 0; i < 5; i++) {
	joyStick[i] = i2cReadByte(0x60 + i);
	}
	}
	void dispLcd() {
	dispBuff.setTextSize(2);
	dispBuff.fillRect( 0,30,80,130,BLACK);
	dispBuff.setCursor(5,30);
	dispBuff.printf("R %03d",joyStick[2]);
	dispBuff.setCursor(28,47);
	dispBuff.printf("%03d",joyStick[3]);
	dispBuff.setCursor(5,70);
	dispBuff.printf("L %03d",joyStick[0]);
	dispBuff.setCursor(28,87);
	dispBuff.printf("%03d",joyStick[1]);
	dispBuff.setCursor(5,111);
	if (errCode!='0') dispBuff.print(errStr);
	else dispBuff.print(" ");
	float vBatt= M5.Axp.GetBatVoltage();
	dispBuff.setCursor(10,140);
	dispBuff.printf("%4.2fV ", vBatt);
	dispBuff.pushSprite(0,0);
	}
	void i2cWriteBuff(uint8_t addr, uint8_t* data, uint16_t len) {
	Wire.beginTransmission(0x38);
	Wire.write(addr);
	for (int i = 0; i < len; i++) {
	Wire.write(data[i]);
	}
	Wire.endTransmission();
	}
	uint8_t i2cReadByte(uint8_t addr) {
	Wire.beginTransmission(0x38);
	Wire.write(addr);
	Wire.endTransmission();
	Wire.requestFrom(0x38, 1);
	return Wire.read();
	}
	class funcClientCallbacks: public BLEClientCallbacks {
	void onConnect(BLEClient* pClient) {
	};
	void onDisconnect(BLEClient* pClient) {
	Serial.println("Disconnected");
	pRemoteCharacteristicRX->registerForNotify(NULL);
	delete periphDevice;
	connected = false;
	}
	};
	class advertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
	void onResult(BLEAdvertisedDevice advertisedDevice) {
	Serial.print("Advertised Device found: ");
	Serial.println(advertisedDevice.getName().c_str());
	if (advertisedDevice.haveServiceUUID()) {
	BLEUUID service = advertisedDevice.getServiceUUID();
	if (service.equals(serviceUUID)) {
	BLEDevice::getScan()->stop();
	dispBuff.fillRect(0,0,80,20, dispBuff.color565(50,50,50));
	dispBuff.setTextSize(1);
	dispBuff.setCursor(10,5);
	dispBuff.printf("Connecting");
	dispBuff.pushSprite(0,0);
	periphDevice = new BLEAdvertisedDevice(advertisedDevice);
	connecting = true;
	scanning = true;
	}
	}
	scanning =true;
	}
	};
	static void notifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
	uint8_t* pData, size_t length, bool isNotify) {
	errCode=pData[0];
	errStr=(char*)pData;
	if (errCode!='0') {
	Serial.print("Error:");
	Serial.println((char*)pData);
	}
	}
	void setupBLE() {
	BLEDevice::init("");
	BLEScan* pBLEScan = BLEDevice::getScan();
	pBLEScan->setAdvertisedDeviceCallbacks(new advertisedDeviceCallbacks());
	pBLEScan->setActiveScan(true);
	dispBuff.setTextSize(1);
	dispBuff.setCursor(17,5);
	dispBuff.printf("Scanning");
	dispBuff.pushSprite(0,0);
	pBLEScan->start(1, false);
	}
	bool connectBLE() {
	Serial.print("Forming a connection to ");
	Serial.println(periphDevice->getAddress().toString().c_str());
	BLEClient* pClient = BLEDevice::createClient();
	Serial.println(" - Created client");
	pClient->setClientCallbacks(new funcClientCallbacks());
	pClient->connect(periphDevice);
	Serial.println(" - Connected to server");
	BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
	if (pRemoteService == nullptr) {
	Serial.print("Failed to find serviceUUID: ");
	Serial.println(serviceUUID.toString().c_str());
	pClient->disconnect();
	return false;
	}
	Serial.println("Found ServiceUUID");
	pRemoteCharacteristicRX = pRemoteService->getCharacteristic(charaUUID_RX);
	if (pRemoteCharacteristicRX == nullptr) {
	Serial.print("Failed to find characteristicUUID: ");
	Serial.println(charaUUID_RX.toString().c_str());
	pClient->disconnect();
	return false;
	}
	Serial.println("Found CharaUUID RX");
	if(pRemoteCharacteristicRX->canNotify()) {
	pRemoteCharacteristicRX->registerForNotify(notifyCallback);
	}
	pRemoteCharacteristicTX = pRemoteService->getCharacteristic(charaUUID_TX);
	if (pRemoteCharacteristicTX == nullptr) {
	Serial.print("Failed to find charaUUID_TX: ");
	Serial.println(charaUUID_TX.toString().c_str());
	pClient->disconnect();
	return false;
	}
	Serial.println("Found CharaUUID TX");
	connected = true;
	return true;
	}

view raw JoyCBLEV09.ino hosted with ❤ by GitHub