Diff: main.cpp

Revision:

34:2c6f635cc8e7

Parent:

33:f1dc3b160eac

Child:

35:132413ec3d65

--- a/main.cpp	Mon Mar 18 18:10:10 2019 +0000
+++ b/main.cpp	Mon Mar 18 18:20:28 2019 +0000
@@ -349,6 +349,7 @@
         int8_t lead;
     
         Comm* p_comm;
+        bool _RUN;
     
         //Run the motor synchronisation
         
@@ -383,6 +384,7 @@
             theStates[0] = 0; theStates[1] = 0; theStates[2] = 0;
             
             p_comm = NULL; // null pointer for now
+            _RUN = false;
     
         }
     
@@ -405,10 +407,13 @@
             pwmCtrl.period(mtrPeriod);
             pwmCtrl.pulsewidth(mtrPeriod*dutyC);
     
+             p_comm = comm;
+            _RUN = true;
+
             // Start motor control thread
             t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));
     
-            p_comm = comm;
+
         }
     
             //Set a given drive state
@@ -473,9 +478,92 @@
         void motorCtrlFn() {
             Ticker motorCtrlTicker;
             motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
-            while (1) {
+
+            // Init some things
+            uint8_t cpyStateCount[3]; 
+            uint8_t cpyCurrentState; 
+
+            int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
+            uint8_t iterElementMax;
+            int16_t totalDegrees;
+            int16_t stateDiff;
+
+            int32_t velocity;                                   //Variable for local velocity calculation
+            int32_t locMotorPos;                                //Local copy of motor position
+            static int32_t oldMotorPos = 0;                     //Old motor position used for calculations
+            static uint8_t motorCtrlCounter = 0;                //Counter to be reset every 10 iterations to get velocity calculation in seconds
+            int32_t torque;                                     //Local variable to set motor torque
+            float sError;                                       //Velocity error between target and reality
+            float rError;                                       //Rotation error between target and reality
+            static float rErrorOld;                             //Old rotation error used for calculation
+
+            //~~~Controller constants~~~~
+            int32_t Kp1=22;                                     //Proportional controller constants 
+            int32_t Kp2=22;                                     //Calculated by trial and error to give optimal accuracy  
+            float   Kd=15.5;    
+            
+
+            while (_RUN) {
                 t_motor_ctrl.signal_wait((int32_t)0x1);
-                p_comm->pc.printf("B4115"); 
+                core_util_critical_section_enter();
+                //Access shared variables here
+                std::copy(stateCount, stateCount+3, cpyStateCount);  
+                // TODO: A thing yes
+                cpyCurrentState = currentState;
+                for (int i = 0; i < 3; ++i) {
+                    stateCount[i] = 0; 
+                }
+                core_util_critical_section_exit();
+
+                iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; 
+
+               
+                totalDegrees = ting[0] * cpyStateCount[iterElementMax];
+                stateDiff = theStates[iterElementMax]-cpyCurrentState;
+                if (stateDiff >= 0) {
+                    totalDegrees = totalDegrees + (ting[1]* stateDiff);  
+                } 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));
+            
+                /*
+                //~~~~~Speed controller~~~~~~
+                velocity = totalDegrees*10;
+                sError = (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
+                } else {
+                    Ys = (int)(Kp1 * sError);                    //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
+                }                                                //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. 
+                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
+                if(rError < 0){                                  //Use the sign of the error to set controller wrt direction of rotation
+                    Ys = -Ys;                               
+                }
+
+                if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){        //Choose Ys or Yr based on distance from target value so that it takes 
+                    torque = Ys;                                            //appropriate steps in the right direction to reach target value
+                } else {
+                    torque = Yr;
+                }
+                if(torque < 0){                                             //Variable torque cannot be negative since it sets the PWM  
+                    torque = -torque;                                       //Hence we make the value positive, 
+                    lead = -2;                                              //and instead set the direction to the opposite one
+                } else {
+                    lead = 2;
+                }
+                if(torque > MAXPWM){                                        //In case the calculated PWM is higher than our maximum 50% allowance,
+                    torque = MAXPWM;                                        //Set it to our max.
+                }   
+
+                motorPower = torque;                                        //Lastly, update global variable motorPower which is updated by interrupt        
+                */
             }
         }