Diff: main.cpp

Revision:

3:4f215646a42b

Parent:

2:da51e13f4ddf

Child:

4:d04afc466198

--- a/main.cpp	Sun Aug 08 12:29:22 2021 +0000
+++ b/main.cpp	Sun Aug 08 14:47:14 2021 +0000
@@ -7,13 +7,15 @@
 #include "platform/mbed_thread.h"
 #include "Settings.h"
 #include "QEI.h"
+#include "math.h"
 //#include "LIS3DH.h"
 
 /*
 Open issues:
 - need to figure out how to handle transitions to error states
 - Need to test how breaking is performed. 
--Test onboard current limitation
+- ensure that motor speed is truely zero before exiting.
+- Test onboard current limitation
 */
 
 Timer encoderTimer;
@@ -56,13 +58,17 @@
 
 //global variables
 double speedFilterConstant;
-char state;
+char _state;
+char _errorId = 0x00;
+bool _isErrorMsg = 0;
+
+
 double currentPulses;
 double lastPulses;
 double lastSpeedRPM;
 double Tnow;
-double demandSpeed_RPM = 0.0;
-double currentSpeedRPM;
+double _demandSpeed_RPM = 0.0;
+double _currentSpeed_RPM;
 double deltaError;
 double lastError;
 double integralTerm;
@@ -77,7 +83,7 @@
 Semaphore semStartTest(0);
 Semaphore semButton(0);
 
-//Mutex mut1;
+Mutex mut1;
 
 double LimitDouble(double input, double min, double max){
     double output;
@@ -124,141 +130,236 @@
     semPrint.release();
 }
 
