2019年8月12日月曜日

M5StickC バランスロボ（MPU6886対応）

This version supports MPU6886 with M5StickC library 0.1.0.
Hardware page is here.

//
// M5StickC Balancing Robot
// by Kiraku Labo, 2019
//
// Supports MPU6886
// Requires M5StickC library 0.1.0 (or above hopefully)
//
// 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>
//Comment out the follwoing two lines for SH200Q
#include "utility/MPU6886.h"
#define IMU MPU6886
//#define SHORT //height of the robot: short ~= 90mm, tall ~= 100mm
#define INVERTED //M5StickC upside down
#define POLARITY //Motor direction reversed
#define BTN_A 37
#define BTN_B 39
#define MOTOR_R 0x65 //A0=low, A1=open
#define MOTOR_L 0x63 //A0=high, A1=open
boolean serialMonitor=false;
int counter=0;
uint32_t time0=0, time1=0;
int16_t counterOverSpd=0, maxOvs=20;
float aveAccX=0.0, aveAccZ=0.0;
float power, powerR, powerL, yawPower;
float varAng, varOmg, varSpd, varDst, varIang;
float gyroYoffset, gyroZoffset, accXoffset, accZoffset;
float gyroYdata, gyroZdata, accXdata, accZdata;
int16_t accX, accY, accZ, tmp, gyroX, gyroY, gyroZ;
float cutoff=0.1; //~=2 * pi * f (Hz) assuming clk is negligible compared to 1/w
const float clk = 0.01; // in sec,
const uint32_t interval = (uint32_t) (clk*1000); // in msec
boolean calibrated=false;
int8_t state=-1;
float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
int16_t maxSpd;
float orientation=0.0;
float moveDestination, moveTarget;
float moveRate=0.0;
float moveStep = 0.2 * clk;
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=false;
float spinDestination, spinTarget, spinRate;
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;
boolean mpu6886;
void setup() {
pinMode(BTN_A, INPUT);
pinMode(BTN_B, INPUT);
M5.begin();
M5.IMU.Init();
Wire.begin(0, 26); //SDA,SCL
Wire.setClock(50000);
Serial.begin(115200);
M5.Axp.ScreenBreath(11);
#ifdef INVERTED
M5.Lcd.setRotation(2);
#else
M5.Lcd.setRotation(0);
#endif
M5.Lcd.setTextFont(4);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(0, 0);
M5.Lcd.setCursor(0,70);
M5.Lcd.println(" G.Cal");
resetMotor();
resetPara();
resetVar();
delay(1000);
getBaselineAccZ();
getBaselineGyro();
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setCursor(10,70);
vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
M5.Lcd.printf("%4.2fv ", vBatt);
}
void loop() {
checkButton();
getGyro();
switch (state) {
case -1:
M5.Lcd.setCursor(10,70);
vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
M5.Lcd.printf("%4.2fv ", vBatt);
M5.Lcd.setCursor(10,130);
M5.Lcd.printf("%4d ", -(int16_t)(aveAccZ-accZoffset));
if (upright()) state=0;
break;
case 0: //baseline
calibrate();
state=1;
break;
case 1: //run
if (!calibrated) state=-1;
else if (standing() && counterOverSpd <= maxOvs) {
calcTarget();
drive();
}
else {
drvMotorR(0);
drvMotorL(0);
counterOverSpd=0;
resetVar();
state=9;
}
break;
case 9: //fell
if (laid()) {
M5.Lcd.fillScreen(BLACK);
state=-1;
}
break;
}
counter += 1;
if (counter >= 100) {
counter = 0;
vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
M5.Lcd.setCursor(10,70);
M5.Lcd.printf("%4.2fv ", vBatt);
sendStatus();
}
#ifdef DEVELOP
devLoop();
#endif
do time1 = millis();
while (time1 - time0 < interval);
time0 = time1;
}
void calibrate() {
resetVar();
drvMotorL(0);
drvMotorR(0);
counterOverSpd=0;
delay(1000);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setCursor(0,70);
M5.Lcd.println(" A.Cal ");
delay(1000);
getBaselineAccX();
calibrated=true;
M5.Lcd.setCursor(0,70);
M5.Lcd.println(" ");
}
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;
moveRate=0.0;
spinContinuous=false;
spinStep=30*clk;
M5.Lcd.print("Stand ");
}
else {
demoMode=1;
moveRate=-1.0;
spinContinuous=true;
spinStep=40*clk;
M5.Lcd.print("Demo ");
}
while (digitalRead(BTN_A) == LOW);
}
void btnAshort() {
M5.Lcd.setCursor(0, 70);
M5.Lcd.print(" G.Cal ");
delay(1000);
M5.IMU.Init();
getBaselineAccZ();
getBaselineGyro();
M5.Lcd.setCursor(0, 70);
M5.Lcd.print(" ");
}
void resetPara() {
#ifdef SHORT
Kang=35.0;
Komg=0.8;
KIang=500.0;
Kyaw=5.0;
Kdst=8.0;
Kspd=2.5;
mechFact=0.25;
mechLRbal=1.0;
punchSpd=25;
punchDur=2;
motorDeadband=20;
motorDeadbandNeg=-motorDeadband;
maxSpd=250; //limit slip
drvOffset=0;
#else
Kang=35.0;
Komg=0.8;
KIang=500.0;
Kyaw=5.0;
Kdst=8.0;
Kspd=2.5;
mechFact=0.37;
mechLRbal=1.0;
punchSpd=0;
punchDur=0;
motorDeadband=25;
motorDeadbandNeg=-motorDeadband;
maxSpd=250; //limit slip
drvOffset=0;
#endif
float balL=2.0*mechLRbal/(1.0+mechLRbal);
mechFactorL=mechFact*balL;
mechFactorR=mechFactorL/mechLRbal;
punchSpd2=max(punchSpd, motorDeadband);
punchSpd2N=-punchSpd2;
}
void getGyro() {
readGyro();
gyroZdps = (gyroZdata - gyroZoffset) * M5.IMU.gRes;
varOmg = (gyroYdata - gyroYoffset) * M5.IMU.gRes; // unit:deg/sec
accXg = (accXdata - accXoffset) * M5.IMU.aRes; //unit:g
orientation += gyroZdps * clk;
varAng += (varOmg + (accXg * 57.3 - varAng) * cutoff ) * clk; //assuming clk*clk is negligible
}
void readGyro() {
// M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
// M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelAdc(&accX,&accY,&accZ);
#ifdef INVERTED
gyroYdata=(float)gyroX;
gyroZdata=-(float)gyroY;
accXdata=(float)accZ;
accZdata=(float)accY;
#else
gyroYdata=-(float)gyroX;
gyroZdata=(float)gyroY;
accXdata=(float)accZ;
accZdata=-(float)accY;
#endif
aveAccX = aveAccX * 0.9 + accXdata * 0.1;
aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
}
bool laid() {
if (abs(aveAccX) > 3000.0) return true;
else return false;
}
bool upright() {
if (abs(aveAccZ-accZoffset) > 3000.0) return true;
else return false;
}
bool standing() {
if (abs(aveAccZ-accZoffset) > 3000.0 && abs(varAng) < 40.0) return true;
else return false;
}
void calcTarget() {
if (spinContinuous) spinTarget += spinStep;
else {
if (spinTarget < spinDestination) spinTarget += spinStep;
if (spinTarget > spinDestination) spinTarget -= spinStep;
}
moveTarget += moveStep * moveRate;
}
void drive() {
varSpd += power * clk;
varDst += Kdst * (varSpd * clk - moveTarget);
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 = (orientation - spinTarget) * Kyaw;
powerR = power + yawPower;
powerL = power - yawPower;
ipowerL = (int16_t) constrain(powerL * mechFactorL, -maxSpd, maxSpd);
if (ipowerL > 0) {
if (motorLdir == 1) 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 = 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, -maxSpd, maxSpd);
if (ipowerR > 0) {
if (motorRdir == 1) 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 = 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;
}
}
void drvMotorL(int16_t pwr) {
#ifdef POLARITY
drvMotor(MOTOR_L, pwr);
#else
drvMotor(MOTOR_L, -pwr);
#endif
}
void drvMotorR(int16_t pwr) {
#ifdef POLARITY
drvMotor(MOTOR_R, pwr);
#else
drvMotor(MOTOR_R, -pwr);
#endif
}
void drvMotor(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() {
drvMotor(MOTOR_R, 0);
drvMotor(MOTOR_L, 0);
}
void resetVar() {
power=0.0;
moveTarget=0.0;
spinDestination=0.0;
spinTarget=0.0;
spinRate=0.0;
orientation=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(" ang=");Serial.print(varAng);
Serial.print(" Pdur="); Serial.print(punchDur);
Serial.print(" gyroY="); Serial.print(gyroYdata);
Serial.print(" MFL="); Serial.print(mechFactorL);
Serial.print(" MFR="); Serial.print(mechFactorR);
Serial.print(", ");
Serial.print(millis()-time0);
Serial.println();
}
view raw DRV8830M5V016.ino hosted with ❤ by GitHub
投稿者 時刻:

0 件のコメント:

コメントを投稿

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