Revision 24:be5fef3dace1, committed 2019-03-16

Comitter:

iachinweze1

Date:

Sat Mar 16 16:43:47 2019 +0000

Parent:

23:ab1cb51527d1

Child:

25:995865498aee

Child:

28:4f02ac845e5d

Commit message:

commitment frightens me

Changed in this revision

--- a/main.cpp	Sat Mar 16 14:06:41 2019 +0000
+++ b/main.cpp	Sat Mar 16 16:43:47 2019 +0000
@@ -72,39 +72,9 @@
 DigitalOut L3H(L3Hpin);
 
 PwmOut pwmCtrl(PWMpin);
-/*//Declare and start threads
-class T_{
 
-    // protected:
-    public:
-
-        Thread *p_comm_in;
-        Thread *p_comm_out;
-        Thread *p_motor_ctrl;
-
-
-
-        T_(){
-            //(priority, stack size,
-            Thread comm_in(osPriorityAboveNormal, 1024);
-            Thread comm_out(osPriorityAboveNormal, 1024);
-            Thread motor_ctrl(osPriorityAboveNormal, 1024);
-
-            p_comm_in = &comm_in;
-            p_comm_out = &comm_out;
-            p_motor_ctrl = &motor_ctrl;
-
-        }
-
-        ~T_(){
-            if (p_comm_in->get_state() == 2)
-                p_comm_in->terminate();
-            if (p_comm_out->get_state() == 2)
-                p_comm_out->terminate();
-            if (p_motor_ctrl->get_state() == 2)
-                p_motor_ctrl->terminate();
-        }
-};*/
+uint8_t stateCount[3];   
+uint8_t theStates[3];
 
 class Comm /*: public T_*/{
 
@@ -299,9 +269,30 @@
     void motorCtrlFn() {
         Ticker motorCtrlTicker;
         motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5);
-        while (1) {
+        uint8_t cpyStateCount[3]; 
+        uint8_t cpyCurrentState; 
+        while (_RUN) {
             t_motor_ctrl.signal_wait((int32_t)0x1);
-            pc.printf("B4115"); 
+            core_util_critical_section_enter();
+            //Access shared variables here
+            std::copy(stateCount, stateCount+3, cpyStateCount);  
+            // TODO: A thing
+            cpyCurrentState = 0;
+            for (int i = 0; i < 3; ++i) {
+                stateCount[i] = 0; 
+            }
+            core_util_critical_section_exit();
+
+            uint8_t iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; 
+
+            int16_t totalDegrees = 360 * cpyStateCount[iterElementMax];
+            int16_t stateDiff = theStates[iterElementMax]-cpyCurrentState;
+            if (stateDiff >= 0) {
+                totalDegrees = totalDegrees + (60* stateDiff);  
+            } else {
+                totalDegrees = totalDegrees + (60*stateDiff*-1); 
+            }
+            pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
         }
     }
 
@@ -320,7 +311,7 @@
 
     Comm() :  pc(SERIAL_TX, SERIAL_RX),
               t_comm_out(osPriorityAboveNormal, 1024),
-              t_motor_ctrl(osPriorityAboveNormal, 1024)
+              t_motor_ctrl(osPriorityAboveNormal2, 1024)
     { // inherit from the RawSerial constructor
 
         pc.printf("%s\n\r", "Welcome" );
@@ -455,6 +446,18 @@
     int8_t currentState = *params[0];
     int8_t offset = *params[1];
 
+    switch (currentState) {
+        case 1:
+            stateCount[0]++; 
+            break; 
+        case (1 + lead): 
+            stateCount[1]++; 
+            break; 
+        case (1 + (lead*2)):
+            stateCount[2]++; 
+            break; 
+    }
+
     motorOut((currentState - offset + lead + 6) % 6);
 }
 
@@ -497,6 +500,10 @@
     //Run the motor synchronisation
     orState = motorHome();
 
+    theStates[0] = orState;
+    theStates[1] = (orState + lead) % 6;
+    theStates[2] = (orState + (lead*2)) % 6;
+
     // Add callbacks
     // I1.fall(&stateUpdate);
     // I2.fall(&stateUpdate);
@@ -567,5 +574,4 @@
             timer.reset();
         }
     }
-
 }
\ No newline at end of file