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
| //(C) Kirakulabo 2017 | |
| //DRV8835 connected to D2,3,4,5,6 (MODE pin to GND) | |
| //MPU6050 (I2C) gyro and acc sensor | |
| //MB1422 (I2C) mag sensor with 3.3V power | |
| //AQM0802 (I2C) LCD driver | |
| //Touch sens antenna connected to A0 | |
| //LED connected to D9 | |
| //1H = 64 escapment wheel turn | |
| //1 escapment turn = 15 tic | |
| //tact time 100ms | |
| //folio calc every 1 turn of ecapement | |
| //V60 switch between Gyro and Mag, | |
| //V61 removed nRF24 | |
| //#define NOMOTOR //define this to simulate motor | |
| //#define GYRO //if defined, gyro(MPU6050). if not, mag(BM1422) | |
| #include <ST7032.h> | |
| #include <RTC.h> | |
| #include <Wire.h> | |
| #define TOUCHPIN A0 | |
| #define LED 9 | |
| #define DRVPWR 2 | |
| #define DRVAIN1 3 | |
| #define DRVAIN2 4 | |
| #define DRVBIN1 5 | |
| #define DRVBIN2 6 | |
| #define NHISTORY 64 | |
| #define BM14 0x0E | |
| ST7032 lcd; | |
| boolean serialMon=false; | |
| volatile long rtcTic=1; | |
| long rtcLastLoop=1; | |
| int dataX, dataY, dataZ; | |
| int angle, angLast; //1deg=10 | |
| int angMax, angMin, angThresh; | |
| int angHyst=50; //5deg | |
| int omegaHysteresis=2000; | |
| int folioDir, folioDirLast; | |
| unsigned long loopTime; | |
| long rtcLastFolioMove; | |
| long tFolio; | |
| long tFolioCumulativeError=0; | |
| long cFolio=-1; | |
| int stepNumber=0; | |
| int stepNumberTarget=0; | |
| long rtcEscWheelHistory[NHISTORY]; | |
| long nEscapeTurn; | |
| long tFolioRateError=0; | |
| long nCumulativeStep=0; | |
| long tFolioErrorMax=0; | |
| long tFolioErrorMin=0; | |
| boolean lcdEnabled=true; | |
| int lcdOnTime=900; //90 sec | |
| int lcdTimer; | |
| int steps; | |
| int accY, gyroY; | |
| int gyroData, omega; | |
| void setup() { | |
| pinMode(DRVPWR, OUTPUT); | |
| pinMode(DRVAIN1, OUTPUT); | |
| pinMode(DRVAIN2, OUTPUT); | |
| pinMode(DRVBIN1, OUTPUT); | |
| pinMode(DRVBIN2, OUTPUT); | |
| pinMode(LED, OUTPUT); | |
| digitalWrite(LED, HIGH); | |
| digitalWrite(DRVPWR, LOW); | |
| #ifdef GYRO | |
| initGyro(); | |
| #endif | |
| setPowerManagementMode(PM_STOP_MODE); | |
| rtc_init(); | |
| rtc_attach_constant_period_interrupt_handler(rtcIntHandler); | |
| rtc_set_constant_period_interrupt_time(RTC_CONSTANT_PERIOD_TIME_1SECOND); | |
| rtc_constant_period_interrupt_on(); | |
| delay(100); | |
| initMag(); | |
| lcd.begin(8,2); //(c,r) | |
| delay(1000); | |
| startup(); | |
| if (serialMon) { | |
| Serial.begin(115200); | |
| Serial.println("Ready"); | |
| Serial.flush(); //this allows serial comm while CPU is in PM_STOP_MODE | |
| } | |
| } | |
| void loop() { | |
| loopTime=millis(); | |
| #ifdef GYRO | |
| getOmega(); | |
| if (omega>omegaHysteresis) folioDir=1; | |
| if (omega<-omegaHysteresis) folioDir=-1; | |
| #else | |
| getAngle(); | |
| if (angle>angThresh+angHyst) folioDir=1; | |
| if (angle<angThresh-angHyst) folioDir=-1; | |
| #endif | |
| if (folioDir==1 && folioDirLast==-1) folioCycle(); | |
| folioDirLast=folioDir; | |
| if (rtcTic != rtcLastLoop) { //executed very 1 sec | |
| rtcLastLoop=rtcTic; | |
| if (lcdEnabled) { | |
| tFolioCumulativeError=tFolio-rtcTic; | |
| lcdDispStatus(); | |
| } | |
| } | |
| delay(50); | |
| touchSense(); | |
| while(millis()-loopTime<100) delay(2); | |
| } | |
| void rtcIntHandler() { | |
| rtcTic++; | |
| } | |
| void startup() { | |
| lcd.setContrast(25); | |
| lcdOn(); | |
| lcdDispText(0,0, "STEPPER "); | |
| lcdDispText(0,1, "TRAINING"); | |
| delay(1000); | |
| for (int i=0; i<25; i++) { | |
| stepNumberTarget+=10; | |
| motorStep(); | |
| delay(200); | |
| } | |
| stepNumber=200; | |
| stepNumberTarget=0; | |
| motorStep(); | |
| nCumulativeStep=0; | |
| lcdDispText(0,0, "WAIT FOR"); | |
| lcdDispText(0,1, "FOLIO MV"); | |
| #ifdef GYRO | |
| do { | |
| getOmega(); | |
| } while (omega<=omegaHysteresis); | |
| delay(10000); | |
| #else | |
| waitFolioMotion(); | |
| lcdDispText(0,0, "CALC ANG"); | |
| lcdDispText(0,1, "THRESHLD"); | |
| getThresh(); | |
| #endif | |
| lcdDispText(0,0, "OK 000"); | |
| lcdDispText(0,1, "0000 000"); | |
| } | |
| void folioCycle() { | |
| digitalWrite(LED, LOW); | |
| delay(1); | |
| digitalWrite(LED, HIGH); | |
| cFolio++; | |
| if (cFolio<0) return; | |
| if (cFolio==0) rtcTic=0; | |
| long rtcTicAtCycle=rtcTic; | |
| rtcLastFolioMove=rtcTicAtCycle; | |
| tFolio=cFolio*15/4; //escapement 1tic=3.75sec | |
| tFolioCumulativeError=tFolio-rtcTicAtCycle; | |
| if (tFolioCumulativeError>tFolioErrorMax) tFolioErrorMax=tFolioCumulativeError; | |
| if (tFolioCumulativeError<tFolioErrorMin) tFolioErrorMin=tFolioCumulativeError; | |
| lcdDispFolioTic(4,1); | |
| lcdDispNum34(4,0, tFolioCumulativeError); | |
| if (abs(tFolioCumulativeError)>300) return; //if error > 5min, do not move motor nor send RF | |
| if (cFolio%15==0) { //every 15 turn =~1min | |
| nEscapeTurn=cFolio/15; | |
| rtcEscWheelHistory[nEscapeTurn%NHISTORY]=rtcTicAtCycle; | |
| if (nEscapeTurn>=4) tFolioRateError=225-(rtcTicAtCycle-rtcEscWheelHistory[(nEscapeTurn+NHISTORY-4)%NHISTORY]); //gain against 4 escape turns ago (=~4min) | |
| else tFolioRateError=tFolioCumulativeError; | |
| lcdDispNum3(5,1, tFolioRateError); | |
| if (abs(tFolioCumulativeError)<=300) { //if error > 5min, do not move motor | |
| if (tFolioRateError*tFolioCumulativeError >=0) steps=(int) ((tFolioRateError*2)/3 + tFolioCumulativeError/5); | |
| else steps=0; | |
| stepNumberTarget = constrain(stepNumber+steps, -200, 200); | |
| steps = stepNumberTarget-stepNumber; | |
| if (stepNumberTarget != stepNumber) { | |
| motorStep(); | |
| lcdDispNum4(0,0, nCumulativeStep); | |
| } | |
| } | |
| } | |
| } | |
| void touchSense() { | |
| int adata1=analogRead(TOUCHPIN); //touch sensor | |
| delay(1); | |
| int adata2=analogRead(TOUCHPIN); | |
| if (abs(adata1-adata2) > 18 || adata1+adata2 < 800) { //touch detect threshold | |
| if (!lcdEnabled) lcdOn(); | |
| lcdTimer=lcdOnTime; | |
| } | |
| else { | |
| if (lcdEnabled) lcdTimer--; | |
| if (lcdTimer<=0) lcdOff(); | |
| } | |
| } | |
| void lcdOn() { | |
| lcdTimer=lcdOnTime; | |
| lcd.display(); | |
| lcdEnabled=true; | |
| lcdDispNum4(0,0, nCumulativeStep); | |
| lcdDispNum4(0,1, tFolioErrorMax); | |
| lcdDispNum34(4,0, tFolioCumulativeError); | |
| lcdDispNum3(5,1, tFolioRateError); | |
| } | |
| void lcdOff() { | |
| lcd.noDisplay(); | |
| lcdEnabled=false; | |
| } | |
| void lcdDispText(int col, int row, String s) { | |
| if (!lcdEnabled) return; | |
| lcd.setCursor(col,row); | |
| lcd.print(s); | |
| } | |
| void lcdDispStatus() { | |
| if (rtcTic-rtcLastFolioMove > 6) lcdDispNum34(4,0, tFolioCumulativeError); //disp LCD even if folio not moving | |
| if (rtcTic%8==0) { | |
| lcdDispNum4(0,0, nCumulativeStep); | |
| lcdDispNum4(0,1, tFolioErrorMax); | |
| } | |
| else if (rtcTic%8==4) { | |
| if (rtcTic<43200) { | |
| lcdDispNum3(0,0, rtcTic/60); | |
| lcdDispText(3,0, "M"); | |
| } | |
| else if (rtcTic<3600000) { | |
| lcdDispNum3(0,0, rtcTic/3600); | |
| lcdDispText(3,0, "H"); | |
| } | |
| else { | |
| lcdDispNum3(0,0, rtcTic/86400); | |
| lcdDispText(3,0, "D"); | |
| } | |
| lcdDispNum4(0,1, tFolioErrorMin); | |
| } | |
| } | |
| void lcdDispNum4(int col, int row, long n) { | |
| if (!lcdEnabled) return; | |
| char pNum[6]; | |
| if (n>999999) sprintf(pNum, "%s", ">1+6"); | |
| else if (n>99999) sprintf(pNum, "%02dE4", n/10000); | |
| else if (n>9999) sprintf(pNum, "%02dE3", n/1000); | |
| else if (n>=0) sprintf(pNum, "%04d", n); | |
| else if (n>=-999) sprintf(pNum, "%04d", n); | |
| else if (n>=-9999) sprintf(pNum, "%02d-2", -n/100); | |
| else if (n>=-99999) sprintf(pNum, "%02d-3", -n/1000); | |
| else if (n>=-999999) sprintf(pNum, "%02d-4", -n/10000); | |
| else sprintf(pNum, "%s", "<1-6"); | |
| lcd.setCursor(col,row); | |
| lcd.print(pNum); | |
| } | |
| void lcdDispNum3(int col, int row, long n) { | |
| if (!lcdEnabled) return; | |
| char pNum[6]; | |
| if (n>999) sprintf(pNum, "%s", ">k+"); | |
| else if (n>=-99) sprintf(pNum, "%03d", n); | |
| else if (n>=-999) sprintf(pNum, "%02d-", -n/10); | |
| else sprintf(pNum, "%s", "<k-"); | |
| lcd.setCursor(col,row); | |
| lcd.print(pNum); | |
| } | |
| void lcdDispNum34(int col, int row, long n) { | |
| if (!lcdEnabled) return; | |
| char pNum[6]; | |
| if (n>999) sprintf(pNum, "%s", " >k+"); | |
| else if (n>=-99) sprintf(pNum, " %03d", n); | |
| else if (n>=-999) sprintf(pNum, "%04d", n); | |
| else sprintf(pNum, "%s", " <k-"); | |
| lcd.setCursor(col,row); | |
| lcd.print(pNum); | |
| } | |
| void lcdDispFolioTic(int col, int row) { | |
| if (!lcdEnabled) return; | |
| lcd.setCursor(col, row); | |
| char n=cFolio%60+0xA1; //"縲�"=0,... "繝ｯ"=59 | |
| lcd.print(n); | |
| } | |
| void motorStep() { | |
| digitalWrite(DRVPWR, HIGH); | |
| delay(5); | |
| while (stepNumberTarget-stepNumber != 0) { | |
| if (stepNumberTarget>stepNumber) stepNumber++; | |
| else stepNumber--; | |
| nCumulativeStep++; | |
| #if defined(NOMOTOR) | |
| Serial.print("Motor setp="); | |
| Serial.flush(); //this allows serial comm while CPU is in PM_STOP_MODE | |
| Serial.println(stepNumber); | |
| Serial.flush(); //this allows serial comm while CPU is in PM_STOP_MODE | |
| #else | |
| motorDrive(); | |
| #endif | |
| delay(10); //motor freq=100Hz | |
| } | |
| motorOut(LOW, LOW, LOW, LOW); | |
| digitalWrite(DRVPWR, LOW); | |
| } | |
| void motorDrive() { | |
| int phase=stepNumber%4; | |
| if (phase<0) phase=4+phase; | |
| if (phase==0) motorOut(HIGH, LOW, HIGH, LOW); | |
| if (phase==1) motorOut(LOW, HIGH, HIGH, LOW); | |
| if (phase==2) motorOut(LOW, HIGH, LOW, HIGH); | |
| if (phase==3) motorOut(HIGH, LOW, LOW, HIGH); | |
| } | |
| void motorOut(byte out1, byte out2, byte out3, byte out4) { | |
| digitalWrite(DRVAIN1, out1); | |
| digitalWrite(DRVAIN2, out2); | |
| digitalWrite(DRVBIN1, out3); | |
| digitalWrite(DRVBIN2, out4); | |
| } | |
| void getMag() { | |
| magWrite(0x1D, 0x40); //Start ADC | |
| delay(1); | |
| dataX=magRead(0x10); | |
| dataY=magRead(0x12); | |
| dataY+=160; | |
| dataZ=magRead(0x14); | |
| dataZ+=50; | |
| } | |
| void initMag() { | |
| magWrite(0x1B, 0x82); //CNTL1 | |
| delay(2); | |
| magWrite(0x5C, 0); //CNTL4 LSB | |
| magWrite(0x5D, 0); //CNTL4 MSB | |
| magWrite(0x1C, 0x08); //CNTL2 DRDY enable with low active | |
| getMag(); | |
| } | |
| void waitFolioMotion() { | |
| angMax=-3600; | |
| angMin=3600; | |
| do { | |
| getAngle(); | |
| if (angle > angMax) angMax=angle; | |
| if (angle < angMin) angMin=angle; | |
| delay(100); | |
| } while (angMax-angMin<300); //30deg | |
| if (serialMon) { | |
| Serial.println("MOTION OK"); | |
| Serial.flush(); | |
| } | |
| } | |
| void getThresh() { | |
| unsigned long msec=millis(); | |
| angMax=-3600; | |
| angMin=3600; | |
| do { | |
| getAngle(); | |
| if (angle > angMax) angMax=angle; | |
| if (angle < angMin) angMin=angle; | |
| delay(100); | |
| } while (millis()-msec<15000); //15second | |
| angThresh=(angMax+angMin)/2; | |
| } | |
| void getAngle() { | |
| getMag(); | |
| angle= (int) (atan2((double)dataZ, (double)dataY)*572.9579); //1800.0/3.141592=572.9579, 180deg=1800 | |
| int diff=angle - angLast; | |
| if (diff > 1800) angle-=3600; | |
| if (diff < -1800) angle+=3600; | |
| angLast=angle; | |
| if (serialMon) { | |
| Serial.print(dataZ); | |
| Serial.print(" "); | |
| Serial.print(dataY); | |
| Serial.print(" "); | |
| Serial.print(angle); | |
| Serial.print(" "); | |
| Serial.print(angMax); | |
| Serial.print(" "); | |
| Serial.print(angMin); | |
| Serial.print(" "); | |
| Serial.println(angThresh); | |
| Serial.flush(); | |
| } | |
| } | |
| void magWrite (unsigned char adr, unsigned char data) { | |
| Wire.beginTransmission(BM14); | |
| Wire.write(adr); | |
| Wire.write(data); | |
| Wire.endTransmission(true); | |
| } | |
| int magRead(unsigned char adr) { | |
| Wire.beginTransmission(BM14); | |
| Wire.write(adr); | |
| Wire.endTransmission(false); | |
| Wire.requestFrom(BM14, 2, (int)true); | |
| return Wire.read()|Wire.read()<<8; | |
| } | |
| void initGyro() { | |
| delay(100); | |
| Wire.beginTransmission(0x68); //Gyro sensor | |
| Wire.write(0x6B); // Power management register1 | |
| Wire.write(0x0A); // wake up MPU-6050, disable Temperature sensor, use gyroY PLL for clock | |
| Wire.endTransmission(true); | |
| Wire.beginTransmission(0x68); //Gyro sensor | |
| Wire.write(0x6C); // Power management register2 | |
| Wire.write(0xED); // disable accX, accZ, gyroX, gyroZ | |
| Wire.endTransmission(true); | |
| } | |
| void getOmega() { | |
| long tmp =0; | |
| for (int i=0; i<5; i++) { | |
| readGyro(); | |
| tmp += (long) gyroData; | |
| delay(2); | |
| } | |
| omega=(int) (tmp / 5); | |
| } | |
| void readGyro() { | |
| Wire.beginTransmission(0x68); | |
| Wire.write(0x3D); | |
| Wire.endTransmission(false); //enable incremental read | |
| Wire.requestFrom(0x68, 2, (int)true); //used to be requestFrom(0x68, 14, true) but got warning. | |
| accY=Wire.read()<<8|Wire.read(); //0x3D | |
| Wire.beginTransmission(0x68); | |
| Wire.write(0x45); | |
| Wire.endTransmission(false); //enable incremental read | |
| Wire.requestFrom(0x68, 2, (int)true); //used to be requestFrom(0x68, 14, true) but got warning. | |
| gyroY=Wire.read()<<8|Wire.read(); //0x45 | |
| gyroData = gyroY; | |
| } | |
