ESPNOWを使ったジョイスティックコントローラ

	//
	// Joystick Remote
	// Kiraku Labo 2022
	//
	#include <M5Atom.h>
	#define SDA_PIN 26
	#define SCL_PIN 32
	#include <WiFi.h>
	#include <esp_now.h>
	#define GRN 0xf00000
	#define RED 0x00f000
	#define BLU 0x0000f0
	#define CHANNEL 0
	#define JOY_ADDR 0x52
	uint32_t msecJoy=0;
	byte robotMACaddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
	byte xyb[4]; //joystick x, jostick y, joystick button, ATOM button
	byte offset[2]; //joystick x,y offset
	int16_t xy[2], offset16[2];
	const float ROOT2=1.4142;
	uint32_t msRcv=0;
	char s[100];
	void setup() {
	M5.begin(true,false,true);
	M5.dis.drawpix(0, RED);
	Serial.begin(115200);
	Wire.begin(SDA_PIN, SCL_PIN, 50000); //SDA, SCL, freq
	setupESPNOW();
	int16_t x=0, y=0;
	for (int i=0; i<10; i++) {
	getJoy();
	x += xyb[0];
	y += xyb[1];
	}
	offset[0]=(byte)(x/10);
	offset[1]=(byte)(y/10);
	msecJoy=millis();
	}
	void loop() {
	static int8_t joyData[4];
	if (millis()-msecJoy<100) return;
	msecJoy=millis();
	getJoy();
	M5.update();
	joyData[0]=offset[0]-xyb[0];
	joyData[1]=offset[1]-xyb[1];
	joyData[2]=xyb[2];
	if (joyData[2]==1) M5.dis.drawpix(0, BLU);
	else M5.dis.drawpix(0, RED);
	if (M5.Btn.isPressed()) joyData[3]=1; //M5ATOM button (0:off, 1:on)
	else joyData[3]=0;
	esp_now_send(robotMACaddr, (uint8_t*)joyData, 4);
	}
	void getJoy() {
	Wire.requestFrom(JOY_ADDR, 3); //Request 3 bytes
	if (Wire.available()) {
	xyb[0] = Wire.read(); //joy x
	xyb[1] = Wire.read(); //joy y
	xyb[2] = Wire.read(); //joy button (0:off, 1:on for Grove)
	}
	}
	void setupESPNOW() {
	Serial.print("MAC=");
	Serial.println(WiFi.macAddress());
	WiFi.mode(WIFI_STA);
	WiFi.disconnect();
	esp_now_init();
	esp_now_register_recv_cb(onRecv);
	esp_now_peer_info_t peerInfo={};
	memcpy(peerInfo.peer_addr, robotMACaddr, 6);
	peerInfo.channel = CHANNEL;
	peerInfo.encrypt = false;
	esp_now_add_peer(&peerInfo);
	}
	void onRecv(const uint8_t * mac, const uint8_t *recvData, int len) {
	for (int i=0; i<len; i++) {
	s[i]=recvData[i];
	}
	s[len]='\0';
	Serial.flush();
	Serial.println(s);
	msRcv=millis();
	M5.dis.drawpix(0, GRN);
	}
	void sendData(String s) {
	esp_now_send(robotMACaddr, (const byte*)s.c_str(), s.length());
	}

view raw ESPNOWjoy09P.ino hosted with ❤ by GitHub

M5ATOMバランスロボをESPNOWで操縦

	//
	// M5ATOM Balancing Robot
	// Kiraku Labo, 2022
	//
	// 1. Power on and wait for a circle to shows up.
	// 2. Hold the robot upright.
	// 3. When a smile shows up, release the robot.
	//
	#include <M5Atom.h>
	#include <esp_now.h>
	#include <WiFi.h>
	#define CHANNEL 0
	#define MAXDRV 255
	#define SDA_PIN 26
	#define SCL_PIN 32
	#define MOTOR_R 0x65
	#define MOTOR_L 0x63
	#define BLU 0x0000ff
	#define GRN 0xff0000
	#define RED 0x00ff00
	#define BLK 0x000000
	boolean serialMonitor=true;
	uint16_t counter=0, counterOverSpd=0, state=0;
	uint32_t time0=0, time1=0;
	float power, powerR, powerL, yawPower, maxPwr;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0, yawAng=0.0;
	float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
	float gyroYdata, gyroZdata, accXdata, accXoffset, gyroZoffset, gyroYoffset;
	const uint32_t interval = 10; // in msec
	const float dt=(float)interval/1000.0; // in sec
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float mechFactorR, mechFactorL;
	float spinStep, spinDest, spinTarget, spinFact=1.0;
	bool joyButton=false;
	int16_t drvOffset, punchSpd, punchSpd2, punchSpd2N, punchDur, punchCountL=0, punchCountR=0;
	int16_t motorDeadband=0, counterTrip=0;
	int8_t motorRdir=0, motorLdir=0;
	bool spinContinuous=false;
	byte joyMACaddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};//{0x50, 0x02, 0x91, 0x86, 0x99, 0x68};
	const byte smile[] = {0,0,0,0,0,
	0,1,0,1,0,
	0,0,0,0,0,
	1,0,0,0,1,
	0,1,1,1,0};
	const byte frown[] = {0,0,0,0,0,
	0,1,0,1,0,
	0,0,0,0,0,
	0,1,1,1,0,
	1,0,0,0,1};
	const byte circle[] = {0,1,1,1,0,
	1,0,0,0,1,
	1,0,0,0,1,
	1,0,0,0,1,
	0,1,1,1,0};
	const byte square[] = {0,0,0,0,0,
	0,1,1,1,0,
	0,1,0,1,0,
	0,1,1,1,0,
	0,0,0,0,0};
	const byte cross[] = {1,0,0,0,1,
	0,1,0,1,0,
	0,0,1,0,0,
	0,1,0,1,0,
	1,0,0,0,1};
	const byte heart[] = {0,1,0,1,0,
	1,0,1,0,1,
	1,0,0,0,1,
	0,1,0,1,0,
	0,0,1,0,0};
	const byte all[] = {1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1};
	void setup() {
	M5.begin(true, true, true); //Serial, i2c(Wire1), disp
	Wire.begin(SDA_PIN, SCL_PIN, 50000); //clock=50kHz
	Wire.beginTransmission(MOTOR_R); //check motor driver
	if (Wire.endTransmission()!=0) { //motor driver connection problem
	ledDispImg(cross, RED);
	delay(5000);
	}
	M5.IMU.Init();
	setupESPNOW();
	delay(1500);
	resetMotor();
	resetPara();
	resetVar();
	getBaselineOmg();
	ledDispImg(circle, GRN);
	}
	void loop() {
	static uint32_t msec=0;
	getIMU();
	switch (state) {
	case 0:
	if (joyButton) ledDispImg(circle, RED);
	else ledDispImg(circle, GRN);
	if (abs(accXdata)<0.25 && aveAbsOmg<1.0) {
	ledDispImg(square, GRN);
	getBaseline();
	msec=millis();
	resetVar();
	state=1;
	}
	break;
	case 1:
	if (millis()-msec>200) state=2;
	break;
	case 2:
	if (abs(varAng)>30.0 || counterOverSpd>25) {
	if (serialMonitor) sendStatus();
	resetMotor();
	resetVar();
	ledDispImg(frown, RED);
	state=0;
	}
	else {
	if (joyButton) ledDispImg(smile, RED);
	else ledDispImg(smile, GRN);
	calcTarget();
	drive();
	}
	break;
	}
	counter += 1;
	if (counter%25==0) {
	sendData("vs=" + String(varSpd) + "\nvd=" + String(varDst) + "\nva=" + String(varAng) + "\npw=" + String(power));
	}
	if (counter >= 100) {
	counter = 0;
	if (serialMonitor) sendStatus();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void getIMU() {
	readIMU();
	varOmg = gyroYdata - gyroYoffset;
	aveAbsOmg = aveAbsOmg * 0.8 + abs(varOmg) * 0.2;
	yawAng += (gyroZdata - gyroZoffset) * dt;
	varAng = (varAng + varOmg * dt) * 0.995 + (accXdata - accXoffset) * 57.3 * 0.005;
	}
	void readIMU() {
	float gX, gY, gZ, aX, aY, aZ;
	M5.IMU.getGyroData(&gX,&gY,&gZ);
	M5.IMU.getAccelData(&aX, &aY, &aZ);
	gyroYdata=-gX;
	gyroZdata=-gY;
	accXdata=aZ;
	}
	void calcTarget() {
	spinTarget += spinStep;
	if (movePwrTarget < movePower-movePwrStep) movePwrTarget += movePwrStep;
	else if (movePwrTarget > movePower+movePwrStep) movePwrTarget -= movePwrStep;
	else movePwrTarget = movePower;
	}
	void drive() {
	varSpd += power * dt;
	varDst += Kdst * varSpd * dt;
	varIang += KIang * varAng * dt;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power) > 500.0) counterOverSpd += 1;
	else counterOverSpd=0;
	power = constrain(power, -maxPwr, maxPwr);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerR = power + yawPower + movePwrTarget;
	powerL = power - yawPower + movePwrTarget;
	int16_t 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(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(ipowerL-motorDeadband+drvOffset);
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}
	int16_t 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(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(ipowerR-motorDeadband+drvOffset);
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	}
	void drvMotorL(int16_t pwr) {
	drv8830(MOTOR_L, constrain(pwr, -255, 255));
	}
	void drvMotorR(int16_t pwr) {
	drv8830(MOTOR_R, constrain(pwr, -255, 255));
	}
	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;
	}
	void resetPara() {
	Kang=35.0;
	Komg=0.6;
	KIang=700.0;
	Kdst=11.0;
	Kspd=7.5;
	Kyaw=10.0;
	mechFactorL=0.7;
	mechFactorR=0.7;
	maxPwr=550.0;
	punchSpd=50;
	punchDur=1;
	motorDeadband=35;
	punchSpd2=max(punchSpd, motorDeadband);
	punchSpd2N=-punchSpd2;
	drvOffset=0;
	}
	void resetVar() {
	power=0.0;
	movePwrTarget=0.0;
	movePower=0.0;
	spinStep=0.0;
	spinDest=0.0;
	spinTarget=0.0;
	yawAng=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	}
	void getBaseline() {
	accXoffset=0.0;
	gyroZoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	accXoffset += accXdata;
	gyroZoffset += gyroZdata;
	delay(10);
	}
	accXoffset /= 30.0;
	gyroZoffset /= 30.0;
	}
	void getBaselineOmg() {
	gyroYoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	gyroYoffset += gyroYdata;
	delay(10);
	}
	gyroYoffset /= 30.0;
	}
	void sendStatus () {
	Serial.print("state="); Serial.print(state);
	Serial.print(" accX="); Serial.print(accXdata);
	Serial.print(" ang=");Serial.print(varAng);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println("ms");
	}
	void ledDispImg(const byte* p, uint64_t c) {
	for (byte i=0; i<25; i++) {
	M5.dis.drawpix(i, p[i]*c);
	}
	}
	void setupESPNOW() {
	Serial.print("MAC=");
	Serial.println(WiFi.macAddress());
	WiFi.mode(WIFI_STA);
	esp_now_init();
	esp_now_register_recv_cb(onRecv);
	esp_now_peer_info_t peerInfo={};
	memcpy(peerInfo.peer_addr, joyMACaddr, 6);
	peerInfo.channel = CHANNEL;
	peerInfo.encrypt = false;
	esp_now_add_peer(&peerInfo);
	}
	void onRecv(const uint8_t * mac, const uint8_t *recvData, int len) {
	if ((int8_t)recvData[2]==1) joyButton=true;
	else joyButton=false;
	steer((int8_t)recvData[0], (int8_t)recvData[1]);
	}
	void sendData(String s) {
	esp_now_send(joyMACaddr, (const byte*)s.c_str(), s.length());
	}
	void steer(int8_t x, int8_t y) {
	if (abs(y)>20) movePower=(float)y*0.5;
	else movePower=0.0;
	if (abs(x)>30) spinStep=(float)x*0.015;
	else spinStep=0.0;
	}

view raw M5BalanceV068P.ino hosted with ❤ by GitHub

