2018年7月14日土曜日

サーボで超簡単倒立振子

#include "Arduino.h"
#include <Wire.h>
#include <EEPROM.h>
#define IRINT_PIN 2 // IR Receive
#define LED_R 22
#define LED_G 23
#define LED_B 24
#define HCODE 0x4D //M
#define SERVO_PIN_R 10
#define SERVO_PIN_L 9
#define TONE_PIN A3
#define INT_IR 0 // IR RECEIVE interrupt number
#define IR_ON LOW
#define IR_OFF HIGH
#define IR_BUF_LEN 200
#define MIN_IR_INTERVAL 220
#define IR_CIRL 0
#define IR_UP 1
#define IR_CIRR 2
#define IR_LEFT 3
#define IR_CENTER 4
#define IR_RIGHT 5
#define IR_BAL_LF 6
#define IR_DOWN 7
#define IR_BAL_RF 8
#define IR_BAL_LB 9
#define IR_BAL_RB 10
#define MAX_IR_BUTTON 11
void sendSerial();
void resetPara();
void resetVar();
void getGyro();
void readGyro();
void calibGyro();
void calibAcc();
void calcTarget();
boolean pendDrive();
void motor();
void resetMotor();
boolean laid();
boolean upright();
boolean standing();
void getIR();
void regIR();
void printIrData();
void decodeNECAEHA();
void decodeSONY();
void irInt();
void irCommand();
void printIrData(String s);
void blinkLED(int n);
void pendulum();
void drvMotor(int pwmR, int pwmL);
void freeMotor();
void setColor(int r, int g, int b);
void readEEPROMbal();
void writeEEPROMbal();
void readEEPROMbutton();
void writeEEPROMbutton();
void selectMode();
int accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
int counter=0;
unsigned long time0=0;
double power, powerR, powerL;
double snsGyroZ, snsAccX;
double varAng, varOmg, varSpd, varDst, varIang, varIdst;
double gyroOffsetY, gyroOffsetZ, accOffsetX;
double gyroDataY, gyroDataZ, accDataX, accDataZ;
double gyroLSB=1.0/131.0; // deg/sec
double accLSB=1.0/16384.0; // g
double pendClk = 0.01; // in sec,
unsigned long pendInterval = 10000; // in usec
double cutoff = 0.1; // 2 * PI * f (f=Hz) //0.1
double Kang, Komg, Kdst, Kspd, Kspin, KIang, KIdst;
double spinPower;
double moveTarget, moveRate=0.0; //moveRate 0:stop, +:forward, -:backward
double moveStep = 0.0013;
double spinDestination, spinTarget;
double spinStep = 0.0;
double spinAngle;
boolean spinContinuous=false;
int pendPhase=-1;
double mechFactorR, mechFactorL;
double maxSpd;
int counterOverSpd;
int maxOverSpd=30;
double aveAccX=0.0;
double aveAccZ=0.0;
boolean debug=false;
boolean serialMonitor=false;
int pulseZeroR, pulseZeroL;
volatile unsigned long irMicroOn;
volatile unsigned long irMicroOff;
volatile unsigned int irDataOn[200];
volatile unsigned int irDataOff[200];
volatile int irOnIndex=0;
volatile int irOffIndex=0;
volatile boolean irStarted=false;
volatile boolean irDecoding=false;
volatile unsigned long lastIrTime=0;
boolean irReady=false;
byte irData[30];
unsigned int ircode;
unsigned int customerCode;
unsigned int irCustomer;
unsigned int irButton[MAX_IR_BUTTON];
int irButtonNum=-1;
int demoMode=0;
int8_t drvHLbalance=0;
boolean colorR, colorG, colorB;
void setup() {
analogWriteFrequency(10000);
attachInterrupt(INT_IR, irInt, CHANGE);
pinMode(IRINT_PIN, INPUT_PULLUP);
pinMode(LED_R, OUTPUT);
digitalWrite(LED_R, HIGH);
pinMode(LED_G, OUTPUT);
digitalWrite(LED_G, HIGH);
pinMode(LED_B, OUTPUT);
digitalWrite(LED_B, HIGH);
pinMode(12, OUTPUT);
digitalWrite(12, LOW);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
pinMode(TONE_PIN, OUTPUT);
pinMode(A4, OUTPUT);
digitalWrite(A4, HIGH);
pinMode(A5, INPUT_PULLUP);
pinMode(A6, OUTPUT);
digitalWrite(A6, LOW);
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();
pinMode(SERVO_PIN_R, OUTPUT);
pinMode(SERVO_PIN_L, OUTPUT);
delay(100);
selectMode();
readEEPROMbal();
readEEPROMbutton();
resetPara();
resetVar();
calibGyro();
blinkLED(3);
}
void loop(){
time0=micros();
selectMode();
getIR();
getGyro();
pendulum();
counter += 1;
if (counter >= 100) {
counter = 0;
if (debug) digitalWrite(LED_R, LOW);
if (serialMonitor) sendSerial();
if (debug) digitalWrite(LED_R, HIGH);
}
while (micros() - time0 < pendInterval);
}
void pendulum() {
switch (pendPhase) {
case -1: //wait until upright
calcTarget();
motor();
if (upright()) pendPhase=0;
break;
case 0: //calib Acc
resetVar();
delay(1300);
if (!upright()) pendPhase=2;
else {
calibAcc();
if (upright()) pendPhase=1;
else pendPhase=2;
}
break;
case 1: //run
if (!standing()) {
pendPhase=6;
}
else {
// if (demoMode==2) demo8();
calcTarget();
boolean ok=pendDrive();
if (!ok) {
delay(1300);
pendPhase=2;
}
}
break;
case 2: //wait until upright
freeMotor();
if (upright()) pendPhase=0;
break;
case 6: //fell
if (laid()) pendPhase=8;
break;
case 8:
pendPhase=2;
break;
}
}
void calcTarget() {
if (spinContinuous) {
spinTarget += spinStep;
}
else {
if (spinTarget < spinDestination) spinTarget += spinStep;
if (spinTarget > spinDestination) spinTarget -= spinStep;
}
moveTarget += moveStep * moveRate;
}
boolean pendDrive() {
varSpd += power * pendClk;
varDst += Kdst * (varSpd * pendClk - moveTarget);
varIang += KIang * varAng * pendClk;
varIdst += KIdst * varDst * pendClk;
power = varIang + varIdst + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
if (abs(power) > 5000.0) counterOverSpd +=1;
else counterOverSpd=0;
if (counterOverSpd > maxOverSpd) {
resetMotor();
resetVar();
return false;
}
spinPower = (spinAngle - spinTarget) * Kspin;
powerL = power + spinPower;
powerR = power - spinPower;
motor();
return true;
}
void motor() {
int pwmR= (int) (constrain(powerR * mechFactorR, -maxSpd, maxSpd));
int pwmL= (int) (constrain(powerL * mechFactorL, -maxSpd, maxSpd));
drvMotor(pwmR, pwmL);
}
void drvMotor(int pwmR, int pwmL) {
int pulseR=constrain(pulseZeroR+pwmR, 1000, 2000);
int pulseL=constrain(pulseZeroL-pwmL, 1000, 2000);
bool doneR=false;
bool doneL=false;
unsigned long usec=micros();
digitalWrite(SERVO_PIN_R, HIGH);
digitalWrite(SERVO_PIN_L, HIGH);
while(!doneR || !doneL) {
unsigned long width=micros()-usec;
if (width>=pulseR) {
digitalWrite(SERVO_PIN_R, LOW);
doneR=true;
}
if (width>=pulseL) {
digitalWrite(SERVO_PIN_L, LOW);
doneL=true;
}
}
}
void getGyro() {
readGyro();
snsGyroZ = (gyroDataZ - gyroOffsetZ) * gyroLSB;
varOmg = (gyroDataY - gyroOffsetY) * gyroLSB; // unit:deg/sec
snsAccX = (accDataX - accOffsetX) * accLSB; //unit:g
spinAngle += snsGyroZ * pendClk;
varAng += (varOmg + (snsAccX * 57.3 - varAng) * cutoff ) * pendClk;
//angle filter accX=g*sin(ang) -> accX/g=sin(ang)=ang [rad] (if ang is small) -> ang=accX*57.3[deg]
}
void readGyro() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
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
gyroDataZ = (double) gyroY;
gyroDataY = (double) gyroZ;
accDataX = -(double) accX;
aveAccX = aveAccX * 0.9 + accDataX * 0.1;
accDataZ = -(double) accY;
aveAccZ = aveAccZ * 0.9 + accDataZ * 0.1;
}
boolean laid() {
if (abs(aveAccX) >13000.0) return true;
else return false;
}
boolean upright() {
if (abs(aveAccZ) >13000.0) return true;
else return false;
}
boolean standing() {
if (abs(aveAccZ) >8000.0 && abs(varAng) < 40.0) return true;
else {
resetMotor();
resetVar();
return false;
}
}
void calibAcc() {
tone(TONE_PIN, 8);
accOffsetX=0.0;
for (int i=1; i <= 30; i++) {
readGyro();
accOffsetX += accDataX;
delay(20);
}
accOffsetX /= 30.0;
noTone(TONE_PIN);
}
void calibGyro() {
tone(TONE_PIN, 8);
gyroOffsetZ=gyroOffsetY=0.0;
for (int i=1; i <= 30; i++) {
readGyro();
gyroOffsetZ += gyroDataZ;
gyroOffsetY += gyroDataY;
delay(20);
}
gyroOffsetY /= 30.0;
gyroOffsetZ /= 30.0;
noTone(TONE_PIN);
}
void resetVar() {
power=0.0;
moveTarget=0.0;
spinDestination=0.0;
spinTarget=0.0;
spinAngle=0.0;
varAng=0.0;
varOmg=0.0;
varDst=0.0;
varSpd=0.0;
varIang=0.0;
varIdst=0.0;
counterOverSpd=0;
spinContinuous=false;
moveRate=0.0;
}
void resetPara() {
Komg=3.5;
Kang=350.0;
KIang=1400.0;
Kspin=20.0;
Kspd=12.0;
Kdst=8.0;
KIdst=0.0;
mechFactorR=0.25;
mechFactorL=0.25;
maxSpd=1000.0;
pulseZeroR=1453;//less=forward
pulseZeroL=1478;//more=forward
}
void freeMotor() {
digitalWrite(SERVO_PIN_R, LOW);
digitalWrite(SERVO_PIN_L, LOW);
}
void resetMotor() {
drvMotor(0, 0);
counterOverSpd=0;
sendSerial();
}
void blinkLED(int n) {
for (int i=0; i<n; i++) {
setColor(1,0,0);
digitalWrite(LED_R, LOW);
delay(10);
digitalWrite(LED_R, HIGH);
delay(200);
}
}
void setColor(int r, int g, int b) {
colorR=r;
colorG=g;
colorB=b;
}
void sendSerial () {
Serial.print(micros()-time0);
Serial.print(" phase=");Serial.print(pendPhase);
// Serial.print(" dist=");Serial.print(distance);
Serial.print(" accX="); Serial.print(accDataX);
Serial.print(" ang=");Serial.print(varAng);
// Serial.print(" temp = "); Serial.print(temp/340.0+36.53);
// Serial.print(" Omg="); Serial.print(varOmg);
Serial.print(" powerR="); Serial.print(powerR);
Serial.print(" powerL="); Serial.print(powerL);
Serial.print(" Dst="); Serial.print(varDst);
// Serial.print(" aveDst="); Serial.print(aveDst);
Serial.print(" Iang="); Serial.print(varIang);
Serial.print(", ");
Serial.print(micros()-time0);
Serial.println();
}
void getIR() {
if (!(irStarted && (micros()-irMicroOff>10000))) return;
irStarted=false;
irDecoding=true;
if (irDataOn[0]>7000) { //NEC
if (irOffIndex >=33) {
decodeNECAEHA();
printIrData("NEC");
}
}
else if (irDataOn[0]>2600) { //AEHA
if (irOffIndex >=41) {
decodeNECAEHA();
printIrData("AEHA");
}
}
else if (irDataOn[0]>1800) { //SONY
if (irOnIndex >=12) {
decodeSONY();
printIrData("SONY");
}
}
if (irReady) {
irReady=false;
lastIrTime=millis();
if (irButtonNum>=0 && irButtonNum<MAX_IR_BUTTON && pendPhase==-1) regIR();
else irCommand();
}
irDecoding=false;
}
void irCommand() {
if ((ircode==irButton[IR_CENTER])&&(customerCode==irCustomer)) {
if (pendPhase!=-1) {
if (abs(moveRate)>1.0 || abs(spinStep)>0.1) {
tone(TONE_PIN, 1000, 100);
}
demoMode=0;
spinContinuous=false;
spinStep=0.0;
moveRate=0.0;
spinDestination = spinTarget;
}
}
else if ((ircode==irButton[IR_RIGHT])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
for (int i=0; i<100; i++) {
drvMotor(-80, 0);
delay(8);
}
// drvMotor(0, 0);
}
else if (demoMode==0) {
tone(TONE_PIN, 2000, 100);
if (spinContinuous) spinDestination=spinTarget;
spinContinuous=false;
spinStep=0.6;
spinDestination += 30.0;
}
}
else if ((ircode==irButton[IR_LEFT])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
for (int i=0; i<100; i++) {
drvMotor(0, -80);
delay(8);
}
// drvMotor(0, 0);
}
else if (demoMode==0) {
tone(TONE_PIN, 2000, 100);
if (spinContinuous) spinDestination=spinTarget;
spinContinuous=false;
spinStep=0.6;
spinDestination -= 30.0;
}
}
else if ((ircode==irButton[IR_UP])&&(customerCode==irCustomer)) {
if (pendPhase!=-1 && demoMode==0) {
tone(TONE_PIN, 1000, 100);
moveRate-=20.0;
}
}
else if ((ircode==irButton[IR_DOWN])&&(customerCode==irCustomer)) {
if (pendPhase!=-1 && demoMode==0) {
tone(TONE_PIN, 1000, 100);
moveRate+=20.0;
}
}
else if ((ircode==irButton[IR_CIRL])&&(customerCode==irCustomer)) {
if (pendPhase!=-1 && demoMode==0) {
tone(TONE_PIN, 2000, 20);
if (!spinContinuous) spinStep=0.0;
spinContinuous=true;
spinStep-=0.3;
}
}
else if ((ircode==irButton[IR_CIRR])&&(customerCode==irCustomer)) {
if (pendPhase!=-1 && demoMode==0) {
tone(TONE_PIN, 2000, 20);
if (!spinContinuous) spinStep=0.0;
spinContinuous=true;
spinStep+=0.3;
}
}
else if ((ircode==irButton[IR_BAL_LF])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
pulseZeroL+=10;
tone(TONE_PIN, 4000, 10);
delay(20);
digitalWrite(LED_B, LOW);
writeEEPROMbalL();
digitalWrite(LED_B, HIGH);
}
}
else if ((ircode==irButton[IR_BAL_LB])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
pulseZeroL-=10;
tone(TONE_PIN, 4000, 10);
delay(20);
digitalWrite(LED_B, LOW);
writeEEPROMbalL();
digitalWrite(LED_B, HIGH);
}
}
else if ((ircode==irButton[IR_BAL_RF])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
pulseZeroR-=10;
tone(TONE_PIN, 4000, 10);
delay(20);
digitalWrite(LED_B, LOW);
writeEEPROMbalR();
digitalWrite(LED_B, HIGH);
}
}
else if ((ircode==irButton[IR_BAL_RB])&&(customerCode==irCustomer)) {
if (pendPhase==-1) {
pulseZeroR+=10;
tone(TONE_PIN, 4000, 10);
delay(20);
digitalWrite(LED_B, LOW);
writeEEPROMbalR();
digitalWrite(LED_B, HIGH);
}
}
else {
if (pendPhase == -1 && debug && irButtonNum==-1) {
digitalWrite(LED_G, LOW);
irButtonNum=0;
}
}
}
void regIR() {
if (irButtonNum<MAX_IR_BUTTON) {
irButton[irButtonNum]=ircode;
irCustomer=customerCode;
if (serialMonitor) {
Serial.print("set irButton[");
Serial.print(irButtonNum);
Serial.print("] = ");
Serial.println(ircode, HEX);
Serial.print("set irCustomer = ");
Serial.println(customerCode, HEX);
}
if (irButtonNum==0) {
irButtonNum++;
tone(TONE_PIN, 4000, 100);
}
else if (irButton[irButtonNum-1]!=ircode) {
irButtonNum++;
tone(TONE_PIN, 4000, 100);
}
else tone(TONE_PIN, 1000, 100);
if (irButtonNum==MAX_IR_BUTTON) {
delay(200);
writeEEPROMbutton();
digitalWrite(LED_G, HIGH);
}
}
}
void irInt() {
if (millis()-lastIrTime<MIN_IR_INTERVAL) return;
if (irDecoding) return;
if (digitalRead(IRINT_PIN) == IR_ON) {
irMicroOn=micros();
if (irStarted) {
irDataOff[irOffIndex]=irMicroOn-irMicroOff;
irOffIndex++;
if (irOffIndex>=IR_BUF_LEN) irStarted=false;
}
else {
irStarted=true;
irOnIndex=0;
irOffIndex=0;
irMicroOff=irMicroOn;
}
}
else {
irMicroOff=micros();
irDataOn[irOnIndex]=irMicroOff-irMicroOn;
irOnIndex++;
if (irOnIndex>=IR_BUF_LEN) irStarted=false;
}
}
void decodeNECAEHA() {
int len=irOffIndex/8;
int idx=1;
for (int i=0; i<len; i++) {
irData[i]=0;
for (int j=0; j<8; j++) {
irData[i]>>=1;
if (irDataOff[idx]>1000) irData[i]|=0x80;
idx++;
}
}
customerCode=irData[0]<<8|irData[1];
ircode=irData[len-2]<<8|irData[len-1];
irReady=true;
}
void decodeSONY() {
byte data=0;
for (int i=1; i<=7; i++) {
if (irDataOn[i]>900) data|=0x80;
data>>=1;
}
unsigned int addr=0;
int idx=8;
for (int i=0; i<16; i++) {
addr>>=1;
if (idx<irOnIndex && irDataOn[idx]<1800) {
if (irDataOn[idx]>900) addr|=0x8000;
idx++;
}
}
customerCode=addr;
ircode=(unsigned int)data;
irReady=true;
}
void printIrData(String s) {
if (!serialMonitor) return;
/*
for (int i=0; i<irOffIndex; i++) {
Serial.print(irDataOn[i]);Serial.print(" ");Serial.println(irDataOff[i]);
}
Serial.println("");
*/
Serial.println(s);
Serial.print("Customer=");Serial.println(customerCode, HEX);
Serial.print("Code=");Serial.println(ircode, HEX);
}
void readEEPROMbal() {
if (EEPROM.read(0)==HCODE) {
pulseZeroL=EEPROM.read(1)<<8|EEPROM.read(2);
pulseZeroR=EEPROM.read(3)<<8|EEPROM.read(4);
if (serialMonitor) {Serial.print("pulseZeroL=");Serial.print(pulseZeroL);Serial.print(" pulseZeroR=");Serial.println(pulseZeroR);}
}
else {
noInterrupts();
EEPROM.write(0, HCODE);
EEPROM.write(1, (byte)((pulseZeroL>>8)&0xFF));
EEPROM.write(2, (byte)(pulseZeroL&0xFF));
EEPROM.write(3, (byte)((pulseZeroR>>8)&0xFF));
EEPROM.write(4, (byte)(pulseZeroR&0xFF));
interrupts();
}
}
void writeEEPROMbalL() {
noInterrupts();
EEPROM.write(1, (byte)((pulseZeroL>>8)&0xFF));
EEPROM.write(2, (byte)(pulseZeroL&0xFF));
interrupts();
if (serialMonitor) {Serial.print("pulseZeroL=");Serial.println(pulseZeroL);}
}
void writeEEPROMbalR() {
noInterrupts();
EEPROM.write(3, (byte)((pulseZeroR>>8)&0xFF));
EEPROM.write(4, (byte)(pulseZeroR&0xFF));
interrupts();
if (serialMonitor) {Serial.print("pulseZeroR=");Serial.println(pulseZeroR);}
}
void readEEPROMbutton() {
if (EEPROM.read(5)==HCODE) {
irCustomer=EEPROM.read(6)<<8|EEPROM.read(7);
if (serialMonitor) {Serial.print("CUST=");Serial.println(irCustomer,HEX);}
for (int i=0; i<MAX_IR_BUTTON; i++) {
irButton[i]=EEPROM.read(8+i*2)<<8|EEPROM.read(9+i*2);
if (serialMonitor) {Serial.print(" CODE=");Serial.println(irButton[i],HEX);}
}
}
}
void writeEEPROMbutton() {
noInterrupts();
EEPROM.write(5, HCODE);
EEPROM.write(6, (byte)((irCustomer>>8)&0xFF));
EEPROM.write(7, (byte)(irCustomer&0xFF));
for (int i=0; i<MAX_IR_BUTTON; i++) {
EEPROM.write(8+i*2, (byte)((irButton[i]>>8)&0xFF));
EEPROM.write(9+i*2, (byte)(irButton[i]&0xFF));
}
interrupts();
}
void selectMode() {
if (digitalRead(A5)==LOW) {
debug=true;
serialMonitor=true;
}
else {
irButtonNum=-1;
digitalWrite(LED_G, HIGH);
debug=false;
serialMonitor=false;
}
}
view raw ServoSG90KurumiV08p.ino hosted with ❤ by GitHub
投稿者 時刻: 4 件のコメント:
登録: 投稿 (Atom)