M5Stack BugC self-balance bot

balance_bug.ino

#include <M5StickC.h>
#include "bugC.h"

#define sampleTime  0.005

float pitch  = 0.0F;
float roll  = 0.0F;
float yaw  = 0.0F;
float roll_offset = 0.0F;

float P = 8.0F;
float I = 10.0F;
float D = 0.05F;

float errorSum = 0.0F;
float lastAngle = 0.0F;

void setup() {
  // put your setup code here, to run once:
  M5.begin();
  Wire.begin(0, 26, 400000);
  M5.IMU.Init();
  M5.Lcd.setRotation(2);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(3);
}


void loop() {
  // put your main code here, to run repeatedly:

  M5.update();

  M5.IMU.getAhrsData(&pitch,&roll,&yaw);

  if(M5.BtnA.wasPressed()){
      roll_offset = roll;
      errorSum = 0;
  }
  float angle = roll-roll_offset;
  errorSum = errorSum + angle;  
  errorSum = constrain(errorSum, -300, 300);
  
  float motorSpeed = P*angle+I*errorSum*sampleTime-D*(angle-lastAngle)/sampleTime;
  BugCSetAllSpeed(-motorSpeed, motorSpeed, 0, 0);
  lastAngle = angle;
  
  
  
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.printf("%3.0f ", roll-roll_offset);

  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf("%3.0f ", motorSpeed);
  delay(sampleTime*1000);
}