M5StickCとサーボでバランスロボ

	// 2022 Kiraku Labo
	#include <M5StickC.h>
	#define SERVO_PIN_R 0
	#define SERVO_PIN_L 26
	int16_t pulseZeroR, pulseZeroL;
	boolean serialMonitor=true;
	uint16_t counter=0, counterOverSpd=0, state=0, walkPhase=0;
	uint32_t time0=0, time1=0;
	float power, powerR, powerL, yawPower, maxPwr;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0, yawAng=0.0;
	float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
	float gyroYdata, gyroZdata, accXdata, gyroYoffset, accXoffset, gyroZoffset;
	const uint32_t interval = 10; // in msec
	const float dt=(float)interval/1000.0; // in sec
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float mechFactorR, mechFactorL;
	float spinStep, spinDest, spinTarget, spinFact=1.0;
	void setup() {
	M5.begin();
	M5.Axp.ScreenBreath(11);
	M5.Lcd.setRotation(3);
	M5.Lcd.setTextFont(1);
	M5.Lcd.fillScreen(BLACK);
	M5.IMU.Init();
	pinMode(SERVO_PIN_L, OUTPUT);
	pinMode(SERVO_PIN_R, OUTPUT);
	resetMotor();
	resetPara();
	resetVar();
	delay(1000);
	getBaselineGyro();
	}
	void loop() {
	static uint32_t msec=0;
	getIMU();
	switch (state) {
	case 0:
	if (abs(accXdata)<0.25 && aveAbsOmg<1.0) {
	M5.Lcd.setCursor(10, 20);
	M5.Lcd.println("Calibrating..");
	getBaseline();
	msec=millis();
	resetVar();
	state=1;
	}
	break;
	case 1:
	if (millis()-msec>200) {
	M5.Lcd.fillScreen(BLACK);
	state=2;
	}
	break;
	case 2:
	if (abs(varAng)>30.0 || counterOverSpd>20) {
	if (serialMonitor) sendStatus();
	resetMotor();
	resetVar();
	state=0;
	}
	else {
	calcTarget();
	drive();
	}
	break;
	}
	counter += 1;
	if (counter >= 100) {
	counter = 0;
	if (serialMonitor) sendStatus();
	M5.Lcd.setCursor(10, 10);
	float vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
	M5.Lcd.printf("Bat=%4.2fv ", vBatt);
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void getIMU() {
	readIMU();
	varOmg = gyroYdata - gyroYoffset;
	aveAbsOmg = aveAbsOmg * 0.8 + abs(varOmg) * 0.2;
	yawAng += (gyroZdata - gyroZoffset) * dt;
	varAng = (varAng + varOmg * dt) * 0.995 + (accXdata - accXoffset) * 57.3 * 0.005;
	}
	void readIMU() {
	float gX, gY, gZ, aX, aY, aZ;
	M5.IMU.getGyroData(&gX,&gY,&gZ);
	M5.IMU.getAccelData(&aX, &aY, &aZ);
	gyroYdata=-gY;
	gyroZdata=-gZ;
	accXdata=aX;
	}
	void calcTarget() {
	if (abs(movePower)>10.0) spinFact=constrain(-(powerR+powerL)/150.0, -3.0, 3.0); //moving
	else spinFact=1.0; //standing
	spinTarget += spinStep * spinFact;
	if (movePwrTarget < movePower-movePwrStep) movePwrTarget += movePwrStep;
	else if (movePwrTarget > movePower+movePwrStep) movePwrTarget -= movePwrStep;
	else movePwrTarget = movePower;
	}
	void drive() {
	varSpd += power * dt;
	varDst += Kdst * varSpd * dt;
	varIang += KIang * varAng * dt;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power)>1000.0) counterOverSpd += 1;
	else counterOverSpd=0;
	power = constrain(power, -maxPwr, maxPwr);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerL = power - yawPower + movePwrTarget;
	powerR = power + yawPower + movePwrTarget;
	int16_t ipowerL = (int16_t) constrain(powerL * mechFactorL, -maxPwr, maxPwr);
	int16_t ipowerR = (int16_t) constrain(powerR * mechFactorR, -maxPwr, maxPwr);
	drvMotor(ipowerL, ipowerR);
	}
	void drvMotor(int pwmL, int pwmR) {
	int pulseR=constrain(pulseZeroR-pwmR, 1000, 2000);
	int pulseL=constrain(pulseZeroL+pwmL, 1000, 2000);
	bool doneR=false;
	bool doneL=false;
	uint32_t usec=micros();
	digitalWrite(SERVO_PIN_R, HIGH);
	digitalWrite(SERVO_PIN_L, HIGH);
	while(!doneR || !doneL) {
	uint32_t width=micros()-usec;
	if (width>=pulseL) {
	digitalWrite(SERVO_PIN_L, LOW);
	doneL=true;
	}
	if (width>=pulseR) {
	digitalWrite(SERVO_PIN_R, LOW);
	doneR=true;
	}
	}
	}
	void resetVar() {
	power=0.0;
	movePwrTarget=0.0;
	movePower=0.0;
	spinStep=0.0;
	spinDest=0.0;
	spinTarget=0.0;
	yawAng=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	}
	void resetPara() {
	Kang=35.0;
	Komg=0.8;
	KIang=700.0;
	Kyaw=4.0;
	Kdst=20.0;
	Kspd=2.5;
	mechFactorR=0.5;
	mechFactorL=0.5;
	maxPwr=500.0;
	pulseZeroL=1460; //inc=forward
	pulseZeroR=1490; //dec=forward
	}
	void resetMotor() {
	digitalWrite(SERVO_PIN_L, LOW);
	digitalWrite(SERVO_PIN_R, LOW);
	counterOverSpd=0;
	}
	void getBaseline() {
	accXoffset=0.0;
	gyroZoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	accXoffset += accXdata;
	gyroZoffset += gyroZdata;
	delay(10);
	}
	accXoffset /= 30.0;
	gyroZoffset /= 30.0;
	}
	void getBaselineGyro() {
	gyroYoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	gyroYoffset += gyroYdata;
	delay(10);
	}
	gyroYoffset /= 30.0;
	}
	void sendStatus () {
	Serial.print("state="); Serial.print(state);
	Serial.print(" accX="); Serial.print(accXdata);
	Serial.print(" ang=");Serial.print(varAng);
	Serial.print(" omg="); Serial.print(varOmg);
	Serial.print(" pwr="); Serial.print(power);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println("ms");
	}

view raw M5BalServoV048.ino hosted with ❤ by GitHub

