Diff: main.cpp

Revision:

5:e9bb800a3742

Parent:

4:d04afc466198

Child:

6:004dc33f4081

--- a/main.cpp	Sun Aug 08 14:59:54 2021 +0000
+++ b/main.cpp	Mon Aug 09 18:59:04 2021 +0000
@@ -7,13 +7,13 @@
 #include "platform/mbed_thread.h"
 #include "Settings.h"
 #include "QEI.h"
-#include "math.h"
 //#include "LIS3DH.h"
 
 /*
 Open issues:
-- Test how breaking is performed. 
-- Test onboard current limitation
+- need to figure out how to handle transitions to error states
+- Need to test how breaking is performed. 
+-Test onboard current limitation
 */
 
 Timer encoderTimer;
@@ -56,17 +56,13 @@
 
 //global variables
 double speedFilterConstant;
-char _state;
-char _errorId = 0x00;
-bool _isErrorMsg = 0;
-
-
+char state;
 double currentPulses;
 double lastPulses;
 double lastSpeedRPM;
 double Tnow;
-double _demandSpeed_RPM = 0.0;
-double _currentSpeed_RPM;
+double demandSpeed_RPM = 0.0;
+double currentSpeedRPM;
 double deltaError;
 double lastError;
 double integralTerm;
@@ -128,252 +124,141 @@
     semPrint.release();
 }
 
-void PrintMessages(){
+void PrintTimeRemaining(){
     while(1){
         semPrint.wait();
-        
-        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){    
+        if(state == 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);
-
-            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.
+            double printDemandSpeed = demandSpeed_RPM;
+            double printCurrentSpeed = currentSpeedRPM;
             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;
-            }
+            printf("%f\t%f\r\n",demandSpeed_RPM, currentSpeedRPM);
         }
     }
 }
 
-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
-        //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.
+        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
+            
             //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;
-            }*/
-            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;
-                demandSpeed_RPM += targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
-            }
-            else if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
-                demandSpeed_RPM = targetSpeed_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;
-            }
-            else {//((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
-                demandSpeed_RPM = 0.0;
-            }
-        }
-        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();
+                //printf("warm up %f\r\n", demandSpeed_RPM);
+                //deactivate motor?
             }
-        }
-        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 = demandSpeed_RPM;
-        _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;
-        
-        
-        //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
-            Timer timerMotorStopTimeout;
-            bool isTimeout;
-            timerMotorStopTimeout.start();
-            if(timerMotorStopTimeout.read() > MOTOR_STOP_TIMEOUT){
-                isTimeout = 1;
+            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);
+            }
+            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{
-                isTimeout = 0;
+            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);
+            }
+            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((deltaPulses == 0) || isTimeout){//check if motor has come to a complete stop or if timeout has occured
-                timerMotorStopTimeout.stop();
-                timerMotorStopTimeout.reset();
-                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{
-                    mut1.lock();
-                    localErrorId = _errorId;
-                    mut1.unlock();
-                    switch (localErrorId){
-                        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;
+            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);
+        
+            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
                 testTimer.stop(); //stop and reset timers
                 testTimer.reset();
                 Tnow = testTimer.read();
-                encoder.reset();//reset encoder
-                //encoderTimer.stop();
-                //encoderTimer.reset();
+                encoderTimer.stop();
+                encoderTimer.reset();
+                EN_FAULTA.write(0);//disable motor
                 
-    
-                //printf("state = %d\r\n",state);
+                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 CentrifugeTestInit(){
-//    while(1){    
-//        semStartTest.wait();
+void CentrifugeTest(){
+    while(1){    
+        semStartTest.wait();
         printf("\r\n Test starting \r\n");
-        mut1.lock();
-        _state = STATE_RUNNING;
-        _errorId = 0x00;
-        mut1.unlock();
+        state = STATE_RUNNING;
         //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
-        PrintThread.start(PrintMessages);
+        encoderTimer.start();
+        PrintThread.start(PrintTimeRemaining);
         printf("\r\n Test setup complete \r\n");
-        EN_FAULTA.write(1);//enable motor
-        //encoderTimer.start();
-//        if(state == STATE_RUNNING){
-//            printf("\r\n running %d\r\n",state);
-//        }
+        EN_FAULTA.write(1);
+        if(state == STATE_RUNNING){
+            printf("\r\n running %d\r\n",state);
+        }
         
-    //}
+        
+    }
 }
 
 void ReleaseReadButton(){
@@ -386,12 +271,6 @@
     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())
@@ -402,7 +281,7 @@
             }
             ThisThread::sleep_for(10);
         }
-        switch (localState)
+        switch (state)
         {
             case STATE_READY:
                 if(count > countThreashold)
@@ -410,17 +289,14 @@
                     printf("button released count = %d\r\n",count);
                     count = 0;
                     //CentrifugeTestThread.start(CentrifugeTest);
-                    CentrifugeTestInit();
+                    semStartTest.release();
                     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);
-                    mut1.lock();
-                    _state = STATE_HALT;
-                    mut1.unlock();
+                    EN_FAULTA.write(0);
                 }
                 break;
                 
@@ -431,7 +307,7 @@
                 break;
         }
         count = 0;
-        //ThisThread::sleep_for(100);
+        ThisThread::sleep_for(100);
     }
 }
 
@@ -464,13 +340,12 @@
     pinButton.rise(ISR_Button_Rise);
     pinButton.fall(ISR_Button_Fall);
 */
-    mut1.lock();
-    _state = STATE_READY;//set state to READY
-    mut1.unlock();
+    
+    state = STATE_READY;//set state to READY
     //start all threads
     MotorControlThread.start(MotorControl);
     
-    //CentrifugeTestThread.start(CentrifugeTest);
+    CentrifugeTestThread.start(CentrifugeTest);
     //start print thread.
     
     ReadButtonThread.start(ReadButton);