2020年8月10日月曜日

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

投稿者 時刻:
ラベル: ,

0 件のコメント:

コメントを投稿

登録: コメントの投稿 (Atom)