GR-ADZUKIで倒立振子

	/*GR-ADZUKI Sketch Template Version: V1.02*/
	//wheele dia ~= 50mm
	//Gear ratio ~= 114 (TAMIYA Double Gear Box)
	//Battery: Alkaline x 3
	//MPU/Driver: GR-ADZUKI
	//Gyro: MPU6050 (I2C)
	//SW1: Circle
	#include "Arduino.h"
	#include <Wire.h>
	#define LED5 13
	#define leftFwd 6
	#define leftRvs 11
	#define rightFwd 5
	#define rightRvs 10
	#define ON LOW //LOW active LED
	#define OFF HIGH
	#define SW1 2
	#define SW2 3
	#define BATT A3
	void resetPara();
	void resetVar();
	void getGyro();
	void readGyro();
	void calcTarget();
	void motor();
	void selectMode();
	void calibrate();
	bool laid();
	bool upright();
	bool standing();
	void getBaseline();
	void drive();
	void resetMotor();
	void adaptCalibAccX();
	void sendSerial();
	void drvMotorR(int pwm);
	void drvMotorL(int pwm);
	int state=-1;
	int cycleCount=0;
	unsigned long time1=0;
	unsigned long time0=0;
	float power, powerR, powerL, yawPower;
	int ipowerR, ipowerL;
	float Kang, Komg, Kdst, Kspd, Kyaw, KIang, KIdst;
	float gyroZdps, accXg;
	float varAng, varOmg, varSpd, varDst, varIang, varIdst;
	float aveDst, aveDst0;
	int accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
	float gyroZoffset, gyroYoffset, accXoffset;
	float gyroZdata, gyroYdata, accXdata, accZdata;
	float aveAccX=0.0;
	float aveAccZ=0.0;
	float moveStep, moveTarget;
	float spinStep, spinTarget;
	float orientation;
	int motorRdir=0; //0:stop 1:+ -1:-
	int motorLdir=0;
	int overSpdCount;
	float battFactor=1.3; // 2016.07.14
	float bVolt;
	float aveVolt=4.5;
	bool usbPower=false;
	bool serialMonitor=false;
	const float gyroLSB=1.0/131.0; // deg/sec
	const float accLSB=1.0/16384.0; // g
	const float clk = 0.01; // in sec,
	const unsigned long interval = (unsigned long) (clk*1000.0); // in msec
	const float filterConst = 0.1;
	const float maxSpd=250.0;
	const float battConst=6.6/1023.0;
	const int driverHLoffset=27; //bigger : backword
	const float mechFactorR=0.55;
	const float mechFactorL=0.55;
	const int backlashSpd=30;
	void setup() {
	analogWriteFrequency(25000);
	pinMode(leftFwd,OUTPUT);
	pinMode(leftRvs,OUTPUT);
	pinMode(rightFwd,OUTPUT);
	pinMode(rightRvs,OUTPUT);
	pinMode(LED5, OUTPUT);
	pinMode(SW1, INPUT_PULLUP);
	pinMode(SW2, INPUT_PULLUP);
	digitalWrite(LED5, OFF);
	Serial.begin(115200);
	Wire.begin();
	Wire.beginTransmission(0x68); //Gyro sensor
	Wire.write(0x6B); // Power management register
	Wire.write(0); // wake up MPU-6050
	Wire.endTransmission(true);
	delay(500);
	checkVoltage();
	resetPara();
	resetVar();
	selectMode();
	delay(1000);
	}
	void loop(){
	monitorVoltage();
	getGyro();
	switch (state) {
	case -1:
	calcTarget();
	if (upright()) state =0;
	break;
	case 0: //baseline
	calibrate();
	state=1;
	break;
	case 1: //run
	if (standing() && overSpdCount <= 20) {
	calcTarget();
	drive();
	}
	else {
	resetMotor();
	resetVar();
	if (upright()) state=0;
	else state=9;
	}
	break;
	case 2: //wait until upright
	if (upright()) state=0;
	break;
	case 9: //fell
	if (laid()) state=2;
	break;
	}
	cycleCount += 1;
	if (cycleCount >= 100) {
	cycleCount = 0;
	adaptCalibAccX();
	if (serialMonitor) sendSerial();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void resetPara() {
	Kang=150.0;//150
	Komg=3.0;//3.0
	KIang=1500.0;//1200
	Kyaw=15.0;//15
	Kdst=80.0;//80
	Kspd=3.5;//3.5
	KIdst=0.0;
	}
	void calcTarget() {
	spinTarget += spinStep;
	moveTarget += moveStep;
	}
	void drive() {
	varSpd += power * clk;
	varDst += Kdst * (varSpd * clk - moveTarget);
	aveDst = aveDst * 0.99 + varDst * 0.01;
	varIang += KIang * varAng * clk;
	varIdst += KIdst * varDst * clk;
	power = varIang + varIdst + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power) > 300.0) overSpdCount += 1;
	else overSpdCount =0;
	if (overSpdCount > 20) return;
	yawPower = (orientation - spinTarget) * Kyaw;
	powerR = power + yawPower;
	powerL = power - yawPower;
	motor();
	}
	void motor() {
	ipowerR = (int) (constrain(powerR * mechFactorR * battFactor, -maxSpd, maxSpd));
	if (ipowerR > 0) {
	if (motorRdir == 1) drvMotorR(ipowerR);
	else drvMotorR(ipowerR + backlashSpd);
	motorRdir = 1;
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) drvMotorR(ipowerR);
	else drvMotorR(ipowerR - backlashSpd);
	motorRdir = -1;
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	ipowerL = (int) (constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd));
	if (ipowerL > 0) {
	if (motorLdir == 1) drvMotorL(ipowerL);
	else drvMotorL(ipowerL + backlashSpd);
	motorLdir = 1;
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) drvMotorL(ipowerL);
	else drvMotorL(ipowerL - backlashSpd);
	motorLdir = -1;
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}
	}
	void getGyro() {
	readGyro();
	gyroZdps = (gyroZdata - gyroZoffset) * gyroLSB;
	varOmg = (gyroYdata - gyroYoffset) * gyroLSB; // unit:deg/sec
	accXg = (accXdata - accXoffset) * accLSB; //unit:g
	orientation += gyroZdps * clk;
	varAng += (varOmg + (accXg * 57.3 - varAng) * filterConst ) * clk;
	// varAng += varOmg * clk;
	}
	void readGyro() {
	Wire.beginTransmission(0x68);
	Wire.write(0x3B);
	Wire.endTransmission(false); //enable incremental read
	Wire.requestFrom(0x68, 14, (int)true); //used to be requestFrom(0x68, 14, true) but got warning.
	accX=Wire.read()<<8|Wire.read(); //0x3B
	accY=Wire.read()<<8|Wire.read(); //0x3D
	accZ=Wire.read()<<8|Wire.read(); //0x3F
	temp=Wire.read()<<8|Wire.read(); //0x41
	gyroX=Wire.read()<<8|Wire.read(); //0x43
	gyroY=Wire.read()<<8|Wire.read(); //0x45
	gyroZ=Wire.read()<<8|Wire.read(); //0x47
	gyroZdata = -(float) gyroX;
	gyroYdata = (float) gyroY;
	accXdata = -(float) accZ;
	aveAccX = aveAccX * 0.9 + accXdata * 0.1;
	accZdata = -(float) accX;
	aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
	}
	bool laid() {
	if (abs(aveAccX) >13000.0) return true;
	else return false;
	}
	bool upright() {
	if (abs(aveAccZ) >13000.0) return true;
	else return false;
	}
	bool standing() {
	if (abs(aveAccZ) > 10000.0 && abs(varAng) < 40.0) return true;
	else return false;
	}
	void calibrate() {
	resetVar();
	resetMotor();
	delay(2000);
	blinkLED(2);
	delay(500);
	getBaseline();
	delay(500);
	blinkLED(2);
	}
	void getBaseline() {
	gyroYoffset=gyroZoffset=0.0;
	accXoffset=0.0;
	for (int i=1; i <= 30; i++){
	readGyro();
	gyroYoffset += gyroYdata;
	gyroZoffset += gyroZdata;
	accXoffset += accXdata;
	delay(10);
	}
	gyroZoffset /= 30.0;
	gyroYoffset /= 30.0;
	accXoffset /= 30.0;
	}
	void adaptCalibAccX() {
	if (aveDst > 100.0 && aveDst > aveDst0) accXoffset -= 20.0;
	if (aveDst < -100.0 && aveDst < aveDst0) accXoffset += 20.0;
	aveDst0=aveDst;
	}
	void resetVar() {
	power=0.0;
	moveTarget=0.0;
	spinTarget=0.0;
	orientation=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	varIdst=0.0;
	aveDst=0.0;
	aveDst0=0.0;
	}
	void resetMotor() {
	drvMotorR(0);
	drvMotorL(0);
	overSpdCount=0;
	}
	void checkVoltage() {
	usbPower=false;
	bVolt = ((float) analogRead(BATT)) * battConst;
	if (serialMonitor) Serial.println(bVolt);
	if (bVolt > 4.4) blinkLED(3);
	else if (bVolt > 4.1) blinkLED(2);
	else if (bVolt > 3.8) blinkLED(1);
	else if (bVolt > 0.5) blinkLED(200);
	else usbPower=true;
	}
	void blinkLED(int n) {
	for (int i=0; i<n; i++) {
	digitalWrite(LED5,ON);
	delay(10);
	digitalWrite(LED5,OFF);
	delay(200);
	}
	}
	void monitorVoltage() {
	digitalWrite(LED5, OFF);
	bVolt = ((float) analogRead(BATT)) * battConst;
	if (bVolt<3.4) {
	digitalWrite(LED5, ON);
	if (serialMonitor) {
	Serial.print("Batt=");
	Serial.println(bVolt);
	}
	}
	aveVolt = aveVolt * 0.99 + bVolt * 0.01;
	if (usbPower) battFactor = 1.3;
	else battFactor = 5.0 / aveVolt;
	}
	void drvMotorR(int pwm) {
	if (pwm >0) {
	digitalWrite(rightRvs, HIGH);
	analogWrite(rightFwd, constrain(256-pwm, -255, 255));
	}
	else if (pwm < 0) {
	digitalWrite(rightRvs, LOW);
	analogWrite(rightFwd, constrain(-pwm+driverHLoffset, -255, 255));
	}
	else {
	digitalWrite(rightRvs, LOW);
	analogWrite(rightFwd, constrain(pwm, -255, 255));
	}
	}
	void drvMotorL(int pwm) {
	if (pwm >0) {
	digitalWrite(leftRvs, HIGH);
	analogWrite(leftFwd, constrain(256-pwm, -255, 255));
	}
	else if (pwm < 0) {
	digitalWrite(leftRvs, LOW);
	analogWrite(leftFwd, constrain(-pwm+driverHLoffset, -255, 255));
	}
	else {
	digitalWrite(leftRvs, LOW);
	analogWrite(leftFwd, constrain(pwm, -255, 255));
	}
	}
	void selectMode() {
	if (digitalRead(SW1) == LOW) { //circle
	moveStep=0.003;
	spinStep=0.2;
	blinkLED(1);
	}
	}
	void sendSerial () {
	Serial.print(millis()-time0);
	Serial.print(" state=");Serial.print(state);
	Serial.print(" accX="); Serial.print(aveAccX);
	Serial.print(" Xoffset=");Serial.print(accXoffset);
	Serial.print(" accZ="); Serial.print(aveAccZ);
	Serial.print(" ang=");Serial.print(varAng);
	// Serial.print(" Omg="); Serial.print(varOmg);
	// Serial.print(" temp="); Serial.print(temp/340.0+36.53);
	Serial.print(" Dst="); Serial.print(varDst);
	Serial.print(" aveDst=");Serial.print(aveDst);
	Serial.print(" power="); Serial.print(power);
	// Serial.print(" ori="); Serial.print(orientation);
	Serial.print(" batt="); Serial.print(aveVolt);
	Serial.print(" bFactor=");Serial.print(battFactor);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println();
	}

view raw adzuki_robo_4.ino hosted with ❤ by GitHub