-void PrintTimeRemaining(){
+void PrintMessages(){
     while(1){
         semPrint.wait();
-        if(state == STATE_RUNNING){    
+        
+        mut1.lock();//lock mutex to prevent race condition.
+        double localDemandSpeed = _demandSpeed_RPM;
+        double localCurrentSpeed = _currentSpeed_RPM;
+        char localErrorId = _errorId;
+        bool localIsErrorMsg = _isErrorMsg;
+        char localState = _state;
+        mut1.unlock();
+        
+        if(localState == STATE_RUNNING){    
             double timeElapsed = testTimer.read();
             double timeRemaining = SPIN_T[5] - timeElapsed;
             int displayTime = int(timeRemaining)+1;
             //printf("Time remaining %d s\r\n", displayTime);
-//            mut1.lock();
-//            double printDemandSpeed = demandSpeed_RPM;
-//            double printCurrentSpeed = currentSpeedRPM;
-//            mut1.unlock();//this is the first change
-            printf("%f\t%f\r\n",demandSpeed_RPM, currentSpeedRPM);
+
+            printf("%f\t%f\r\n",localDemandSpeed, localCurrentSpeed);
         }
+        if(localIsErrorMsg){
+            localIsErrorMsg = 0;//clear local error message flag.
+            mut1.lock();
+            _isErrorMsg = 0;//clear global error message flag.
+            mut1.unlock();
+            
+            switch (localErrorId){
+                case ERROR_ACCEL:
+                    printf("Excess vibration detected\r\n");
+                    break;
+                case ERROR_MAN_STOP:
+                    printf("Manual stop detected\r\n");
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+}
+
+int CheckAccelerometer(){
+    //read accelerometer
+    double xAccel;
+    double yAccel;
+    
+    double magnitude = xAccel*xAccel + yAccel*yAccel;
+    magnitude = sqrt(magnitude);
+    
+    if (magnitude > VIBRATION_THRESHOLD){
+        return 1;
+    }
+    else{
+        return 0;
     }
 }
 
 void MotorControl(){
     while(1){
         semMotorControl.wait();//wait for a signal
-        if(state == STATE_RUNNING){//need to check if this is the best condition to look for.
-            
-            Tnow = testTimer.read();
-            //check accel. If problem, change state to ERROR
-            
-            //int deltaT = encoderTimer.read();//read current time in seconds
-            
+        //grab global variables
+        mut1.lock();//lock mutex to prevent race condition.
+        char localErrorId = _errorId;
+        bool localIsErrorMsg = _isErrorMsg;
+        char localState = _state;
+        mut1.unlock();
+        
+        Tnow = testTimer.read();
+        //check accel. If problem, change state to ERROR
+        if(CheckAccelerometer()){
+            mut1.lock();
+            _state= STATE_HALT;
+            _errorId = ERROR_ACCEL;
+            _isErrorMsg = 1;//set flag for sending an error message
+            mut1.unlock();
+        }
+        //int deltaT = encoderTimer.read();//read current time in seconds //don't need this????
+        static double demandSpeed_RPM;
+        
+        if(localState == STATE_RUNNING){//need to check if this is the best condition to look for.
             //calculate current demand
-            if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
+            /*if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
                 demandSpeed_RPM = 0.0;
-                //printf("warm up %f\r\n", demandSpeed_RPM);
-                //deactivate motor?
-            }
+            }*/
             if ((SPIN_T[1] <= Tnow)&&(Tnow < SPIN_T[2])){//test is in ramp up
-            
-                double a = Tnow - SPIN_T[1];
-                demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP;
-                //printf("ramp up %f\r\n", demandSpeed_RPM);
+                //double a = Tnow - SPIN_T[1];
+                //demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP;
+                demandSpeed_RPM += targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
             }
-            if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
+            else if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
                 demandSpeed_RPM = targetSpeed_RPM;
-                //printf("coast %f\r\n", demandSpeed_RPM);
+            }
+            else if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down
+                //double a = Tnow - SPIN_T[3];
+                //demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM);
+                demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
             }
-            if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down
-                double a = Tnow - SPIN_T[3];
-                demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM);
-                //printf("ramp down %f\r\n", demandSpeed_RPM);
+            else {//((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
+                demandSpeed_RPM = 0.0;
             }
-            if ((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
-                demandSpeed_RPM = 0.0;
-                //printf("cool down %f\r\n", demandSpeed_RPM);
-                //deactivate motor?
+        }
+        if(localState == STATE_HALT){//if halt condition is set
+            if(demandSpeed_RPM > 0.0){
+                demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
+            }
+            else{
+                mut1.lock();
+                _state = STATE_ERROR;//once demand speed is 0, go to error state.
+                mut1.unlock();
             }
-            
-            demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
-            currentPulses = encoder.getPulses();//calculate current speed
-            double deltaPulses = currentPulses - lastPulses;
-            double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
-            speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
-            
-            currentSpeedRPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
-            
-            double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
-            deltaError = error - lastError;
-            double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
-            integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
-            integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
-            
-            output = Kp * error; //calculate output
-            output += integralTerm;
-            output += Kd*errorDot;
-            output = LimitDouble(output,-1.0,1.0);
-            //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output);
+        }
+        demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
+        currentPulses = encoder.getPulses();//calculate current speed
+        double deltaPulses = currentPulses - lastPulses;
+        double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
+        speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
+        double currentSpeed_RPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
+        
+        mut1.lock();//update global variables 
+        _demandSpeed_RPM = demandSpeedRPM;
+        _currentSpeed_RPM = currentSpeed_RPM;
+        mut1.unlock();//end of global variable write
+        
+        double error = demandSpeed_RPM - currentSpeed_RPM;//calculate error
+        deltaError = error - lastError;
+        double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
+        integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
+        integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
+        
+        output = Kp * error; //calculate output
+        output += integralTerm;
+        output += Kd*errorDot;
+        output = LimitDouble(output,-1.0,1.0);
+        //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeed_RPM, error, output);
+    
+        if(output >=0){//Set direction
+            PHA.write(0);
+        }
+        else{
+            PHA.write(1);
+            output = -1*output;
+        }
+        PWMA.write(output);//write to motor
+        
+        lastPulses = currentPulses;//update variables
+        lastError = error;
+        lastSpeedRPM = currentSpeed_RPM;
+        
         
-            if(output >=0){//Set direction
-                PHA.write(0);
-            }else{
-                PHA.write(1);
-                output = -1*output;
-            }
-            PWMA.write(output);//write to motor
-            
-            lastPulses = currentPulses;//update 
-            lastError = error;
-            lastSpeedRPM = currentSpeedRPM;
-            
-            //exit when test has completed
-            if (Tnow >= SPIN_T[5]){
-                printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
-                state = STATE_READY;//change state
+        //exit when test has completed
+        if ((Tnow >= SPIN_T[5]) || (localState == STATE_ERROR)){ //either if test time has expired, or if system has gone into an error state
+            if(deltaPulses == 0){//check if motor has come to a complete stop
+                tickerPrint.detach();//stop the print thread from firing
+                PrintThread.terminate();//terminate print thread???
+                EN_FAULTA.write(0);//disable motor
+                tickerMotorControl.detach(); //detach the semaphore release for motor control
+                
+                //ensure rotor has come to a complete stop.
+                int deltaPulses;
+                deltaPulses = encoder.getPulses();//calculate current change in pulses
+                while(deltaPulses>0){//loop forever until system has come to complete stop
+                    deltaPulses = encoder.getPulses();//calculate current speed
+                    encoder.reset();//reset encoder count
+                    ThisThread::sleep_for(10);
+                }
+                //Inform user why test has ended.
+                if(localState == !STATE_ERROR){
+                    printf("Test complete\r\n");//Test completed cleanly
+                }
+                else{
+                    switch (errorId){
+                        case ERROR_ACCEL://accelerometer detected excess vibration
+                            printf("Test terminated.\r\n Please check holder and restart system.\r\n");
+                            break;
+                        case ERROR_MAN_STOP://test was terminated manually
+                            printf("Test terminated manually. \r\nPlease restart system to resume testing.\r\n");
+                            break;
+                        default:
+                            break;
+                    }
+                }
+                mut1.lock()
+                _state = STATE_READY;//change state
+                mut1.unlock();
+                demandSpeed_RPM = 0.0;//update variables
+                lastPulses = 0.0;
+                lastError = 0.0;
+                lastSpeedRPM = 0.0;
                 testTimer.stop(); //stop and reset timers
                 testTimer.reset();
                 Tnow = testTimer.read();
-                encoderTimer.stop();
-                encoderTimer.reset();
-                EN_FAULTA.write(0);//disable motor
+                encoder.reset();//reset encoder
+                //encoderTimer.stop();
+                //encoderTimer.reset();
                 
-                tickerMotorControl.detach(); //detach the semaphore release for motor control
-                tickerPrint.detach();
-                
+    
                 printf("state = %d\r\n",state);
                 TestCompleteNotification();//send notification
                 //deactivate motor
-                //PrintThread.terminate();//terminate print thread
                 //CentrifugeTestThread.terminate();//terminate threads
             }
         }
     }
 }
 
-void CentrifugeTest(){
-    while(1){    
-        semStartTest.wait();
+void CentrifugeTestInit(){
+//    while(1){    
+//        semStartTest.wait();
         printf("\r\n Test starting \r\n");
-        state = STATE_RUNNING;
+        mut1.lock();
+        _state = STATE_RUNNING;
+        _errorId = 0x00;
+        mut1.unlock();
         //set up test
         testTimer.reset();
         testTimer.start();//start timer
         
-        char spinState;
+        //char spinState;
         
         //set up ticker to allow motor control thread to run periodically
-        
         encoder.reset();//reset encoder
         lastPulses = 0;//reset previous encoder reading
-        encoderTimer.start();
-        PrintThread.start(PrintTimeRemaining);
+        PrintThread.start(PrintMessages);
         printf("\r\n Test setup complete \r\n");
-        EN_FAULTA.write(1);
-        if(state == STATE_RUNNING){
-            printf("\r\n running %d\r\n",state);
-        }
+        EN_FAULTA.write(1);//enable motor
+        //encoderTimer.start();
+//        if(state == STATE_RUNNING){
+//            printf("\r\n running %d\r\n",state);
+//        }
         
-        
-    }
+    //}
 }
 
 void ReleaseReadButton(){
@@ -271,6 +372,12 @@
     while (1)
     {
         semButton.wait();
+        mut1.lock();//grab global variables
+        char localErrorId = _errorId;
+        bool localIsErrorMsg = _isErrorMsg;
+        char localState = _state;
+        mut1.unlock();
+        
         int count = 0;
             
         while(!pinButton.read())
@@ -281,7 +388,7 @@
             }
             ThisThread::sleep_for(10);
         }
-        switch (state)
+        switch (localState)
         {
             case STATE_READY:
                 if(count > countThreashold)
@@ -289,14 +396,17 @@
                     printf("button released count = %d\r\n",count);
                     count = 0;
                     //CentrifugeTestThread.start(CentrifugeTest);
-                    semStartTest.release();
+                    CentrifugeTestInit();
                     tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency
                     tickerPrint.attach(&PrintRelease,PRINT_TIME_S);
                 }
                 break;
             case STATE_RUNNING:
                 if(count >1){
-                    EN_FAULTA.write(0);
+                    //EN_FAULTA.write(0);
+                    mut1.lock();
+                    _state = STATE_HALT;
+                    mut1.unlock();
                 }
                 break;
                 
@@ -307,7 +417,7 @@
                 break;
         }
         count = 0;
-        ThisThread::sleep_for(100);
+        //ThisThread::sleep_for(100);
     }
 }
 
@@ -345,7 +455,7 @@
     //start all threads
     MotorControlThread.start(MotorControl);
     
-    CentrifugeTestThread.start(CentrifugeTest);
+    //CentrifugeTestThread.start(CentrifugeTest);
     //start print thread.
     
     ReadButtonThread.start(ReadButton);