木楽らぼソースコード: GR-KURUMIと電動消しゴムでバランスロボ

	//
	//Keshigomu Robo
	//by Kiraku Labo, 2019
	//
	//wheele dia ~= 4mm
	//Battery: LiPo 3.7V
	//MPU: GR-KURUMI or ProMini 3.3V
	//Gyro: MPU6050
	//Motor driver: DRV8835 connected to Arduino pin 3,9,10,11

	#include <Wire.h>

	#define KURUMI
	#define IR_MARGIN 250

	//TV remote (Sony format, first 12 bit only, bit order not observed)
	#define BTN_UP 0x810
	#define BTN_DOWN 0xE10
	#define BTN_LEFT 0xC10
	#define BTN_RIGHT 0xA10
	#define BTN_CENTER 0x210
	#define BTN_FUNC 0x5D0 //Screen key

	#ifdef KURUMI
	#define LED 23
	#define LED_R 22
	#define LED_B 24
	#define LED_ON LOW
	#define LED_OFF HIGH
	#else
	#define LED 13
	#define LED_ON HIGH
	#define LDE_OFF LOW
	#endif

	#define IR_ON LOW
	#define IR_OFF HIGH
	#define IRT1 600
	#define IRT2 1200
	#define IRT3 1800
	#define IRT4 2400
	#define OSP_PWR_TH 250.0 //power
	#define OSP_LMT_TH 6 //count
	#define OSP_STP_TH 4 //count

	bool serialMonitor=true;
	bool spinContinuous=true;
	bool moveStepUp=true;
	int16_t accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
	int16_t state=-1;
	int16_t counter=0;
	int16_t ipowerR=0, ipowerL=0;
	int16_t motorRdir=0, motorLdir=0; //stop 1:+ -1:-
	int16_t counterOverSpd, counterTrip;

	const float gyroLSB=4.0/131.0; // unit:deg/sec (full scalse 8.0:2000, 4.0:1000, 2.0:500, 1.0:250 deg/sec)
	const float accLSB=1.0/16384.0; // unit:g
	const float clk=0.01; // in sec,
	const float cutoff=0.1; // 2 * PI * f (f=Hz)
	float power, powerR, powerL;
	float movePower=0.0, moveTarget, moveStep=1.0;
	float varAng, varOmg, varSpd, varDst, varIang, aveAbsOmg=0.0;
	float gyroZoffset, gyroYoffset, accXoffset;
	float gyroZdata, gyroYdata, accXdata, accZdata, aveAccZ=0.0;
	float spinDest, spinTarget, spinFact=1.0;
	float spinStep=0.0; //degree per 10msec
	float orientation, yawPower;
	float battFactor=1.0;
	uint32_t time0=0, time1=0, irRcvTime=0;
	const uint32_t interval = (unsigned long) (clk*1000.0); // in msec

	float Kang=40.0;
	float Komg=0.8;
	float KIang=600.0;
	float Kyaw=3.0;
	float Kdst=35.0;
	float Kspd=8.0;
	float mechFactorR=1.0;
	float mechFactorL=1.0;
	float maxPwr=200.0;
	int16_t punchPwr=30;
	int16_t motorDeadBand=10;

	volatile byte irBit;
	volatile byte irDataArray[14];
	volatile uint32_t irDur[14];
	volatile uint32_t irOnTime=0, irOffTime=0;
	volatile bool tvRemote, irDataReady=false;

	void setup() {
	attachInterrupt(0, irInt, CHANGE);
	pinMode(5,OUTPUT); //MODE (HIGH:phase/en LOW:in/in)
	digitalWrite(5, HIGH);
	#ifdef KURUMI
	analogWriteFrequency(31000);
	#else
	TCCR1B = TCCR1B & B11111000 \| B00000001; //pwm freq=31khz (D9 and D10)
	#endif
	pinMode(2,INPUT); //IR
	pinMode(3,OUTPUT); //A phase (Right)
	pinMode(9,OUTPUT); //A enable
	pinMode(11,OUTPUT); //B phase (Left)
	pinMode(10,OUTPUT); //B enable
	pinMode(LED, OUTPUT); //LED
	digitalWrite(LED, LED_OFF);
	Wire.begin();
	#ifdef KURUMI
	pinMode(LED_R, OUTPUT);
	digitalWrite(LED_R, LED_OFF);
	pinMode(LED_B, OUTPUT);
	digitalWrite(LED_B, LED_OFF);
	#else
	Wire.setClock(50000L);
	#endif
	Wire.beginTransmission(0x68); //MPU6050
	Wire.write(0x6B); // Power management register
	Wire.write(0x00); // wake up MPU6050
	Wire.endTransmission();
	Wire.beginTransmission(0x68);
	Wire.write(0x1B); //gyro full scale retister
	Wire.write(0x10); //0x00:250, 0x08:500, 0x10:1000, 0x18:2000 deg/sec
	Wire.endTransmission();
	delay(2000); //stabilize MPU6050
	Serial.begin(115200);
	resetVar();
	getBaseline1();
	}

	void loop() {
	getIR();
	getGyro();
	if (state==-1) {
	if (abs(accXdata)<0.2 && aveAbsOmg<1.0) state =0;
	}
	else if (state==0) { //baseline
	#ifdef KURUMI
	digitalWrite(LED_R, LED_OFF);
	digitalWrite(LED_B, LED_OFF);
	#endif
	calibrate();
	state=1;
	}
	else if (state==1) { //run
	if (abs(varAng)>30.0) {
	#ifdef KURUMI
	digitalWrite(LED_R, LED_ON);
	#endif
	fell();
	}
	else if (abs(aveAccZ)<0.3) {
	#ifdef KURUMI
	digitalWrite(LED_B, LED_ON);
	#endif
	fell();
	}
	else if (counterTrip>OSP_STP_TH) {
	#ifdef KURUMI
	digitalWrite(LED_R, LED_ON);
	digitalWrite(LED_B, LED_ON);
	#endif
	fell();
	}
	else {
	calcTarget();
	drive();
	}
	}
	else if (state== 9) { //fell
	if (abs(aveAccZ)<0.3) { //laid
	state=-1;
	}
	}
	counter = ++counter % 100;
	if (counter==0 && serialMonitor) sendSerial();
	if (!tvRemote && millis()-irRcvTime>100) {
	movePower=0.0;
	spinStep=0.0;
	}
	do {
	time1 = millis();
	} while (time1 - time0 < interval);
	time0 = time1;
	}

	void fell() {
	sendSerial();
	resetMotor();
	resetVar();
	state=-1;
	}

	void calcTarget() {
	if (abs(movePower)>1.0) spinFact=constrain(-(powerR+powerL)/10.0, -1.0, 1.0); //moving
	else spinFact=1.0; //standing
	if (spinContinuous) spinTarget += spinStep * spinFact;
	else {
	if (spinTarget < spinDest-spinStep) spinTarget += spinStep;
	if (spinTarget > spinDest+spinStep) spinTarget -= spinStep;
	}
	if (moveStepUp) {
	if (moveTarget < movePower-moveStep) moveTarget += moveStep;
	if (moveTarget > movePower+moveStep) moveTarget -= moveStep;
	}
	else moveTarget = movePower;
	}

	void drive() {
	varSpd += power * clk;
	varDst += Kdst * varSpd * clk;
	varIang += KIang * varAng * clk;
	power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power) > OSP_PWR_TH) counterOverSpd += 1;
	else {
	counterOverSpd =0;
	counterTrip=0;
	}
	if (counterOverSpd > OSP_LMT_TH) {
	counterTrip += 1;
	counterOverSpd=0;
	power=0.0; //limit slip
	}
	power=constrain(power, -maxPwr, maxPwr);
	yawPower = (orientation - spinTarget) * Kyaw;
	powerR = power + yawPower + moveTarget;
	powerL = power - yawPower + moveTarget;
	motor();
	}

	void motor() {
	ipowerR = (int) (powerR * mechFactorR * battFactor);
	if (ipowerR > 0) {
	if (motorRdir == 1) drvMotorR(max(ipowerR, motorDeadBand));
	else drvMotorR(max(ipowerR, motorDeadBand) + punchPwr);
	motorRdir = 1;
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) drvMotorR(min(ipowerR, motorDeadBand));
	else drvMotorR(min(ipowerR, motorDeadBand) - punchPwr);
	motorRdir = -1;
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	ipowerL = (int) (powerL * mechFactorL * battFactor);
	if (ipowerL > 0) {
	if (motorLdir == 1) drvMotorL(max(ipowerL, motorDeadBand));
	else drvMotorL(max(ipowerL, motorDeadBand) + punchPwr);
	motorLdir = 1;
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) drvMotorL(min(ipowerL, motorDeadBand));
	else drvMotorL(min(ipowerL, motorDeadBand) - punchPwr);
	motorLdir = -1;
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}
	}

	void drvMotorR(int pwm) {
	pwm=constrain(pwm, -255, 255);
	if (pwm>=0) {
	digitalWrite(3,LOW);
	analogWrite(9,pwm);
	}
	else {
	digitalWrite(3,HIGH);
	analogWrite(9,-pwm);
	}
	}

	void drvMotorL(int pwm) {
	pwm=constrain(pwm, -255, 255);
	if (pwm>=0) {
	digitalWrite(11,LOW);
	analogWrite(10,pwm);
	}
	else {
	digitalWrite(11,HIGH);
	analogWrite(10,-pwm);
	}
	}

	void resetMotor() {
	drvMotorR(0);
	drvMotorL(0);
	counterOverSpd=0;
	counterTrip=0;
	}

	void getGyro() {
	readGyro();
	varOmg = (gyroYdata - gyroYoffset);
	if (state!=1) aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
	orientation += (gyroZdata - gyroZoffset) * clk;
	varAng += (varOmg + ((accXdata - accXoffset) * 57.3 - varAng) * cutoff ) * clk;
	}

	void readGyro() {
	Wire.beginTransmission(0x68);
	Wire.write(0x3B);
	Wire.endTransmission();
	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
	// temperature = (float)temp/340.0+36.53;
	gyroZdata = -((float)gyroY) * gyroLSB;
	gyroYdata = -(float)gyroZ * gyroLSB;
	accXdata = -(float)accX * accLSB;
	accZdata = (float)accY * accLSB;
	aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
	}

	void calibrate() {
	resetVar();
	resetMotor();
	digitalWrite(LED, LED_ON);
	delay(500);
	if (serialMonitor) sendSerial();
	getBaseline2();
	digitalWrite(LED, LED_OFF);
	}

	void getBaseline1() {
	digitalWrite(LED, LED_ON);
	gyroZoffset=gyroYoffset=0.0;
	for (int i=1; i <= 30; i++) {
	readGyro();
	gyroYoffset += gyroYdata;
	gyroZoffset += gyroZdata;
	delay(20);
	}
	gyroYoffset /= 30.0;
	gyroZoffset /= 30.0;
	digitalWrite(LED, LED_OFF);
	}

	void getBaseline2() {
	gyroZoffset=accXoffset=0.0;
	for (int i=1; i <= 30; i++) {
	readGyro();
	accXoffset += accXdata;
	gyroZoffset += gyroZdata;
	delay(20);
	}
	accXoffset /= 30.0;
	gyroZoffset /= 30.0;
	}

	void resetVar() {
	power=0.0;
	moveTarget=0.0;
	movePower=0.0;
	spinDest=0.0;
	spinTarget=0.0;
	spinStep=0.0;
	orientation=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	}

	void irInt() {
	uint32_t usec;
	if (irDataReady) return; //code may be longer than 12 bit
	if (digitalRead(2) == IR_ON) {
	irOnTime=micros();
	usec=irOnTime-irOffTime; //Off duration
	if ((usec<IRT1-IR_MARGIN) \|\| (usec>IRT2+IR_MARGIN)) irBit=0;
	else if (tvRemote) {
	if (usec<IRT1+IR_MARGIN) ; //Sony Tv
	else irBit=0;
	}
	else {
	if (irBit==1 && (usec>IRT2-IR_MARGIN)) ; //Joystick
	else if (irBit>=2 && (usec<IRT2+IR_MARGIN)) ; //Joystick
	else irBit=0;
	}
	}
	else {
	irOffTime=micros();
	usec=irOffTime-irOnTime; //On duration
	if ((irBit>0) && (usec>=IRT1-IR_MARGIN) && (usec<=IRT1+IR_MARGIN)) {
	// irDur[irBit]=usec;
	irDataArray[irBit]=0;
	irBit++;
	}
	else if ((irBit>0) && (usec>=IRT2-IR_MARGIN) && (usec<=IRT2+IR_MARGIN)) {
	// irDur[irBit]=usec;
	irDataArray[irBit]=1;
	irBit++;
	}
	else if ((usec>=IRT4-IR_MARGIN) && (usec<=IRT4+IR_MARGIN)) { //tvRemote
	tvRemote=true;
	irBit=1;
	}
	else {
	tvRemote=false;
	irBit=0;
	}
	if (irBit>=13) { //12 bit received
	irDataReady=true;
	}
	}
	}

	void getIR() {
	if (irDataReady) {
	irDataReady=false;
	if (tvRemote && millis()-irRcvTime<300) return;
	irRcvTime=millis();
	if (tvRemote) decodeRemote();
	}
	}

	void decodeRemote() {
	int16_t irData=0;
	digitalWrite(LED_R, LED_ON);
	for (int i=1; i<=12; i++) {
	irData *= 2;
	irData += irDataArray[i];
	// Serial.print(irDur[i]);Serial.print(" ");
	}
	// Serial.println();
	if (serialMonitor) Serial.println(irData, HEX);
	if (irData==BTN_FUNC) { //circle demo
	spinStep=0.4;
	movePower=-20.0;
	}
	else if (irData==BTN_CENTER) { //btn5 stop
	spinStep=0.0;
	movePower=0.0;
	}
	else if (irData==BTN_LEFT) { //btn4 left
	spinStep -=0.2;
	}
	else if (irData==BTN_RIGHT) { //btn6 right
	spinStep +=0.2;
	}
	else if (irData==BTN_UP) { //btn2 up
	if (abs(spinStep)<0.01) movePower -=20,0;
	spinStep=0.0;
	}
	else if (irData==BTN_DOWN) { //btn8 down
	if (abs(spinStep)<0.01) movePower +=20.0;
	spinStep=0.0;
	}
	digitalWrite(LED_R, LED_OFF);
	}

	void sendSerial () {
	Serial.print(millis()-time0);
	Serial.print(" state=");Serial.print(state);
	// Serial.print(" temp = "); Serial.print((float)temp/340.0+36.53);
	Serial.print(" accX="); Serial.print(accXdata);
	// Serial.print(" accZ=");Serial.print(accZdata);
	// Serial.print(" gyYoffset="); Serial.print(gyroYoffset);
	Serial.print(" ang=");Serial.print(varAng);
	// Serial.print(" Omg="); Serial.print(varOmg);
	Serial.print(" pwr="); Serial.print(power);
	Serial.print(" yP="); Serial.print(yawPower);
	// Serial.print(" mP="); Serial.print(movePower);
	// Serial.print(" Iang="); Serial.print(varIang);
	Serial.print(" ");
	Serial.println(millis()-time0);
	}

view raw DRV8835KeshiV20p.ino hosted with ❤ by GitHub

木楽らぼソースコード

2019年12月18日水曜日

GR-KURUMIと電動消しゴムでバランスロボ

0 件のコメント:

コメントを投稿