设为首页收藏本站
首页 > 论坛 > Arduino

[项目]来自日本的脑洞——M5StickC平衡机器人

vany5921 | 中级技师 | 创造力： | 帖子： | 发消息 | 串个门 | 加好友 | 打招呼
2019-9-12 13:40:32 [显示全部楼层]
4673浏览
3回复
0收藏
查看: 4673|回复: 3

[项目] 来自日本的脑洞——M5StickC平衡机器人

[复制链接]
来自日本的脑洞——M5StickC平衡机器人图4
M5StickC内置了SH200Q/MPU6886，也就是加速度计和陀螺仪 ，添加驱动板和小电机就可以完成平衡机器人，今天就分享一个日本maker做的一个有趣的项目，直接利用电机轴来进行平衡。
电路原理图比较简单，使用DRV8830的I2C接口与M5StickC的G0和G26连接，这里接入了电阻进行上拉，防止电路干扰。
来自日本的脑洞——M5StickC平衡机器人图1
根据电路使用洞洞板连接，同时电机也要固定到洞洞板两个角上，电机倾斜45度。
来自日本的脑洞——M5StickC平衡机器人图2
电机使用的型号是RM0TV0009AWZZ ，电机轴套上0.8毫米的热缩管。
来自日本的脑洞——M5StickC平衡机器人图3
为了让机器人的平衡性更好可以适当贴上双面胶调整配重
代码部分，由于M5StickC有MPU6886和SH200Q两个版本的传感器芯片，因此有两个版本的代码
[mw_shl_code=c,true]//SH200Q
// M5StickC Balancing Robot
// by Kiraku Labo, 2019
//
// 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>

//#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;

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);
  #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();
}[/mw_shl_code]

[mw_shl_code=c,true]//MPU6886
// 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();
}[/mw_shl_code]



回复

使用道具 举报

精彩评论(3)

gada888  版主

发表于 2019-9-13 09:11:54

有趣
回复

使用道具 举报

诩  初级技匠

发表于 2022-4-21 08:01:53

整理下代码啊哥，太长了
回复

使用道具 举报

诩  初级技匠

发表于 2022-4-21 08:35:58

不过腿那么细还能站直的机器人
一定是出自高手
回复

使用道具 举报

高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
撰写新帖
关于楼主

楼主的其它帖子

background 创造力兑换 background 心愿单 background 创客导航
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail