Revision 28:613a88b88074, committed 2019-03-22

Comitter:

Olaffo

Date:

Fri Mar 22 19:13:05 2019 +0000

Parent:

27:71914f339d6b

Child:

29:cb4db90cfd63

Commit message:

attempt at CHA CHB control

Changed in this revision

--- 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);