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