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
| //DRV8835 connected to D2,3,4,5,6,9 | |
| //MBM1422 mag sensor with 3.3V power | |
| //Touch sens antenna connected to A0 | |
| //LED connected to D13 | |
| //1H = 64 escapment wheel turn | |
| //1 escapment turn = 15 tic | |
| //tact time 100ms | |
| //folio calc every 4 min | |
| //#define NOMOTOR | |
| #include <ST7032.h> | |
| #include <RTC.h> | |
| #include <Wire.h> | |
| #define DRVPWR 9 | |
| #define DRVMODE 2 | |
| #define DRVAIN1 3 | |
| #define DRVAIN2 4 | |
| #define DRVBIN1 5 | |
| #define DRVBIN2 6 | |
| #define NHISTORY 64 | |
| #define BM14 0x0E | |
| ST7032 lcd; | |
| volatile long rtcTic=1; | |
| long rtcLastLoop=1; | |
| int dataX, dataY, dataZ; | |
| int angle, angLast; //1deg=10 | |
| int angMax, angMin; | |
| int angThresh; | |
| int angHyst=50; //5deg | |
| int folioDir, folioDirLast; | |
| unsigned long loopTime; | |
| long rtcLastFolioMove; | |
| long tFolio, tFolioCumulativeError; | |
| long cFolio=-3; | |
| int stepNumber=0; | |
| int stepNumberTarget=0; | |
| long rtcEscWheelHistory[NHISTORY]; | |
| long nEscapeTurn, thresh; | |
| long tFolioRateError; | |
| long nCumulativeStep=0; | |
| long tFolioErrorMax=0; | |
| long tFolioErrorMin=0; | |
| boolean lcdEnabled=true; | |
| int lcdTimerCount=900; //90 sec | |
| int lcdTimer; | |
| void setup() { | |
| pinMode(DRVPWR, OUTPUT); | |
| pinMode(DRVMODE, OUTPUT); | |
| pinMode(DRVAIN1, OUTPUT); | |
| pinMode(DRVAIN2, OUTPUT); | |
| pinMode(DRVBIN1, OUTPUT); | |
| pinMode(DRVBIN2, OUTPUT); | |
| pinMode(13, OUTPUT); | |
| digitalWrite(13, HIGH); | |
| digitalWrite(DRVPWR, LOW); | |
| digitalWrite(DRVMODE, LOW); | |
| Serial.begin(115200); | |
| 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(); | |
| Serial.println("Ready"); | |
| } | |
| void loop() { | |
| loopTime=millis(); | |
| getAngle(); | |
| if (angle>angThresh+angHyst) folioDir=1; | |
| if (angle<angThresh-angHyst) folioDir=-1; | |
| if (folioDir==1 && folioDirLast==-1) folioCycle(); | |
| folioDirLast=folioDir; | |
| if (rtcTic != rtcLastLoop && cFolio>0) { //executed very 1 sec | |
| rtcLastLoop=rtcTic; | |
| lcdDispStatus(); | |
| } | |
| delay(50); | |
| touchSens(); | |
| 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<20; i++) { | |
| stepNumberTarget+=10; | |
| motorStep(); | |
| delay(200); | |
| } | |
| stepNumber=190; | |
| stepNumberTarget=0; | |
| motorStep(); | |
| nCumulativeStep=0; | |
| lcdDispText(0,0, "WAIT FOR"); | |
| lcdDispText(0,1, "FOLIO MV"); | |
| waitFolioMotion(); | |
| lcdDispText(0,0, "CALC ANG"); | |
| lcdDispText(0,1, "THRESHLD"); | |
| getThresh(); | |
| lcdDispText(0,0, "OK 000"); | |
| lcdDispText(0,1, "0000 000"); | |
| } | |
| void folioCycle() { | |
| digitalWrite(13, LOW); | |
| delay(1); | |
| digitalWrite(13, HIGH); | |
| rtcLastFolioMove=rtcTic; | |
| cFolio++; | |
| if (cFolio==0) { | |
| rtcTic=0; | |
| cFolio=0; | |
| stepNumber=stepNumberTarget=0; | |
| tFolioErrorMax=tFolioErrorMin=0; | |
| nCumulativeStep=0; | |
| } | |
| else if (cFolio<=0 || rtcTic<=0) return; | |
| tFolio=cFolio*15/4; //escapement 1tic=3.75sec | |
| tFolioCumulativeError=tFolio-rtcTic; | |
| lcdDispFolioTic(4,1); | |
| lcdDispNum34(4,0, tFolioCumulativeError); | |
| if (tFolioCumulativeError>tFolioErrorMax) tFolioErrorMax=tFolioCumulativeError; | |
| if (tFolioCumulativeError<tFolioErrorMin) tFolioErrorMin=tFolioCumulativeError; | |
| if (cFolio%60==0) { //every 60 turn =~4min | |
| nEscapeTurn=cFolio/15; | |
| rtcEscWheelHistory[nEscapeTurn%NHISTORY]=rtcTic; | |
| if (nEscapeTurn>=4) tFolioRateError=225-(rtcTic-rtcEscWheelHistory[(nEscapeTurn+NHISTORY-4)%NHISTORY]); //gain against 4 turns ago | |
| else tFolioRateError=0; | |
| lcdDispNum3(5,1, tFolioRateError); | |
| if (tFolioRateError*tFolioCumulativeError >=0) stepNumberTarget += (int) tFolioRateError/3 + (int) tFolioCumulativeError/4; | |
| if (stepNumberTarget != stepNumber) { | |
| motorStep(); | |
| lcdDispNum4(0,0, nCumulativeStep); | |
| } | |
| } | |
| } | |
| void touchSens() { | |
| int adata1=analogRead(A0); //touch sensor | |
| delay(1); | |
| int adata2=analogRead(A0); | |
| if (abs(adata1-adata2) > 18 || adata1+adata2 < 800) { //touch detect threshold | |
| if (!lcdEnabled) lcdOn(); | |
| lcdTimer=lcdTimerCount; | |
| } | |
| else { | |
| if (lcdEnabled) lcdTimer--; | |
| if (lcdTimer<=0) lcdOff(); | |
| } | |
| } | |
| void lcdOn() { | |
| lcdTimer=lcdTimerCount; | |
| 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) { //disp LCD even if folio not moving | |
| tFolioCumulativeError=tFolio-rtcTic; | |
| lcdDispNum34(4,0, tFolioCumulativeError); | |
| } | |
| 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%15+0xb1; //"ア"=0,... "ソ"=14 | |
| lcd.print(n); | |
| } | |
| void motorStep() { | |
| digitalWrite(DRVPWR, HIGH); | |
| delay(5); | |
| while (stepNumberTarget-stepNumber != 0) { | |
| if (stepNumberTarget>stepNumber) stepNumber++; | |
| else stepNumber--; | |
| nCumulativeStep++; | |
| #ifdef NOMOTOR | |
| Serial.print("Motor setp=");Serial.println(stepNumber); | |
| #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() { | |
| // AVE_A defaul=0 -> 4times averate | |
| 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); | |
| } | |
| 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 | |
| int diff=angle - angLast; | |
| if (diff > 1800) angle-=3600; | |
| if (diff < -1800) angle+=3600; | |
| angLast=angle; | |
| /* | |
| Serial.print(dataZ); | |
| Serial.print(" "); | |
| Serial.print(dataY); | |
| Serial.print(" "); | |
| Serial.print(angle); | |
| */ | |
| } | |
| 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; | |
| } | |
0 件のコメント:
コメントを投稿