Revision 35:132413ec3d65, committed 2019-03-18

Comitter:

CallumAlder

Date:

Mon Mar 18 19:22:27 2019 +0000

Parent:

34:2c6f635cc8e7

Child:

36:d3ad69448670

Child:

37:71adabab284a

Commit message:

PID implemented, but pretty broke?

Changed in this revision

--- a/main.cpp	Mon Mar 18 18:20:28 2019 +0000
+++ b/main.cpp	Mon Mar 18 19:22:27 2019 +0000
@@ -8,7 +8,7 @@
 Indx
 newCmd
 MAXCMDLENGTH
-move the global variables to a class because we arent paeasents
+move the global variables to a class because we arent paeasents - Mission Failed
 */
 
 //Photointerrupter input pins
@@ -191,7 +191,7 @@
                 //Case switch to choose serial output based on incoming message
                 switch (pMessage->type) {
                     case motorState:
-                        pc.printf("The motor is currently in state %x\n\r", pMessage->message);
+                        pc.printf("\r>%s< The motor is currently in state %x\n\r", inCharQ, pMessage->message);
                         break;
                     case hashRate:
                         pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message);
@@ -202,24 +202,24 @@
                         returnCursor();
                         break;
                     case keyAdded:
-                        pc.printf("New Key Added:\t0x%016x\n\r", pMessage->message);
+                        pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", inCharQ, pMessage->message);
                         break;
                     case torque:
-                        pc.printf("Motor Torque set to:\t%d\n\r", pMessage->message);
+                        pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, pMessage->message);
                         break;
                     case velIn:
-                        pc.printf("Target Velocity set to:\t%.2f\n\r", targetVel);
+                        pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, targetVel);
                         break;
                     case velOut:
-                        pc.printf("Current Velocity:\t%.2f\n\r", \
+                        pc.printf("\r>%s< Current Velocity:\t%.2f\n\r", inCharQ, \
                                 (float) ((int32_t) pMessage->message / 6));
                         break;
                     case posIn:
-                        pc.printf("Target Rotation set to:\t%.2f\n\r", \
+                        pc.printf("\r>%s< Target Rotation set to:\t%.2f\n\r", inCharQ, \
                                 (float) ((int32_t) pMessage->message / 6));
                         break;
                     case posOut:
-                        pc.printf("Current Position:\t%.2f\n\r", \
+                        pc.printf("\r>%s< Current Position:\t%.2f\n\r", inCharQ, \
                                 (float) ((int32_t) pMessage->message / 6));
                         break;
                     case error:
@@ -228,7 +228,7 @@
                             inCharQ[i] = ' ';
                         break;
                     default:
-                        pc.printf("Unknown Error. Message: %x\n\r", pMessage->message);
+                        pc.printf("\r>%s< Unknown Error. Message: %x\n\r", inCharQ, pMessage->message);
                         break;
                 }
                 mailStack.free(pMessage);
@@ -272,8 +272,6 @@
             inCharQIdx = 0;
             // inCharQIdx = MAXCMDLENGTH-1;
     
-    
-    
             pc.attach(callback(this, &Comm::serialISR));
     
             // Thread t_comm_in(osPriorityAboveNormal, 1024);
@@ -359,6 +357,8 @@
         uint8_t theStates[3];   // The Key states
     
         Thread t_motor_ctrl;    // Thread for motor Control
+        
+        uint32_t MAXPWM;
     
     public:
     
@@ -373,18 +373,18 @@
             orState = motorHome();             //Rotot offset at motor state 0
             currentState = readRotorState();   //Current Rotor State
             // stateList[6] = {0,0,0, 0,0,0};     //All possible rotor states stored
+            lead = 2;  //2 for forwards, -2 for backwards
     
             theStates[0] = orState;
             theStates[1] = (orState + lead) % 6;
             theStates[2] = (orState + (lead*2)) % 6;
-    
-            lead = 2;  //2 for forwards, -2 for backwards
-           
+        
             stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0;
-            theStates[0] = 0; theStates[1] = 0; theStates[2] = 0;
             
             p_comm = NULL; // null pointer for now
             _RUN = false;
+            
+            MAXPWM = mtrPeriod*dutyC;
     
         }
     
@@ -412,6 +412,8 @@
 
             // Start motor control thread
             t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));
+
+            MAXPWM = mtrPeriod*dutyC;
     
 
         }
@@ -525,12 +527,11 @@
                 } else {
                     totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); 
                 }
-                p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
+                //p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
             
-                /*
                 //~~~~~Speed controller~~~~~~
                 velocity = totalDegrees*10;
-                sError = (targetVel * 6) - abs(velocity);        //Read global variable targetVel updated by interrupt and calculate error between target and reality
+                sError = (p_comm->targetVel * 6) - abs(velocity);        //Read global variable targetVel updated by interrupt and calculate error between target and reality
                 int32_t Ys;                                      //Initialise controller output Ys  (s=speed)
                 if (sError == -abs(velocity)) {                  //Check if user entered V0, 
                     Ys = MAXPWM;                                 //and set the output to maximum as specified
@@ -539,7 +540,7 @@
                 }                                                //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
                 
                 //~~~~~Rotation control~~~~~~
-                rError = targetRot - (locMotorPos/6);            //Read global variable targetRot updated by interrupt and calculate the rotation error. 
+                rError = p_comm->targetRot - cpyCurrentState;            //Read global variable targetRot updated by interrupt and calculate the rotation error. 
                 int32_t Yr;                                      //Initialise controller output Yr (r=rotations)
                 Yr = Kp2*rError + Kd*(rError - rErrorOld);       //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)        
                 rErrorOld = rError;                              //Update rotation error
@@ -562,8 +563,9 @@
                     torque = MAXPWM;                                        //Set it to our max.
                 }   
 
-                motorPower = torque;                                        //Lastly, update global variable motorPower which is updated by interrupt        
-                */
+                pwmCtrl.pulsewidth(torque);
+                p_comm->motorPower = torque;                                        //Lastly, update global variable motorPower which is updated by interrupt        
+
             }
         }