Revision 25:995865498aee, committed 2019-03-16

Comitter:

adehadd

Date:

Sat Mar 16 17:39:22 2019 +0000

Parent:

24:be5fef3dace1

Child:

26:fb6151e5907d

Commit message:

Lab 3 up to controlling motor speed

Changed in this revision

--- a/main.cpp	Sat Mar 16 16:43:47 2019 +0000
+++ b/main.cpp	Sat Mar 16 17:39:22 2019 +0000
@@ -276,7 +276,7 @@
             core_util_critical_section_enter();
             //Access shared variables here
             std::copy(stateCount, stateCount+3, cpyStateCount);  
-            // TODO: A thing
+            // TODO: A thing yes
             cpyCurrentState = 0;
             for (int i = 0; i < 3; ++i) {
                 stateCount[i] = 0; 
@@ -285,15 +285,28 @@
 
             uint8_t iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; 
 
-            int16_t totalDegrees = 360 * cpyStateCount[iterElementMax];
+            int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
+            int16_t totalDegrees = ting[0] * cpyStateCount[iterElementMax];
             int16_t stateDiff = theStates[iterElementMax]-cpyCurrentState;
             if (stateDiff >= 0) {
-                totalDegrees = totalDegrees + (60* stateDiff);  
+                totalDegrees = totalDegrees + (ting[1]* stateDiff);  
             } else {
-                totalDegrees = totalDegrees + (60*stateDiff*-1); 
+                totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); 
             }
             pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
+        
+        //~~~~~Speed controller~~~~~~
+        /*
+        sError = (targetVel * 6) - abs(totalDegrees)*10;        //Read global variable targetVel updated by interrupt and calculate error between target and reality
+        int32_t Ys;                                      //Initialise controller output Ys  
+        if (sError == -abs(velocity)) {                  //Check if user entered V0, 
+            Ys = MAXPWM;                                 //and set the output to maximum as specified
+        } else {
+            Ys = (int)(Kp1 * sError);                    //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
+        } 
+        */
         }
+        
     }
 
     void motorCtrlTick(){