M5ATOMでバランスロボ

	//
	// M5ATOM Balancing Robot V047
	// Kiraku Labo, 2022
	//
	// 1. Power on and wait for a circle to shows up.
	// 2. Hold the robot upright and still.
	// 3. When a smile shows up, release the robot.
	//
	// Push Matrix button to switch between standig mode and walking mode.
	//
	#include <M5Atom.h>
	#define SDA_PIN 26
	#define SCL_PIN 32
	#define MOTOR_R 0x65
	#define MOTOR_L 0x63
	#define BLU 0x0000ff
	#define GRN 0xff0000
	#define RED 0x00ff00
	#define BLK 0x000000
	boolean serialMonitor=false;
	uint16_t counter=0, counterOverSpd=0, state=0, walkPhase=0;
	uint32_t time0=0, time1=0;
	float power, powerR, powerL, yawPower, maxPwr;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0, yawAng=0.0;
	float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
	float gyroYdata, gyroZdata, accXdata, accXoffset, gyroZoffset;
	const uint32_t interval = 10; // in msec
	const float deltaT=(float)interval/1000.0; // in sec
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float mechFactorR, mechFactorL;
	float spinStep, spinDest, spinTarget, spinFact=1.0;
	bool walkMode=false;
	const byte smile[] = {0,0,0,0,0,
	0,1,0,1,0,
	0,0,0,0,0,
	1,0,0,0,1,
	0,1,1,1,0};
	const byte frown[] = {0,0,0,0,0,
	0,1,0,1,0,
	0,0,0,0,0,
	0,1,1,1,0,
	1,0,0,0,1};
	const byte circle[] = {0,1,1,1,0,
	1,0,0,0,1,
	1,0,0,0,1,
	1,0,0,0,1,
	0,1,1,1,0};
	const byte square[] = {0,0,0,0,0,
	0,1,1,1,0,
	0,1,0,1,0,
	0,1,1,1,0,
	0,0,0,0,0};
	const byte cross[] = {1,0,0,0,1,
	0,1,0,1,0,
	0,0,1,0,0,
	0,1,0,1,0,
	1,0,0,0,1};
	const byte heart[] = {0,1,0,1,0,
	1,0,1,0,1,
	1,0,0,0,1,
	0,1,0,1,0,
	0,0,1,0,0};
	const byte all[] = {1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1,
	1,1,1,1,1};
	void setup() {
	M5.begin(true, true, true); //Serial, i2c(Wire1), disp
	Wire.begin(SDA_PIN, SCL_PIN, 50000); //clock=50kHz
	Wire.beginTransmission(MOTOR_R); //check motor driver
	if (Wire.endTransmission()!=0) { //motor driver connection problem
	ledDispImg(cross, RED);
	delay(5000);
	}
	M5.IMU.Init();
	resetMotor();
	resetPara();
	resetVar();
	ledDispImg(circle, GRN);
	}
	void loop() {
	static uint32_t msec=0;
	M5.update();
	checkButton();
	getIMU();
	switch (state) {
	case 0:
	if (abs(accXdata)<0.25 && aveAbsOmg<1.0) {
	ledDispImg(square, walkMode?BLU:GRN);
	getBaseline();
	msec=millis();
	resetVar();
	state=1;
	}
	break;
	case 1:
	if (millis()-msec>200) {
	state=2;
	if (walkMode) walkPhase=1;
	ledDispImg(smile, walkMode?BLU:GRN);
	}
	break;
	case 2:
	if (abs(varAng)>30.0 || counterOverSpd>25) {
	if (serialMonitor) sendStatus();
	resetMotor();
	resetVar();
	ledDispImg(frown, RED);
	state=0;
	}
	else {
	if (walkMode) walk();
	calcTarget();
	drive();
	}
	break;
	}
	counter += 1;
	if (counter >= 100) {
	counter = 0;
	if (serialMonitor) sendStatus();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void walk() {
	static uint32_t ms=0;
	switch (walkPhase) {
	case 1:
	ms=millis();
	walkPhase=2;
	break;
	case 2:
	if (millis()-ms>2000) walkPhase=3;
	break;
	case 3:
	spinStep=0.4;
	movePower=40.0;
	walkPhase=4;
	break;
	case 4:
	break;
	}
	}
	void checkButton() {
	if (M5.Btn.wasPressed()) {
	if (walkMode) {
	walkMode=false;
	if (state==0) ledDispImg(all, BLK);
	else ledDispImg(smile, GRN);
	spinStep=0.0;
	movePower=0.0;
	}
	else {
	walkMode=true;
	walkPhase=1;
	if (state==0) ledDispImg(heart, BLU);
	else ledDispImg(smile, BLU);
	}
	}
	}
	void getIMU() {
	readIMU();
	varOmg = gyroYdata;
	aveAbsOmg = aveAbsOmg * 0.8 + abs(varOmg) * 0.2;
	yawAng += (gyroZdata - gyroZoffset) * deltaT;
	varAng = (varAng + varOmg * deltaT) * 0.995 + (accXdata - accXoffset) * 57.3 * 0.005;
	}
	void readIMU() {
	float gX, gY, gZ, aX, aY, aZ;
	M5.IMU.getGyroData(&gX,&gY,&gZ);
	M5.IMU.getAccelData(&aX, &aY, &aZ);
	gyroYdata=-gX;
	gyroZdata=-gY;
	accXdata=aZ;
	}
	void calcTarget() {
	if (abs(movePower)>10.0) spinFact=constrain(-(powerR+powerL)/150.0, -3.0, 3.0); //moving
	else spinFact=1.0; //standing
	spinTarget += spinStep * spinFact;
	if (movePwrTarget < movePower-movePwrStep) movePwrTarget += movePwrStep;
	else if (movePwrTarget > movePower+movePwrStep) movePwrTarget -= movePwrStep;
	else movePwrTarget = movePower;
	}
	void drive() {
	varSpd += power * deltaT;
	varDst += Kdst * varSpd * deltaT;
	varIang += KIang * varAng * deltaT;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power)>500.0) counterOverSpd += 1;
	else counterOverSpd=0;
	power = constrain(power, -maxPwr, maxPwr);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerR = power + yawPower + movePwrTarget;
	powerL = power - yawPower + movePwrTarget;
	int16_t ipowerL = (int16_t) constrain(powerL * mechFactorL, -maxPwr, maxPwr);
	drv8830(MOTOR_L, ipowerL);
	int16_t ipowerR = (int16_t) constrain(powerR * mechFactorR, -maxPwr, maxPwr);
	drv8830(MOTOR_R, ipowerR);
	}
	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;
	}
	void resetPara() {
	Kang=35.0;
	Komg=0.6;
	KIang=500.0;
	Kdst=11.0;
	Kspd=2.5;
	Kyaw=10.0;
	mechFactorL=1.0;
	mechFactorR=1.0;
	maxPwr=250.0;
	}
	void resetVar() {
	power=0.0;
	movePwrTarget=0.0;
	movePower=0.0;
	spinStep=0.0;
	spinDest=0.0;
	spinTarget=0.0;
	yawAng=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	}
	void getBaseline() {
	accXoffset=0.0;
	gyroZoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	accXoffset += accXdata;
	gyroZoffset += gyroZdata;
	delay(10);
	}
	accXoffset /= 30.0;
	gyroZoffset /= 30.0;
	}
	void sendStatus () {
	Serial.print("state="); Serial.print(state);
	Serial.print(" accX="); Serial.print(accXdata);
	Serial.print(" ang=");Serial.print(varAng);
	Serial.print(" gyroY="); Serial.print(gyroYdata);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println("ms");
	}
	void ledDispImg(const byte* p, uint64_t c) {
	for (byte i=0; i<25; i++) {
	M5.dis.drawpix(i, p[i]*c);
	}
	}

view raw M5BalanceV047.ino hosted with ❤ by GitHub

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

 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

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

micro:bitと電動消しゴムでバランスロボ

ハードウェアは こちらをご覧ください。

PWM周波数の変更のため、wiring_analog_nRF51.c 内の関数 void analogWrite( uint32_t ulPin, uint32_t ulValue ) の

NRF_TIMER1->PRESCALER = (NRF_TIMER1->PRESCALER & ~TIMER_PRESCALER_PRESCALER_Msk) | ((7 << TIMER_PRESCALER_PRESCALER_Pos) & TIMER_PRESCALER_PRESCALER_Msk);

を 

NRF_TIMER1->PRESCALER = (NRF_TIMER1->PRESCALER & ~TIMER_PRESCALER_PRESCALER_Msk) | ((3 << TIMER_PRESCALER_PRESCALER_Pos) & TIMER_PRESCALER_PRESCALER_Msk);

 に変更しました。

 また、i2cのタイムアウトのために、Wire_nRF51.cpp 内の関数 uint8_t TwoWire::endTransmission(bool stopBit) の

    _p_twi->TXD = txBuffer.read_char();
    while(!_p_twi->EVENTS_TXDSENT && !_p_twi->EVENTS_ERROR) ;

を

     _p_twi->TXD = txBuffer.read_char();
    uint32_t usecTimeOut=micros();
    while(!_p_twi->EVENTS_TXDSENT && !_p_twi->EVENTS_ERROR) {
      if (micros()-usecTimeOut>3000) return 4;
    }

 に変更しました。

