M5StickCとサーボでバランスロボ

	// 2022 Kiraku Labo
	#include <M5StickC.h>
	#define SERVO_PIN_R 0
	#define SERVO_PIN_L 26
	int16_t pulseZeroR, pulseZeroL;
	boolean serialMonitor=true;
	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, gyroYoffset, accXoffset, gyroZoffset;
	const uint32_t interval = 10; // in msec
	const float dt=(float)interval/1000.0; // in sec
	float movePower=0.0, movePwrTarget, movePwrStep=1.0;
	float mechFactorR, mechFactorL;
	float spinStep, spinDest, spinTarget, spinFact=1.0;
	void setup() {
	M5.begin();
	M5.Axp.ScreenBreath(11);
	M5.Lcd.setRotation(3);
	M5.Lcd.setTextFont(1);
	M5.Lcd.fillScreen(BLACK);
	M5.IMU.Init();
	pinMode(SERVO_PIN_L, OUTPUT);
	pinMode(SERVO_PIN_R, OUTPUT);
	resetMotor();
	resetPara();
	resetVar();
	delay(1000);
	getBaselineGyro();
	}
	void loop() {
	static uint32_t msec=0;
	getIMU();
	switch (state) {
	case 0:
	if (abs(accXdata)<0.25 && aveAbsOmg<1.0) {
	M5.Lcd.setCursor(10, 20);
	M5.Lcd.println("Calibrating..");
	getBaseline();
	msec=millis();
	resetVar();
	state=1;
	}
	break;
	case 1:
	if (millis()-msec>200) {
	M5.Lcd.fillScreen(BLACK);
	state=2;
	}
	break;
	case 2:
	if (abs(varAng)>30.0 || counterOverSpd>20) {
	if (serialMonitor) sendStatus();
	resetMotor();
	resetVar();
	state=0;
	}
	else {
	calcTarget();
	drive();
	}
	break;
	}
	counter += 1;
	if (counter >= 100) {
	counter = 0;
	if (serialMonitor) sendStatus();
	M5.Lcd.setCursor(10, 10);
	float vBatt= M5.Axp.GetVbatData() * 1.1 / 1000;
	M5.Lcd.printf("Bat=%4.2fv ", vBatt);
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void getIMU() {
	readIMU();
	varOmg = gyroYdata - gyroYoffset;
	aveAbsOmg = aveAbsOmg * 0.8 + abs(varOmg) * 0.2;
	yawAng += (gyroZdata - gyroZoffset) * dt;
	varAng = (varAng + varOmg * dt) * 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=-gY;
	gyroZdata=-gZ;
	accXdata=aX;
	}
	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 * dt;
	varDst += Kdst * varSpd * dt;
	varIang += KIang * varAng * dt;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power)>1000.0) counterOverSpd += 1;
	else counterOverSpd=0;
	power = constrain(power, -maxPwr, maxPwr);
	yawPower = (yawAng - spinTarget) * Kyaw;
	powerL = power - yawPower + movePwrTarget;
	powerR = power + yawPower + movePwrTarget;
	int16_t ipowerL = (int16_t) constrain(powerL * mechFactorL, -maxPwr, maxPwr);
	int16_t ipowerR = (int16_t) constrain(powerR * mechFactorR, -maxPwr, maxPwr);
	drvMotor(ipowerL, ipowerR);
	}
	void drvMotor(int pwmL, int pwmR) {
	int pulseR=constrain(pulseZeroR-pwmR, 1000, 2000);
	int pulseL=constrain(pulseZeroL+pwmL, 1000, 2000);
	bool doneR=false;
	bool doneL=false;
	uint32_t usec=micros();
	digitalWrite(SERVO_PIN_R, HIGH);
	digitalWrite(SERVO_PIN_L, HIGH);
	while(!doneR || !doneL) {
	uint32_t width=micros()-usec;
	if (width>=pulseL) {
	digitalWrite(SERVO_PIN_L, LOW);
	doneL=true;
	}
	if (width>=pulseR) {
	digitalWrite(SERVO_PIN_R, LOW);
	doneR=true;
	}
	}
	}
	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 resetPara() {
	Kang=35.0;
	Komg=0.8;
	KIang=700.0;
	Kyaw=4.0;
	Kdst=20.0;
	Kspd=2.5;
	mechFactorR=0.5;
	mechFactorL=0.5;
	maxPwr=500.0;
	pulseZeroL=1460; //inc=forward
	pulseZeroR=1490; //dec=forward
	}
	void resetMotor() {
	digitalWrite(SERVO_PIN_L, LOW);
	digitalWrite(SERVO_PIN_R, LOW);
	counterOverSpd=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 getBaselineGyro() {
	gyroYoffset=0.0;
	for (int i=0; i < 30; i++){
	readIMU();
	gyroYoffset += gyroYdata;
	delay(10);
	}
	gyroYoffset /= 30.0;
	}
	void sendStatus () {
	Serial.print("state="); Serial.print(state);
	Serial.print(" accX="); Serial.print(accXdata);
	Serial.print(" ang=");Serial.print(varAng);
	Serial.print(" omg="); Serial.print(varOmg);
	Serial.print(" pwr="); Serial.print(power);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println("ms");
	}

view raw M5BalServoV048.ino hosted with ❤ by GitHub