Diff: main.cpp

Revision:

12:bc34f264e2f2

Parent:

11:62d2a592b1ae

Child:

13:32e1ae4221f7

--- a/main.cpp	Thu Aug 12 15:15:32 2021 +0000
+++ b/main.cpp	Thu Aug 12 15:47:50 2021 +0000
@@ -147,13 +147,26 @@
             double timeElapsed = testTimer.read();
             double timeRemaining = SPIN_T[5] - timeElapsed;
             int displayTime = int(timeRemaining)+1;
-            printf("Time remaining %d s\r\n", displayTime);
+            //printf("Time remaining %d s\r\n", displayTime);
             //mut1.lock();
             double printDemandSpeed = demandSpeed_RPM;
             double printCurrentSpeed = currentSpeedRPM;
             //mut1.unlock();
             //printf("%f\t%f\r\n", currentSpeedRPM,_MagFil);
-            //printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed);
+            printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed);
+            //printf("%f\r\n",_MagFil);
+        }
+        if(state == STATE_ERROR){    
+            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();
+            //printf("%f\t%f\r\n", currentSpeedRPM,_MagFil);
+            printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed);
             //printf("%f\r\n",_MagFil);
         }
     }
@@ -165,37 +178,37 @@
     static double lastErrorDot;
     while(1){
         semMotorControl.wait();//wait for a signal
+        
+        pinLedRed = 1;
+        Tnow = testTimer.read();
+        //check accel. If problem, change state to ERROR
+        //check accel here
+        
+        /*
+        Ax = 0.2*pinAx.read() + 0.8*Ax - 0.5;
+        Ay = 0.2*pinAy.read() + 0.8*Ay - 0.5;
+        Az = 0.2*pinAz.read() + 0.8*Az - 0.5;
+        */
+        
+        Ax = pinAx.read() - 0.5;
+        Ay = pinAy.read() - 0.5;
+        Az = pinAz.read() - 0.5;
+        
+        
+        //read accel
+        Mag = Ax*Ax + Ay*Ay;
+        Mag = sqrt(Mag);
+        //Mag *=ACCEL_SCALE;
+        MagFil = 0.1*Mag + 0.9*MagFil;
+        _MagFil = MagFil;
+        if (MagFil > VIBRATION_THRESHOLD){
+            //mut1.lock();
+            //state = STATE_ERROR;
+            printf("Excess vibration detected\r\n");
+            //mut1.unlock();
+        }
+            //int deltaT = encoderTimer.read();//read current time in seconds
         if(state == STATE_RUNNING){//need to check if this is the best condition to look for.
-            pinLedRed = 1;
-            Tnow = testTimer.read();
-            //check accel. If problem, change state to ERROR
-            //check accel here
-            
-            /*
-            Ax = 0.2*pinAx.read() + 0.8*Ax - 0.5;
-            Ay = 0.2*pinAy.read() + 0.8*Ay - 0.5;
-            Az = 0.2*pinAz.read() + 0.8*Az - 0.5;
-            */
-            
-            Ax = pinAx.read() - 0.5;
-            Ay = pinAy.read() - 0.5;
-            Az = pinAz.read() - 0.5;
-            
-            
-            //read accel
-            Mag = Ax*Ax + Ay*Ay;
-            Mag = sqrt(Mag);
-            //Mag *=ACCEL_SCALE;
-            MagFil = 0.1*Mag + 0.9*MagFil;
-            _MagFil = MagFil;
-            if (MagFil > VIBRATION_THRESHOLD){
-                //mut1.lock();
-                //state = STATE_ERROR;
-                printf("Excess vibration detected\r\n");
-                //mut1.unlock();
-            }
-            //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
                 demandSpeed_RPM = 0.0;
@@ -222,10 +235,12 @@
                 //printf("cool down %f\r\n", demandSpeed_RPM);
                 //deactivate motor?
             }
+        }
             
-/*            if(state == STATE_ERROR){
+            if(state == STATE_ERROR){
                 demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
-            }*/
+            }
+        
             
             demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
             currentPulses = encoder.getPulses();//calculate current speed
@@ -256,9 +271,9 @@
                 output = -1*output;
             }
             
-/*            if(state == STATE_ERROR){
-                output = 0;
-            }*/
+//            if(state == STATE_ERROR){
+//                output = 0;
+//            }
                 
             PWMA.write(output);//write to motor
             lastPulses = currentPulses;//update 
@@ -276,6 +291,8 @@
                 encoderTimer.stop();
                 encoderTimer.reset();
                 PWMA.write(0.0);
+                
+                //add check if motor has stopped
                 ThisThread::sleep_for(500);
                 EN_FAULTA.write(0);//disable motor
                 EN_FAULTB.write(0);
@@ -293,31 +310,10 @@
                 //PrintThread.terminate();//terminate print thread
                 //CentrifugeTestThread.terminate();//terminate threads
             }
-        } //end running conditional
-        /*else{
-            Tnow = 0.0;
-            state = STATE_READY;//change state
-            testTimer.stop(); //stop and reset timers
-            testTimer.reset();
-            //Tnow = testTimer.read();
-            encoderTimer.stop();
-            encoderTimer.reset();
-            //PWMA.write(0.0);
-            ThisThread::sleep_for(500);
-            EN_FAULTA.write(0);//disable motor
-            EN_FAULTB.write(0);
             
-            tickerMotorControl.detach(); //detach the semaphore release for motor control
-            tickerPrint.detach();
-            
-            printf("Test terminated by user\r\n");
-            pinLedRed = 0;
-            //TestCompleteNotification();//send notification
-            lastErrorDot = 0.0;
-            lastError = 0.0;
-            integralTerm = 0.0;
-        } */
-    }
+        //} //end running conditional
+ 
+    }//end while
 }
 
 void CentrifugeTest(){
@@ -355,9 +351,15 @@
 void ReadButton()
 {
     int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S);
+    int errorBlinkCount;
     while (1)
     {
         semButton.wait();
+        
+        if(state == STATE_ERROR){
+            
+        }
+        
         int count = 0;
         int blinkCount = 0;
         pinLedRed = 0;
@@ -382,7 +384,7 @@
                     printf("Button pressed. Hold for %f s\r\n",BUTTON_HOLD_TIME_S);
                 }
                 else if(state == STATE_RUNNING){
-                    printf("Test terminated by user. Please restart system.\r\n");
+                    printf("Test terminated by user. Slowing to stop. Please restart system when safe.\r\n");
                     state = STATE_ERROR;
                 }
             }
@@ -415,7 +417,11 @@
                 
             case STATE_ERROR:
                 //printf("Please restart the system. \r\n");
-            
+                errorBlinkCount++;
+                if(errorBlinkCount >=50){
+                    errorBlinkCount = 0;
+                    pinLedRed = !pinLedRed;
+                }
                 break;
             default:
                 break;