This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| //Inverted Pendulum Robot | |
| //DRV8835 driver | |
| //wheele dia = 55mm | |
| //Battery: NiHM x 4 | |
| //MPU: ProMini 3.3V | |
| //Gyro: MPU6050 (I2C pullup 4.7kohm) | |
| //Motor driver: DRV8835 connected to Arduino pin 8,9,10,11 | |
| //Distance sensor: Sharp GP2Y0A21 connected to Arduino pin A1 | |
| //Trace sensor: Reflectance (distance from axis ~= 30mm) connected to Arduino pin A0 | |
| //IR sensor PL-IRM2161-XD1 connected to Arduino pin 2 | |
| #include <Wire.h> | |
| #include <IRremote.h> | |
| IRrecv receiveIR(2); | |
| decode_results results; | |
| void motor(); | |
| void drvMotorR(int pwm); | |
| void drvMotorL(int pwm); | |
| void drive(); | |
| void drvMotor(int pinPhase, int pinEnable, int pwm); | |
| void pulseMotor(int n); | |
| void calibrate(); | |
| void getBaselineAcc(); | |
| void getBaselineGyro(); | |
| bool standing(); | |
| bool laid(); | |
| bool upright(); | |
| void calcTarget(); | |
| void getDistance(); | |
| void getGyro(); | |
| void readGyro(); | |
| void getIR(); | |
| void monitorVoltage(); | |
| void selectMode(); | |
| void getBaselineGyro(); | |
| void resetMotor(); | |
| void resetVar(); | |
| void blinkLED(int n); | |
| void checkVoltage(); | |
| void sendSerial(); | |
| int accX, accY, accZ, temp, gyroX, gyroY, gyroZ; | |
| int counter=0; | |
| unsigned long time1=0; | |
| unsigned long time0=0; | |
| float power, powerR, powerL; | |
| float gyroZdps, accXg; | |
| float varAng, varOmg, varSpd, varDst, varIang, varIdst; | |
| float gyroZoffset, gyroYoffset, accXoffset; | |
| float gyroZdata, gyroYdata, accXdata, accZdata; | |
| float gyroLSB=1.0/131.0; // deg/sec | |
| float accLSB=1.0/16384.0; // g | |
| float clk = 0.01; // in sec, | |
| unsigned long interval = (unsigned long) (clk*1000.0); // in msec | |
| float cutoff = 0.1; // 2 * PI * f (f=Hz) | |
| float yawPower; | |
| float moveTarget, moveRate=0.0; //moveRate 0:stop, +:forward, -:backward | |
| float moveStep = 0.0013; | |
| float spinDestination, spinTarget, spinRate; | |
| float spinStep = 30.0*clk; | |
| float orientation; | |
| float dirStep = 30.0 * clk; //30deg/sec | |
| int state=-1; | |
| int traceSensor; | |
| bool serialMonitor=; | |
| bool spinContinuous=false; | |
| bool follow=false; | |
| bool trace=false; | |
| int ipowerR = 0; | |
| int ipowerL = 0; | |
| int motorRdir=0; //stop 1:+ -1:- | |
| int motorLdir=0; //stop | |
| float battFactor=1.0; | |
| float bVolt; | |
| float distance; | |
| int traceThLevel=500; | |
| int counterOverSpd; | |
| float aveAccX=0.0; | |
| float aveAccZ=0.0; | |
| float Kang=120.0; | |
| float Komg=2.8; | |
| float KIang=2300.0; | |
| float Kyaw=15.0; | |
| float Kdst=100.0; | |
| float Kspd=7.0; | |
| float KIdst=0.0; | |
| float mechFactorR=0.4; | |
| float mechFactorL=0.4; | |
| int backlashSpd=15; | |
| int motorDeadPWM=0; | |
| float maxSpd=250.0; | |
| void setup() { | |
| TCCR1B = TCCR1B & B11111000 | B00000001; | |
| pinMode(2,INPUT); //IR | |
| pinMode(8,OUTPUT); //APhase | |
| pinMode(9,OUTPUT); //AEnable | |
| pinMode(11,OUTPUT); //BPhase | |
| pinMode(10,OUTPUT); //BEnable | |
| pinMode(13, OUTPUT); //LED | |
| Wire.begin(); | |
| Wire.setClock(50000L); | |
| Wire.beginTransmission(0x68); //Gyro sensor | |
| Wire.write(0x6B); // Power management register | |
| Wire.write(0); // wake up MPU-6050 | |
| Wire.endTransmission(true); | |
| receiveIR.enableIRIn(); // Start the receiver | |
| Serial.begin(115200); | |
| checkVoltage(); | |
| resetVar(); | |
| delay(50); //stabilize gyro | |
| getBaselineGyro(); | |
| selectMode(); | |
| } | |
| void loop() { | |
| monitorVoltage(); | |
| getIR(); | |
| traceSensor = analogRead(A0); | |
| getGyro(); | |
| getDistance(); | |
| switch (state) { | |
| case -1: | |
| calcTarget(); | |
| if (upright()) state =0; | |
| break; | |
| case 0: //baseline | |
| calibrate(); | |
| state=1; | |
| break; | |
| case 1: //run | |
| if (standing() && counterOverSpd <= 30) { | |
| calcTarget(); | |
| drive(); | |
| } | |
| else { | |
| sendSerial(); | |
| 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; | |
| } | |
| counter += 1; | |
| if (counter >= 100) { | |
| counter = 0; | |
| if (serialMonitor) sendSerial(); | |
| } | |
| do time1 = millis(); | |
| while (time1 - time0 < interval); | |
| time0 = time1; | |
| } | |
| void getIR() { | |
| if (receiveIR.decode(&results)) { | |
| // Serial.println(results.value, HEX); | |
| switch (results.value & 0x0000FFFF) { | |
| case 0x728D: //LAPIO up | |
| case 0x7C83: //TOSHIBA up | |
| moveRate=4.0; //forward | |
| break; | |
| case 0xF20D: //LAPIO down | |
| case 0xFC03: //TOSHIBA down | |
| moveRate=-4.0; //backward | |
| break; | |
| case 0xFA05: //LAPIO&TOSHIBA left = spin left | |
| spinDestination -= 30.0; | |
| break; | |
| case 0xDA25: //LAPIO&TOSHIBA right = spin right | |
| spinDestination += 30.0; | |
| break; | |
| case 0xBC43: //LAPIO&TOSHIBA OK = stop | |
| trace=false; | |
| follow=false; | |
| spinContinuous=false; | |
| moveRate=0.0; | |
| spinDestination = spinTarget; | |
| break; | |
| case 0x807F: //LAPIO&TOSHIBA button 1 | |
| spinContinuous=true; | |
| follow=false; | |
| trace=false; | |
| moveRate=3.0; | |
| spinStep=0.30; | |
| break; | |
| } | |
| receiveIR.resume(); // Receive the next value | |
| } | |
| } | |
| void calcTarget() { | |
| if (trace) { | |
| float tmp = (float) (traceSensor - traceThLevel); | |
| spinTarget += 0.001 * tmp; | |
| moveRate = 2.0; | |
| } | |
| else { | |
| if (spinContinuous) { | |
| spinTarget += spinStep; | |
| } | |
| else { | |
| if (spinTarget < spinDestination) spinTarget += spinStep; | |
| if (spinTarget > spinDestination) spinTarget -= spinStep; | |
| } | |
| } | |
| if (follow) { | |
| if (distance > 180.0) moveRate=2.8; | |
| else if (distance > 130.0) moveRate=2.4; | |
| else if (distance > 90.0) moveRate=0.0; | |
| else moveRate=-1.0; | |
| } | |
| moveTarget += moveStep * moveRate; | |
| } | |
| void drive() { | |
| varSpd += power * clk; | |
| varDst += Kdst * (varSpd * clk - moveTarget); | |
| varIang += KIang * varAng * clk; | |
| varIdst += KIdst * varDst * clk; | |
| power = varIang + varIdst + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg); | |
| if (abs(power) > 3000.0) counterOverSpd += 1; | |
| else counterOverSpd =0; | |
| if (counterOverSpd > 30) 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 + motorDeadPWM); | |
| else drvMotorR(ipowerR + motorDeadPWM + backlashSpd); //compensate backlash | |
| motorRdir = 1; | |
| } | |
| else if (ipowerR < 0) { | |
| if (motorRdir == -1) drvMotorR(ipowerR - motorDeadPWM); | |
| else drvMotorR(ipowerR - motorDeadPWM - backlashSpd); | |
| motorRdir = -1; | |
| } | |
| else { | |
| drvMotorR(0); | |
| motorRdir = 0; | |
| } | |
| ipowerL = (int) (constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd)); | |
| if (ipowerL > 0) { | |
| if (motorLdir == 1) drvMotorL(ipowerL + motorDeadPWM); | |
| else drvMotorL(ipowerL + motorDeadPWM + backlashSpd); //compensate backlash | |
| motorLdir = 1; | |
| } | |
| else if (ipowerL < 0) { | |
| if (motorLdir == -1) drvMotorL(ipowerL - motorDeadPWM); | |
| else drvMotorL(ipowerL - motorDeadPWM - backlashSpd); | |
| motorLdir = -1; | |
| } | |
| else { | |
| drvMotorL(0); | |
| motorLdir = 0; | |
| } | |
| } | |
| void getDistance() { //distance in mm | |
| int dist=analogRead(A1); | |
| if (dist>0) distance = constrain(95000.0 / ((float) dist) -30.0, 60.0, 400.0); | |
| else distance = -1.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) * cutoff ) * 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) gyroY; | |
| gyroYdata = (float) gyroZ; | |
| accXdata = -(float) accX; | |
| aveAccX = aveAccX * 0.9 + accXdata * 0.1; | |
| accZdata = (float) accY; | |
| 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); | |
| if (serialMonitor) sendSerial(); | |
| pulseMotor(1); | |
| delay(100); | |
| if (serialMonitor) sendSerial(); | |
| getBaselineAcc(); | |
| } | |
| void getBaselineAcc() { | |
| accXoffset=0.0; | |
| for (int i=1; i <= 30; i++) { | |
| readGyro(); | |
| accXoffset += accXdata; | |
| delay(20); | |
| } | |
| accXoffset /= 30; | |
| } | |
| void getBaselineGyro() { | |
| gyroYoffset=gyroZoffset=0.0; | |
| for (int i=1; i <= 30; i++) { | |
| readGyro(); | |
| gyroYoffset += gyroYdata; | |
| gyroZoffset += gyroZdata; | |
| delay(20); | |
| } | |
| gyroZoffset /= 30; | |
| gyroYoffset /= 30; | |
| } | |
| 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; | |
| varIdst=0.0; | |
| } | |
| void resetMotor() { | |
| drvMotorR(0); | |
| drvMotorL(0); | |
| counterOverSpd=0; | |
| sendSerial(); | |
| } | |
| void checkVoltage() { | |
| bVolt = ((float) analogRead(A2)) / 1023.0 * 6.6; | |
| if (bVolt > 5.0) blinkLED(3); | |
| else if (bVolt > 4.8) blinkLED(2); | |
| else if (bVolt > 4.6) blinkLED(1); | |
| else blinkLED(100); | |
| } | |
| void blinkLED(int n) { | |
| for (int i=0; i<n; i++) { | |
| digitalWrite(13,HIGH); | |
| delay(10); | |
| digitalWrite(13,LOW); | |
| delay(200); | |
| } | |
| } | |
| void monitorVoltage() { | |
| digitalWrite(13, LOW); | |
| bVolt = ((float) analogRead(A2)) / 1023.0 * 6.6; | |
| battFactor = 5.0 / bVolt; | |
| if (bVolt<=4.4) { | |
| digitalWrite(13, HIGH); | |
| if (serialMonitor) { | |
| Serial.print("Batt="); | |
| Serial.println(bVolt); | |
| } | |
| } | |
| } | |
| void selectMode() { | |
| getDistance(); | |
| traceSensor = analogRead(A0); | |
| if (distance > 100.0) { | |
| if (traceSensor > traceThLevel) { //follow & trace | |
| trace=true; | |
| follow=true; | |
| blinkLED(3); | |
| } | |
| else { //stand still | |
| trace=false; | |
| follow=false; | |
| blinkLED(1); | |
| } | |
| } | |
| else if (distance > 0.0) { //circle | |
| follow=false; | |
| trace=false; | |
| moveRate=3.0; | |
| spinStep=0.30; | |
| spinContinuous=true; | |
| blinkLED(5); | |
| } | |
| } | |
| void pulseMotor(int n) { | |
| for (int i=0; i<n; i++) { | |
| drvMotorR(motorDeadPWM+50); | |
| delay(15); | |
| drvMotorR(0); | |
| delay(50); | |
| drvMotorR(-motorDeadPWM-50); | |
| delay(15); | |
| drvMotorR(0); | |
| delay(200); | |
| } | |
| } | |
| void drvMotorR(int pwm) { | |
| drvMotor(8,9, constrain(pwm, -255, 255)); | |
| } | |
| void drvMotorL(int pwm) { | |
| drvMotor(11,10, constrain(pwm, -255, 255)); | |
| } | |
| void drvMotor(int pinPhase, int pinEnable, int pwm) { | |
| if (pwm>=0) { | |
| // digitalWrite(pinEnable,LOW); | |
| // delayMicroseconds(10); | |
| digitalWrite(pinPhase,HIGH); | |
| analogWrite(pinEnable,pwm); | |
| } | |
| else { | |
| // digitalWrite(pinEnable,LOW); | |
| // delayMicroseconds(10); | |
| digitalWrite(pinPhase,LOW); | |
| analogWrite(pinEnable,-pwm); | |
| } | |
| } | |
| void sendSerial () { | |
| Serial.print(millis()-time0); | |
| Serial.print(" state=");Serial.print(state); | |
| // Serial.print(" dist=");Serial.print(distance); | |
| // Serial.print(" accX="); Serial.print(accXdata); | |
| // Serial.print(" accOfs=");Serial.print(accXoffset); | |
| // Serial.print(" gyYoffset="); Serial.print(gyroYoffset); | |
| // Serial.print(" ang=");Serial.print(varAng); | |
| // Serial.print(" temp = "); Serial.print(temp/340.0+36.53); | |
| // Serial.print(" Omg="); Serial.print(varOmg); | |
| // Serial.print(" power="); Serial.print(power); | |
| // Serial.print(" Dst="); Serial.print(varDst); | |
| Serial.print(" bVolt="); Serial.print(bVolt); | |
| // Serial.print(" Iang="); Serial.print(varIang); | |
| // Serial.print(" mTarg="); Serial.print(moveTarget); | |
| // Serial.print(" follow="); Serial.print(follow); | |
| // Serial.print(" interval="); Serial.print(interval); | |
| Serial.print(" "); | |
| Serial.println(millis()-time0); | |
| } | |
