This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // | |
| // 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); | |
| } | |
| } |