以下ソースコードです。

	//
	// Keshgomu Robo
	// by Kiraku Labo
	//
	// MPU: micro:bit(nRF51822)
	// Motor driver: KS0308(TB6612)
	// Gyro: GY-521(MPU6050)
	// IR: GP1UXC41QS
	//
	#include <Wire.h>
	#include <BLEPeripheral.h>
	#define KY0308
	#define BUTTON_A 5
	#define BUTTON_B 11
	#define BATT 0
	#define IRPIN 8
	#define DRV_RESET 14
	#ifdef KY0308
	#define PWMA 1
	#define PWMB 2
	#define INA1 13
	#define INA2 12
	#define INB1 15
	#define INB2 16
	#endif
	#define NUM_IR_KEY 12
	#define HEADER 0x4D
	#define MIN_IR_INTERVAL 220
	#define IR_BUF_LEN 200
	#define IR_ON LOW
	#define IR_OFF HIGH
	BLEPeripheral blePeripheral = BLEPeripheral();
	BLEService service = BLEService("75719c1e-6299-11e8-adc0-fa7ae01bbebc");
	BLECharacteristic chRx = BLECharacteristic("75719f7a-6299-11e8-adc0-fa7ae01bbebc", BLEWriteWithoutResponse, 10);
	BLECharacteristic chTx = BLECharacteristic("7571a1e6-6299-11e8-adc0-fa7ae01bbebc", BLENotify, 20);
	extern "C" {void TIMER2_IRQHandler() {intHandler();}}
	byte serialMon=1; //0:No Serial 1:Brief 2:Verbose
	boolean dbg=true;
	uint16_t ver=49;
	int16_t state=-1;
	int16_t accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
	volatile int16_t loopCounter=1, loopCounter0=0;
	uint32_t time0=0, time1=0;
	float power, powerR, powerL;
	float gyroZdps, accXg;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0;;
	float gyroZoffset, gyroYoffset, accXoffset;
	float gyroXdata, gyroZdata, gyroYdata, accXdata, accYdata, accZdata;
	float gyroLSB=1.0/32.8; //at FS=1000dps, 1.0/131.0; //at FS=250dps
	float accLSB=1.0/16384.0; // g
	float cutoff=0.01; //~=2 * pi * f (Hz) assuming clk is negligible compared to 1/w
	float clk=0.01; // in sec,
	uint32_t interval=10; // in msec
	float aveAccZ=0.0;
	int32_t distance;
	float Komg, Kang, KIang, Kyaw, Kspd, Kdst;
	float mechFactorR, mechFactorL;
	int16_t maxSpd;
	int16_t maxOvs=30;
	int16_t punchSpd, drvOffset;
	float yawPower;
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float spinDest, spinTarget, spinFact=1.0;
	float spinStep=0.0;
	float yawAng;
	bool spinContinuous=false;
	int16_t motorDeadBand=0;
	int16_t counterOverSpd;
	int16_t ipowerR=0, ipowerL=0;
	int16_t motorRdir=0, motorLdir=0; //stop 1:+ -1:-
	float battFactor;
	int16_t drvRsim=0, drvLsim=0;
	float lrBalance=0.0, spinBalAdj=0.0;
	float volt;
	volatile byte ledBuf[3][9];
	volatile byte ledRow=0;
	volatile uint16_t irqCounter=0;
	uint16_t devCode[NUM_IR_KEY+1] = {0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF, 0x10EF};
	uint16_t keyCode[NUM_IR_KEY+1] = {0xFFFF, 0xB14E, 0xA05F, 0x21DE, 0x10EF, 0x20DF, 0x807F, 0x11EE, 0x00FF, 0x817E, 0xF807, 0x7887, 0x58A7};
	int16_t keyId=-1;
	volatile uint32_t irMicroOn, irMicroOff;
	volatile uint16_t irDataOn[200], irDataOff[200];
	volatile int16_t irOnIndex=0, irOffIndex=0;
	volatile boolean irStarted=false, irDecoding=false;
	volatile uint32_t lastIrTime=0;
	boolean keyCodeReady=false;
	byte irData[30];
	uint16_t irKeyCode, irDevCode;
	bool byIrRemote=true; //true=TV remote, false=BLE joystick
	float angAdj=0.0;
	byte joyLX, joyLY, joyRX, joyRY, joySW;
	bool errWDT=false, errFell=false, errBat=false, errOvs=false, erri2c=false;
	float errData=0.0;
	const byte rowIO[3] = {26, 27, 28};
	const byte colIO[9] = {3, 4, 10, 23, 24, 25, 9, 7, 6};
	const byte rowIndx[5][5] = {{0,1,0,1,0}, {2,2,2,2,2}, {1,0,1,2,1}, {0,0,0,0,0}, {2,1,2,1,2}};
	const byte colIndx[5][5] = {{0,3,1,4,2}, {3,4,5,6,7}, {1,8,2,8,0}, {7,6,5,4,3}, {2,6,0,5,1}};
	const byte ledEmptyHeart[] = {0x0A, 0x15, 0x11, 0x0A, 0x04};
	const byte ledFilledHeart[] = {0x0A, 0x1F, 0x1F, 0x0E, 0x04};
	const byte ledSmile[] = {0x00, 0x0A, 0x00, 0x11, 0x0E};
	const byte ledFrown[] = {0x00, 0x0A, 0x00, 0x0E, 0x11};
	const byte ledFont[38][5] = {
	{0x04, 0x0A, 0x0A, 0x0A, 0x04}, // 0
	{0x04, 0x0C, 0x04, 0x04, 0x0E}, // 1
	{0x1C, 0x02, 0x0C, 0x10, 0x1E}, // 2
	{0x1E, 0x02, 0x04, 0x12, 0x0C}, // 3
	{0x04, 0x0C, 0x14, 0x1E, 0x04}, // 4
	{0x1C, 0x10, 0x1C, 0x02, 0x1C}, // 5
	{0x0C, 0x10, 0x1C, 0x12, 0x0C}, // 6
	{0x0E, 0x02, 0x02, 0x04, 0x08}, // 7
	{0x0C, 0x12, 0x0C, 0x12, 0x0C}, // 8
	{0x0C, 0x12, 0x0E, 0x02, 0x0C}, // 9
	{0x0C, 0x12, 0x1E, 0x12, 0x12}, // A
	{0x1C, 0x12, 0x1C, 0x12, 0x1C}, // B
	{0x0C, 0x12, 0x10, 0x12, 0x0C}, // C
	{0x1C, 0x12, 0x12, 0x12, 0x1C}, // D
	{0x1E, 0x10, 0x1C, 0x10, 0x1E}, // E
	{0x1E, 0x10, 0x1C, 0x10, 0x10}, // F
	{0x0C, 0x10, 0x16, 0x12, 0x0C}, // G
	{0x12, 0x12, 0x1E, 0x12, 0x12}, // H
	{0x0E, 0x04, 0x04, 0x04, 0x0E}, // I
	{0x1E, 0x04, 0x04, 0x14, 0x08}, // J
	{0x12, 0x14, 0x18, 0x14, 0x12}, // K
	{0x08, 0x08, 0x08, 0x08, 0x0E}, // L
	{0x12, 0x1E, 0x12, 0x12, 0x12}, // M
	{0x12, 0x1A, 0x16, 0x12, 0x12}, // N
	{0x0C, 0x12, 0x12, 0x12, 0x0C}, // O
	{0x1C, 0x12, 0x1C, 0x10, 0x10}, // P
	{0x0C, 0x12, 0x12, 0x0C, 0x06}, // Q
	{0x1C, 0x12, 0x1C, 0x14, 0x12}, // R
	{0x0E, 0x10, 0x0C, 0x02, 0x1C}, // S
	{0x0E, 0x04, 0x04, 0x04, 0x04}, // T
	{0x12, 0x12, 0x12, 0x12, 0x0C}, // U
	{0x12, 0x12, 0x12, 0x0A, 0x04}, // V
	{0x12, 0x12, 0x12, 0x1E, 0x12}, // W
	{0x12, 0x12, 0x0C, 0x12, 0x12}, // X
	{0x12, 0x0A, 0x04, 0x04, 0x04}, // Y
	{0x1E, 0x04, 0x08, 0x10, 0x1E}, // Z
	{0x00, 0x00, 0x00, 0x00, 0x00}, // space
	{0x00, 0x00, 0x00, 0x00, 0x04}}; // .
	void setup() {
	delay(100);
	pinMode(BATT, INPUT);
	pinMode(BUTTON_A, INPUT_PULLUP);
	pinMode(BUTTON_B, INPUT_PULLUP);
	pinMode(IRPIN, INPUT_PULLUP);
	pinMode(DRV_RESET, OUTPUT);
	digitalWrite(DRV_RESET, LOW);
	delay(10);
	digitalWrite(DRV_RESET, HIGH);
	Serial.begin(115200);
	attachInterrupt(IRPIN, irInt, CHANGE);
	Wire.begin();
	Wire.setClock(50000L);
	setupMPU6050();
	setupTimer();
	setupBLE();
	ledInit();
	resetPara();
	resetVar();
	showVer();
	setupMotorDriver(); //allow one sec for moto:bit firmware to be ready ater DRV_RESET
	getBaseline1();
	ledClear();
	}
	void loop() {
	getIR();
	getGyro();
	checkBLE();
	if (digitalRead(BUTTON_A)==LOW) showVolt();
	if (digitalRead(BUTTON_B)==LOW) {
	delay(500);
	getBaseline1();
	}
	if (state==-1) {
	if (abs(accXdata)<0.2 && aveAbsOmg<1.0) {
	errFell=false;
	errOvs=false;
	digitalWrite(DRV_RESET, HIGH);
	state=0;
	}
	else driveSim();
	}
	else if (state==0) {
	calibrate();
	drawBitmap(ledSmile);
	state=1;
	}
	else if (state==1) {
	if (abs(aveAccZ)<0.5 || counterOverSpd>maxOvs) {
	if (counterOverSpd>maxOvs) {
	errOvs=true;
	errData=(float)maxOvs;
	}
	else {
	errFell=true;
	errData=aveAccZ;
	}
	resetMotor();
	resetVar();
	sendStatus();
	state=-1;
	}
	else {
	calcTarget();
	drive();
	}
	}
	loopCounter += 1;
	char err='0';
	if (loopCounter%10==0) { //every 0.1 sec
	getBattVolt();
	setBattFact();
	if (errWDT) err='W';
	else if (erri2c) err='I';
	else if (errBat) err='B';
	else if (errOvs) err='S';
	else if (errFell) {
	err='F';
	drawBitmap(ledFrown);
	}
	else {
	err='0';
	drawBitmap(ledSmile);
	}
	if (err!='0' && err!='F') ledPrint(err);
	}
	if (loopCounter>=100) { //every 1 sec
	loopCounter=0;
	if (errFell || errOvs || errBat) sendBLE(String(err)+String(errData));
	else sendBLE(String(err));
	sendStatus();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void getBaseline1() {
	ledClear();
	drawBitmap(ledEmptyHeart);
	gyroZoffset=gyroYoffset=0.0;
	for (int i=1; i <= 30; i++) {
	readGyro();
	gyroYoffset += gyroYdata;
	gyroZoffset += gyroZdata;
	delay(20);
	}
	gyroYoffset /= 30.0;
	gyroZoffset /= 30.0;
	ledClear();
	}
	void calibrate() {
	resetVar();
	resetMotor();
	drawBitmap(ledFilledHeart);
	delay(300);
	sendStatus();
	getBaseline2();
	ledClear();
	}
	void getBaseline2() {
	gyroZoffset=accXoffset=0.0;
	for (int i=1; i <= 30; i++) {
	readGyro();
	accXoffset += accXdata;
	gyroZoffset += gyroZdata;
	delay(20);
	}
	accXoffset /= 30.0;
	gyroZoffset /= 30.0;
	}
	void resetVar() {
	power=0.0;
	movePwrTarget=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;
	angAdj=0.0;
	}
	void sendStatus() {
	if (serialMon==0) return;
	char buf[30];
	sprintf(buf, "[%d", millis()-time0); Serial.print(buf);
	// sprintf(buf, " st=%d", state); Serial.print(buf);
	// sprintf(buf, " aX=%.2f", accXdata); Serial.print(buf);
	// sprintf(buf, " aY=%.2f", accY); Serial.print(buf);
	// sprintf(buf, " avZ=%.2f", aveAccZ); Serial.print(buf);
	// sprintf(buf, " ang=%.2f", varAng); Serial.print(buf);
	sprintf(buf, " MPT=%.2f", movePwrTarget); Serial.print(buf);
	sprintf(buf, " yPwr=%.2f", yawPower); Serial.print(buf);
	// sprintf(buf, " pwrL=%.2f", powerL); Serial.print(buf);
	// sprintf(buf, " pwrR=%.2f", powerR); Serial.print(buf);
	// sprintf(buf, " drvL=%d", drvLsim); Serial.print(buf);
	// sprintf(buf, " drvR=%d", drvRsim); Serial.print(buf);
	// sprintf(buf, " spT=%.2f", spinTarget); Serial.print(buf);
	// sprintf(buf, " Omg=%5.1f", varOmg); Serial.print(buf);
	// sprintf(buf, " power=%5.0f", power); Serial.print(buf);
	// sprintf(buf, " Iang=%5.0f", varIang); Serial.print(buf);
	sprintf(buf, " %d]", millis()-time0); Serial.print(buf);
	Serial.println();
	Serial.flush();
	}
	void calcTarget() {
	if (spinContinuous) {
	if (abs(movePower)>=5.0) spinFact=constrain((powerR+powerL)/15.0, -1.0, 1.0); //moving
	else spinFact=1.0; //standing
	spinTarget += spinStep * spinFact;
	}
	else {
	if (abs(spinDest-spinTarget)>=abs(spinStep)) spinTarget += spinStep;
	else spinTarget=spinDest;
	}
	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 (abs(power) > 1000.0) counterOverSpd += 1;
	else counterOverSpd =0;
	if (counterOverSpd > maxOvs) return;
	power=constrain(power, -maxSpd, maxSpd);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerR = power + yawPower + movePwrTarget;
	powerL = power - yawPower + movePwrTarget;
	ipowerR = (int16_t) constrain(powerR * mechFactorR * battFactor, -maxSpd, maxSpd);
	if (ipowerR > 0) {
	if (motorRdir == 1) drvMotorR(max(ipowerR, motorDeadBand)+drvOffset);
	else drvMotorR(max(ipowerR, motorDeadBand) + punchSpd+drvOffset);
	motorRdir = 1;
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) drvMotorR(min(ipowerR, -motorDeadBand)+drvOffset);
	else drvMotorR(min(ipowerR, -motorDeadBand) - punchSpd+drvOffset);
	motorRdir = -1;
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	ipowerL = (int16_t) constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd);
	if (ipowerL > 0) {
	if (motorLdir == 1) drvMotorL(max(ipowerL, motorDeadBand)+drvOffset);
	else drvMotorL(max(ipowerL, motorDeadBand) + punchSpd+drvOffset);
	motorLdir = 1;
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) drvMotorL(min(ipowerL, -motorDeadBand)+drvOffset);
	else drvMotorL(min(ipowerL, -motorDeadBand) - punchSpd+drvOffset);
	motorLdir = -1;
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}
	}
	void driveSim() {
	spinTarget = 20.0*spinStep * spinFact;
	calcMoveTarget();
	yawPower = ( - spinTarget) * Kyaw;
	powerR = yawPower + movePwrTarget;
	powerL = -yawPower + movePwrTarget;
	ipowerR = (int16_t) constrain(powerR * mechFactorR * battFactor, -maxSpd, maxSpd);
	if (ipowerR > 0) {
	if (motorRdir == 1) drvRsim=(max(ipowerR, motorDeadBand)+drvOffset);
	else drvRsim=(max(ipowerR, motorDeadBand) + punchSpd+drvOffset);
	drvMotorR(drvRsim);
	motorRdir = 1;
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) drvRsim=(min(ipowerR, -motorDeadBand)+drvOffset);
	else drvRsim=(min(ipowerR, -motorDeadBand) - punchSpd+drvOffset);
	drvMotorR(drvRsim);
	motorRdir = -1;
	}
	else {
	drvRsim=0;
	drvMotorR(0);
	motorRdir = 0;
	}
	ipowerL = (int16_t) constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd);
	if (ipowerL > 0) {
	if (motorLdir == 1) drvLsim=(max(ipowerL, motorDeadBand)+drvOffset);
	else drvLsim=(max(ipowerL, motorDeadBand) + punchSpd+drvOffset);
	drvMotorL(drvLsim);
	motorLdir = 1;
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) drvLsim=(min(ipowerL, -motorDeadBand)+drvOffset);
	else drvLsim=(min(ipowerL, -motorDeadBand) - punchSpd+drvOffset);
	drvMotorL(drvLsim);
	motorLdir = -1;
	}
	else {
	drvLsim=0;
	drvMotorL(0);
	motorLdir = 0;
	}
	}
	void resetMotor() {
	drvMotorR(0);
	drvMotorL(0);
	counterOverSpd=0;
	}
	#ifdef KY0308
	void resetPara() {
	Kang=35.0;
	Komg=0.7;
	KIang=700.0;
	Kyaw=3.0;
	Kdst=25.0;
	Kspd=5.0;
	mechFactorR=0.45;
	mechFactorL=0.5;
	punchSpd=15;
	motorDeadBand=5;
	drvOffset=0;
	maxSpd=200;
	lrBalance=0.0;
	}
	void setupMotorDriver() {
	//IN1,IN2: 0,0=hi z, 1,0=OUT1ON/OUT2OFF, 0,1=OUT1OFF/OUT2ON, 1,1=brake
	pinMode(INA2, OUTPUT);
	pinMode(INA1, OUTPUT);
	pinMode(INB1, OUTPUT);
	pinMode(INB2, OUTPUT);
	pinMode(PWMA, OUTPUT);
	pinMode(PWMB, OUTPUT);
	digitalWrite(14, HIGH);
	}
	void drvMotorR(int pwm) {
	if (pwm>0) {
	digitalWrite(15, LOW);
	digitalWrite(16, HIGH);
	analogWrite(2, pwm);
	}
	else if (pwm<0) {
	digitalWrite(15, HIGH);
	digitalWrite(16, LOW);
	analogWrite(2, -pwm);
	}
	else {
	digitalWrite(15, HIGH);
	digitalWrite(16, HIGH);
	analogWrite(2, 0);
	}
	}
	void drvMotorL(int pwm) {
	if (pwm>0) {
	digitalWrite(12, LOW);
	digitalWrite(13, HIGH);
	analogWrite(1, pwm);
	}
	else if (pwm<0) {
	digitalWrite(12, HIGH);
	digitalWrite(13, LOW);
	analogWrite(1, -pwm);
	}
	else {
	digitalWrite(12, HIGH);
	digitalWrite(13, HIGH);
	analogWrite(1, 0);
	}
	}
	#endif //KY0308
	void getGyro() {
	readGyro();
	varOmg = (gyroYdata - gyroYoffset);
	if (state!=1) aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
	yawAng += (gyroZdata - gyroZoffset) * clk;
	varAng += (varOmg + ((accXdata - accXoffset) * 57.3 - varAng) * cutoff ) * clk;
	// varAng = (varAng+ varOmg*clk)*0.999 + accXg*57.3 *0.001;
	}
	void readGyro() {
	Wire.beginTransmission(0x68);
	Wire.write(0x3B);
	int16_t err=Wire.endTransmission();
	if (err) {
	Serial.println("i2c timeout");Serial.flush();
	erri2c=true;
	reseti2c();
	return;
	}
	else erri2c=false;
	Wire.requestFrom(0x68, 14, true);
	accZ=(Wire.read()<<8|Wire.read()); //0x3B acc x
	accY=Wire.read()<<8|Wire.read(); //0x3D acc y
	accX=Wire.read()<<8|Wire.read(); //0x3F acc z
	temp=Wire.read()<<8|Wire.read(); //0x41
	gyroZ=-(Wire.read()<<8|Wire.read()); //0x43 gyro x
	gyroY=(Wire.read()<<8|Wire.read()); //0x45 gyro y
	gyroX=Wire.read()<<8|Wire.read(); //0x47 gyro z
	gyroXdata = (float) gyroX * gyroLSB;
	gyroZdata = (float) gyroZ * gyroLSB;
	gyroYdata = (float) gyroY * gyroLSB;
	accXdata = (float) accX * accLSB;
	accYdata = (float) accY * accLSB;
	accZdata = (float) accZ * accLSB;
	aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
	}
	void setupMPU6050() {
	Wire.beginTransmission(0x68);
	Wire.write(0x6B); // Power management register
	Wire.write((byte)0x00); // wake up
	Wire.endTransmission();
	Wire.beginTransmission(0x68);
	Wire.write(0x1B); // Gyro config register
	Wire.write((byte)0x10); // Gyro full scale 0x00=250deg/s, 0x08=500deg/s, 0x10=1000deg/s, 0x18=2000deg/s
	Wire.endTransmission();
	}
	void reseti2c() {
	Wire.end();
	pinMode(20, INPUT);
	for (byte i=0; i<20; i++) {
	pinMode(19, INPUT);
	delayMicroseconds(5);
	pinMode(19, OUTPUT);
	digitalWrite(19, LOW);
	delayMicroseconds(5);
	}
	pinMode(19, INPUT);
	delayMicroseconds(10);
	Wire.begin();
	}
	void getBattVolt() {
	volt=(float)analogRead(0)*9.9/1023.0;
	if (volt<7.0) {
	errBat=true;
	errData=volt;
	}
	else errBat=false;
	}
	void setBattFact() {
	if (volt>=4.0) battFactor=6.0/volt;
	else battFactor=1.0;
	}
	void showVer() {
	ledPrint('V');
	delay(500);
	ledPrint(ver/10);
	delay(500);
	ledPrint('.');
	delay(500);
	ledPrint(ver%10);
	delay(500);
	ledClear();
	showVolt();
	}
	void showVolt() {
	getBattVolt();
	ledPrint('B');
	delay(500);
	ledPrint((byte)volt);
	delay(500);
	ledPrint('.');
	delay(500);
	ledPrint((byte)(volt*10.0)%10);
	delay(800);
	}
	void ledClear() {
	for (byte r=0; r<3; r++) {
	for (byte c=0; c<9; c++) {
	ledBuf[r][c]=0;
	}
	}
	}
	void ledPrint(byte c) {
	if (c<=9) drawBitmap(ledFont[c]);
	else if (c==' ') drawBitmap(ledFont[36]);
	else if (c=='.') drawBitmap(ledFont[37]);
	else if (c<='9') drawBitmap(ledFont[c-'0']);
	else drawBitmap(ledFont[c-'A'+10]);
	}
	void ledInit() {
	for (byte c=0; c<9 ; c++) {
	pinMode(colIO[c], OUTPUT);
	digitalWrite(colIO[c], HIGH);
	}
	for (byte r=0; r<3 ; r++) {
	pinMode(rowIO[r], OUTPUT);
	digitalWrite(rowIO[r], LOW);
	}
	for (byte r=0; r<3; r++) {
	for (byte c=0; c<9; c++) {
	ledBuf[r][c]=0;
	}
	}
	ledRow=0;
	}
	void ledHandler() {
	digitalWrite(rowIO[ledRow], LOW);
	ledRow = ++ledRow % 3;
	for (byte c=0; c<9; c++) {
	if (ledBuf[ledRow][c]) digitalWrite(colIO[c], LOW);
	else digitalWrite(colIO[c], HIGH);
	}
	digitalWrite(rowIO[ledRow], HIGH);
	}
	void drawBitmap(const byte* bitmap) {
	ledClear();
	for (byte y=0; y<5; y++) {
	byte b = bitmap[y];
	for (byte x=0; x<5; x++) {
	if(b & 0x10) drawPixel(x, y, 1);
	b <<= 1;
	}
	}
	}
	void drawPixel(byte x, byte y, byte on) { //on:0 or 1
	byte col=colIndx[y][x];
	byte row=rowIndx[y][x];
	ledBuf[row][col]=on;
	}
	void getIR() {
	if (!(irStarted && (micros()-irMicroOff>10000))) return;
	irStarted=false;
	irDecoding=true;
	if (irDataOn[0]>7000) { //NEC
	if (irOffIndex >=33) {
	decodeNECAEHA();
	printIrData("NEC");
	}
	}
	else if (irDataOn[0]>2600) { //AEHA
	if (irOffIndex >=41) {
	decodeNECAEHA();
	printIrData("AEHA");
	}
	}
	else if (irDataOn[0]>1800) { //SONY
	if (irOnIndex >=12) {
	decodeSONY();
	printIrData("SONY");
	}
	}
	if (keyCodeReady) {
	keyCodeReady=false;
	lastIrTime=millis();
	irCommand();
	}
	irDecoding=false;
	}
	void irCommand() {
	for (int i=0; i<=NUM_IR_KEY; i++) {
	if (irKeyCode==keyCode[i] && irDevCode==devCode[i]) {
	byIrRemote=true;
	irExec(i);
	break;
	}
	}
	}
	void irExec(int i) {
	switch (i) {
	case 1: //veer left
	if (!spinContinuous) spinStep=0.0;
	spinContinuous=true;
	spinStep-=0.3;
	break;
	case 2: //forward
	if (abs(movePower)>0.1 && abs(spinStep)>0.1) spinStep=0.0;
	else {
	movePower+=20.0;
	// varAng-=1.0;
	// angAdj=-movePower/2000.0;
	}
	break;
	case 3: //veer right
	if (!spinContinuous) spinStep=0.0;
	spinContinuous=true;
	spinStep+=0.3;
	break;
	case 4: //spin left
	if (spinContinuous) spinDest=spinTarget;
	spinContinuous=false;
	spinStep=-0.3;
	spinDest -= 30.0;
	break;
	case 5: //stop
	spinContinuous=false;
	varAng +=movePower/50.0;
	spinStep=0.0;
	movePower=0.0;
	spinDest = spinTarget;
	break;
	case 6: //spin right
	if (spinContinuous) spinDest=spinTarget;
	spinContinuous=false;
	spinStep=0.3;
	spinDest += 30.0;
	break;
	case 7: //Serial monitor mode
	break;
	case 8: //backward
	if (abs(movePower)>0.1 && abs(spinStep)>0.1) spinStep=0.0;
	else {
	movePower-=20.0;
	// varAng +=1.0;
	// angAdj=-movePower/2000.0;
	}
	break;
	case 9:
	break;
	case 10:
	serialMon++;
	if (serialMon==3) serialMon=0;
	Serial.println("Serial Monitor Mode=" + String(serialMon));
	break;
	}
	}
	void irInt() {
	if (millis()-lastIrTime<MIN_IR_INTERVAL) return;
	if (irDecoding) return;
	if (digitalRead(IRPIN) == IR_ON) {
	irMicroOn=micros();
	if (irStarted) {
	irDataOff[irOffIndex]=irMicroOn-irMicroOff;
	irOffIndex++;
	if (irOffIndex>=IR_BUF_LEN) irStarted=false;
	}
	else {
	irStarted=true;
	irOnIndex=0;
	irOffIndex=0;
	irMicroOff=irMicroOn;
	}
	}
	else {
	irMicroOff=micros();
	irDataOn[irOnIndex]=irMicroOff-irMicroOn;
	irOnIndex++;
	if (irOnIndex>=IR_BUF_LEN) irStarted=false;
	}
	}
	void decodeNECAEHA() {
	int16_t len=irOffIndex/8;
	int16_t idx=1;
	for (int i=0; i<len; i++) {
	irData[i]=0;
	for (int j=0; j<8; j++) {
	irData[i]>>=1;
	if (irDataOff[idx]>1000) irData[i]|=0x80;
	idx++;
	}
	}
	irDevCode=irData[0]<<8|irData[1];
	irKeyCode=irData[len-2]<<8|irData[len-1];
	keyCodeReady=true;
	}
	void decodeSONY() {
	byte data=0;
	for (int i=1; i<=7; i++) {
	if (irDataOn[i]>900) data|=0x80;
	data>>=1;
	}
	uint16_t addr=0;
	int16_t idx=8;
	for (int i=0; i<16; i++) {
	addr>>=1;
	if (idx<irOnIndex && irDataOn[idx]<1800) {
	if (irDataOn[idx]>900) addr|=0x8000;
	idx++;
	}
	}
	irDevCode=addr;
	irKeyCode=(uint16_t)data;
	keyCodeReady=true;
	}
	void printIrData(String s) {
	if (serialMon==0) return;
	if (serialMon==2) {
	for (int i=0; i<irOffIndex; i++) {
	Serial.print(irDataOn[i]);Serial.print(" ");Serial.println(irDataOff[i]);
	}
	Serial.println("");
	}
	Serial.print(s);
	Serial.print(" Dev=");Serial.print(irDevCode, HEX);
	Serial.print(" Key=");Serial.println(irKeyCode, HEX);
	}
	void setupBLE() {
	blePeripheral.setDeviceName("MICROBIT");
	blePeripheral.setLocalName("keshigomu");
	blePeripheral.setAdvertisedServiceUuid(service.uuid());
	blePeripheral.addAttribute(service);
	blePeripheral.addAttribute(chRx);
	blePeripheral.addAttribute(chTx);
	blePeripheral.begin();
	}
	void checkBLE() {
	// blePeripheral.poll();
	BLECentral central = blePeripheral.central();
	if (central) {
	if (central.connected()) {
	if (chRx.written()) {
	if (chRx.value()) {
	joyLX=chRx.value()[0];
	joyLY=chRx.value()[1];
	joyRX=chRx.value()[2];
	joyRY=chRx.value()[3];
	joySW=chRx.value()[4];
	bleCommand();
	// Serial.print(joyRX);Serial.print(" ");Serial.println(joyRY);
	}
	}
	}
	}
	}
	void bleCommand() {
	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;
	}
	}
	void sendBLE(String s) {
	char buf[20]={}; //init with 0
	byte len=s.length();
	if (len>=19) len=19;
	memcpy(buf, s.c_str(), len);
	chTx.setValue((byte*)buf, len+1); //last byte=0
	}
	void setupTimer() {
	NRF_TIMER2->TASKS_STOP = 1;
	NRF_TIMER2->TASKS_CLEAR = 1;
	NRF_TIMER2->MODE = TIMER_MODE_MODE_Timer;
	NRF_TIMER2->PRESCALER = 5; // prescaler 1/32(500kHz)
	NRF_TIMER2->BITMODE = TIMER_BITMODE_BITMODE_16Bit;
	NRF_TIMER2->CC[0] = 500; // comparator (1msec, 1kHz)
	NRF_TIMER2->INTENSET = (TIMER_INTENSET_COMPARE0_Enabled << TIMER_INTENSET_COMPARE0_Pos);
	NRF_TIMER2->SHORTS = (TIMER_SHORTS_COMPARE0_CLEAR_Enabled << TIMER_SHORTS_COMPARE0_CLEAR_Pos);
	NVIC_SetPriority(TIMER2_IRQn, 3);
	NVIC_ClearPendingIRQ(TIMER2_IRQn);
	NVIC_EnableIRQ(TIMER2_IRQn);
	NRF_TIMER2->TASKS_START=1;
	}
	void intHandler() {
	NRF_TIMER2->EVENTS_COMPARE[0]=0;
	ledHandler();
	irqCounter = ++irqCounter%50;
	if (irqCounter==0) { //every 50msec
	if (state==1) { //watch dog
	if (loopCounter==loopCounter0) {
	digitalWrite(DRV_RESET, LOW);
	errWDT=true;
	reseti2c();
	}
	else loopCounter0=loopCounter;
	}
	}
	}

view raw KS03K49.ino hosted with ❤ by GitHub