Diff: main.cpp

Revision:

20:a435105305fe

Parent:

19:526fd700e1b3

Child:

21:0d1025dc6433

--- a/main.cpp	Tue Mar 20 15:10:08 2018 +0000
+++ b/main.cpp	Tue Mar 20 16:51:38 2018 +0000
@@ -53,8 +53,8 @@
 
 
 enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK,
-            MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR,
-            MSG_TEST, MSG_TORQUE};
+            MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, MSG_TORQUE,
+            MSG_TEST, MSG_CUR_SPD};
 
 //Instantiate the serial port
 RawSerial pc(SERIAL_TX, SERIAL_RX);
@@ -77,9 +77,10 @@
 
 
 
+
 typedef struct {
-     uint8_t code;
-     int32_t data;
+    uint8_t code;
+    int32_t data;
 } message_t ;
 
 Mail<message_t,16> outMessages;
@@ -92,7 +93,7 @@
     outMessages.put(pMessage);
 }
 
-Thread commOutT;
+Thread commOutT (osPriorityNormal, 512);
 
 void commOutFn() {
     while(1) {
@@ -121,15 +122,52 @@
     uint8_t newChar = pc.getc();
     inCharQ.put((void*)newChar);
 }
+Thread motorCtrlT (osPriorityNormal, 2048);
+
+int8_t readRotorState();
+void motorOut(int8_t,uint32_t);
+volatile int32_t motorPosition;
+
+void photointerrupter_isr() {
+    static int8_t oldRotorState = 0;
+    int8_t rotorState = readRotorState();
+    motorOut((rotorState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive
+    if (rotorState - oldRotorState == 5) motorPosition--;
+    else if (rotorState - oldRotorState == -5) motorPosition++;
+    else motorPosition += (rotorState - oldRotorState);
+    oldRotorState = rotorState;
+}
+
+//TODO Implement timer for greater accuracy
+void motorCtrlTick(){
+    motorCtrlT.signal_set(0x1);
+}
+
+float currentSpeed = 0;
 
 
-void photointerrupter_isr()
-{
-    int8_t intState = readRotorState();
-    motorOut((intState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive
+void motorCtrlFn(){
+    int32_t posStart, posEnd;
+    Ticker motorCtrlTicker;
+    motorCtrlTicker.attach_us(&motorCtrlTick,100000);
+    // putMessage(MSG_TEST, 0x505);
+    uint8_t cnt = 0;
+
+    while(1){
+        posStart = motorPosition;
+        motorCtrlT.signal_wait(0x1);
+        posEnd = motorPosition;
+        currentSpeed = (posEnd-posStart)*10;
+
+        if(cnt >= 9) {
+            // putMessage(MSG_CUR_SPD, currentSpeed);
+            cnt = 0;
+        }
+        else{ cnt++; }
+    }
 }
 
-Thread decodeT;
+
 
 void setNewCmd(char s[CHAR_ARR_SIZE])
 {
@@ -143,27 +181,32 @@
             rotations_pending_mutex.unlock();
             putMessage(MSG_ROT_PEN, rotations_pending);
         //V
-        } else if (sscanf(s, "V%f", &maxspeed_)) {
+        }
+        else if (sscanf(s, "V%f", &maxspeed_)) {
             maxspeed_mutex.lock();
             maxspeed = maxspeed_;
             maxspeed_mutex.unlock();
             putMessage(MSG_MAX_SPD, maxspeed);
         //K
-        } else if (sscanf(s, "K%llx", &newKey_)) {
+        }
+        else if (sscanf(s, "K%llx", &newKey_)) {
             newKey_mutex.lock();
             newKey = newKey_;
             newKey_mutex.unlock();
             putMessage(MSG_NEW_KEY, newKey);
-        } else if (sscanf(s, "T%u", &torque_)) {
+        }
+        else if (sscanf(s, "T%u", &torque_)) {
             torque = torque_;
             photointerrupter_isr(); //Give it a kick
             putMessage(MSG_TORQUE, torque);
         //ERROR
-        } else
-                putMessage(MSG_INP_ERR, 0x404);
+        }
+        else{
+            putMessage(MSG_INP_ERR, 0x404);
+        }
 }
 
-
+Thread decodeT (osPriorityNormal, 512);
 
 void decodeFn() {
     pc.attach(&serialISR);
@@ -193,10 +236,6 @@
     }
 }
 
-
-
-
-
 volatile uint16_t hashcount = 0;
 
 void do_hashcount() {
@@ -250,16 +289,15 @@
 
     putMessage(MSG_RESET, 0);
 
+    commOutT.start(&commOutFn);
 
-    commOutT.start(&commOutFn);
+    motorCtrlT.start(&motorCtrlFn);
 
     decodeT.start(&decodeFn);
 
-//    pc.printf("Hello\n\r");
-
     //Run the motor synchronisation
     orState = motorHome();
-//    pc.printf("Rotor origin: %x\n\r",orState);
+
     //orState is subtracted from future rotor state inputs to align rotor and motor states
 
     I1.rise(&photointerrupter_isr);
@@ -302,9 +340,6 @@
 
         *key = newKey;
 
-
-
-
         sha256.computeHash(hash, sequence, 64);
 
         if (hash[0] == 0 && hash[1] == 0) {