![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is probably never gonna get done
Diff: main.cpp
- Revision:
- 28:613a88b88074
- Parent:
- 27:71914f339d6b
- Child:
- 29:cb4db90cfd63
--- a/main.cpp Fri Mar 22 12:24:17 2019 +0000 +++ b/main.cpp Fri Mar 22 19:13:05 2019 +0000 @@ -54,6 +54,12 @@ InterruptIn I2(I2pin); InterruptIn I3(I3pin); +//Fine interrupts +InterruptIn CHA(CHApin); +InterruptIn CHB(CHBpin); +uint8_t attached = 0; //interrupt state tracker +DigitalOut IDC(Indicator); + //Motor Drive outputs DigitalOut L1L(L1Lpin); DigitalOut L1H(L1Hpin); @@ -69,6 +75,7 @@ int32_t revoCounter = 0; //Counts the number of revolutions int32_t motorVelocity; uint8_t mode = 6; + //Phase lead to make motor spin //int8_t lead = -2; //2 for forwards, -2 for backwards @@ -78,7 +85,7 @@ float fdata; } mail_t; -Mail<mail_t, 16> mail_box; +Mail<mail_t, 1024> mail_box; Thread commsIn(osPriorityNormal,1024); Thread bitcointhread(osPriorityNormal,1024); Thread motorCtrlT(osPriorityNormal,1024); @@ -96,10 +103,49 @@ float motorPosition; //float pulseWidth; +uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges; +uint32_t encState,totalEncCount; +uint32_t encArray[1024]; +int encIndex = 0; + void serialISR() { uint8_t newChar = pc.getc(); inCharQ.put((void*) newChar); } + + +/***************************************************************/ +//Fine control interrupt functions + +void encISR0() { + if (encState == 3) {encCount++;IDC = !IDC;} + else badEdges++; + encState = 0; + } + +// B Rise +void encISR1() { + if (encState == 0) {encCount++;IDC = !IDC;} + else badEdges++; + encState = 1; + } + +// A Fall +void encISR2() { + if (encState == 1) {encCount++;IDC = !IDC;} + else badEdges++; + encState = 2; + } + +// B Fall +void encISR3() { + if (encState == 2) {encCount++;IDC = !IDC;} + else badEdges++; + encState = 3; + } +//End of block 2 +/***************************************************************/ + /***************************************************************/ //The following block should be moved to a library, but I don't the time to figure this out atm. @@ -115,6 +161,9 @@ STANDARD = 6, FOREVER_FORWARD = 7, LEAD = 9, +FINE_TUNE = 21, +T_ENC = 23, +ENC = 24, INVALID_CMD = 10, @@ -125,7 +174,9 @@ NONCE_DETECT = 14, ROTOR_ORG = 15, -WELCOME = 20 +WELCOME = 20, + +GIVE_ENC = 100 }; @@ -139,6 +190,8 @@ } void commsOutFunc() { + int i; + while (true) { osEvent evt = mail_box.get(); if (evt.status == osEventMail) { @@ -180,16 +233,33 @@ break; case ROTOR_ORG : pc.printf("Rotor origin: %x\n\r", orState); + break; case LEAD : pc.printf("Lead: %d\n\r\n\r", mail->data); break; + case FINE_TUNE : + pc.printf("Interrupts: %d\n\r\n\r", attached); + break; + case T_ENC : + pc.printf("totalEncCount: %d\n\r", mail->data); + break; + case ENC : + pc.printf("EncCount: %d\n\r", mail->data); + break; + case GIVE_ENC : + + for (i=0; i<encIndex; i++ ){ + pc.printf("%d\n\r", encArray[i]); + } + //pc.printf("%d\n\r", encIndex); + break; } mail_box.free(mail); } } } -//End of that block +//End of block 2 /***************************************************************/ /***************************************************************/ @@ -325,6 +395,9 @@ else if (command[0] == 'T') { putMessage(TUNE_CMD); } + else if (command[0] == 'G') { + putMessage(GIVE_ENC); + } memset(command, 0, sizeof(command)); index = 0; } else { @@ -332,6 +405,8 @@ } } } +//End of block 3 +/***************************************************************/ void bitcoin(){ while(1) { @@ -428,6 +503,17 @@ else motorPosition += (rotorState - oldRotorState); //pc.printf ("motorpPosition %f\n\r", motorPosition); oldRotorState = rotorState; + + //putMessage(ENC,encCount); + + if (attached == 1){ + encCount = 0; + attached++; + } + + encArray[encIndex] = encCount; + encIndex++; + } /*void push() { intState = readRotorState(); @@ -450,10 +536,8 @@ float olderror = 0; float Speed; float Rev; - int8_t leads = -2; - int8_t leadr = -2; float kps = 27; - float kis = 0.2; + float kis = 0.4; float kpr = 35; float kdr = 14.75; //proportional constant for speed // Timer to count time passed between ticks to calculate velocity @@ -461,6 +545,11 @@ motorTime.start(); float motorPos; float myTime; + + CHA.rise(&encISR0); + CHB.fall(&encISR3); + CHB.rise(&encISR1); + CHA.fall(&encISR2); Ticker motorCtrlTicker; motorCtrlTicker.attach_us(&motorCtrlTick,100000); @@ -477,13 +566,32 @@ error = Rev + motorPosition_command - motorPos; oldmotorPosition = motorPos; + if ((motorVelocity < 40) && (attached == 0)){ + CHA.rise(&encISR0); + CHB.fall(&encISR3); + CHB.rise(&encISR1); + CHA.fall(&encISR2); + + attached = 1; + } + else if ((motorVelocity >= 60) && (attached > 0)){ + CHA.rise(NULL); + CHB.fall(NULL); + CHB.rise(NULL); + CHA.fall(NULL); + + attached = 0; + + } + + //equation for controls sError = Speed -abs(motorVelocity); - if ((motorVelocity != 0) && (abs(sError)<560)){ - intError = intError + sError; - if (abs(intError*kis)>2000) { - intError = intError - sError; - } + if ((motorVelocity != 0) && (abs(sError)<25)){ + intError = intError + sError; + } + if (abs(intError*kis)>2000) { + intError = intError - sError; } Ts = (kps * sError + kis * intError)*(error/abs(error)); Tr = kpr*error+kdr*(error-olderror)/ myTime; @@ -564,7 +672,7 @@ // ONLY speed control (aka forever forward mode) - else if (mode = FOREVER_FORWARD){ + else if (mode == FOREVER_FORWARD){ if ( Ts >= 0) { lead = -2; } else if ( Ts < 0) { lead = 2; } @@ -585,7 +693,9 @@ //display velocity and motor position putMessage(MOTOR_POS,0,(float)(motorPosition/6.0)); putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0)); - putMessage(LEAD,lead); + //putMessage(ENC,encCount); + //putMessage(LEAD,lead); + putMessage(FINE_TUNE); } olderror=error; } @@ -598,6 +708,8 @@ int8_t orState = motorHome(); putMessage(WELCOME); putMessage(ROTOR_ORG); + + IDC = 1; // Start all threads commsIn.start(commandProcessor);