Diff: main.cpp

Revision:

19:a71ef54d9e94

Parent:

18:5e9b288793bb

Child:

21:26a459e4201c

diff -r 5e9b288793bb -r a71ef54d9e94 main.cpp
--- a/main.cpp	Thu Nov 04 09:17:40 2021 +0000
+++ b/main.cpp	Thu Nov 04 11:29:12 2021 +0000
@@ -172,6 +172,7 @@
     double Ax, Ay, Az, Mag;
     static double MagFil, lastAx, lastAy, lastAz;
     static double lastErrorDot;
+    bool isTestComplete = 0;
     while(1){
         semMotorControl.wait();//wait for a signal
         
@@ -206,39 +207,59 @@
             //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.
             
-            //calculate current demand
-            if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
+            
+        }
+        
+        switch(state){        
+            case STATE_RUNNING:
+                //calculate current demand
+                if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
+                    demandSpeed_RPM = 0.0;
+                    isTestComplete = 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;
+                    isTestComplete = 0;
+                    //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;
+                    isTestComplete = 0;
+                    //printf("coast %f\r\n", demandSpeed_RPM);
+                }
+                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);
+                    isTestComplete = 0;
+                    //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;
+                    isTestComplete = 1;
+                    //printf("cool down %f\r\n", demandSpeed_RPM);
+                    //deactivate motor?
+                }
+                break;
+            case STATE_ERROR:
+                demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
+                if(demandSpeed_RPM < 0.0){
+                    isTestComplete = 1;
+                } else {
+                    isTestComplete = 0;
+                }
+                break;
+            case STATE_READY:
                 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);
-            }
-            if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
-                demandSpeed_RPM = targetSpeed_RPM;
-                //printf("coast %f\r\n", demandSpeed_RPM);
-            }
-            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
+                break;
+            default:
                 demandSpeed_RPM = 0.0;
-                //printf("cool down %f\r\n", demandSpeed_RPM);
-                //deactivate motor?
-            }
-        }
-            
-        if(state == STATE_ERROR){
-            demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
+                break;
         }
     
-        
         demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
         currentPulses = encoder.getPulses();//calculate current speed
         double deltaPulses = currentPulses - lastPulses;
@@ -267,15 +288,7 @@
             PHA.write(1);
             output = -1*output;
         }
-        
-//        if(demandSpeed_RPM = 0.0){
-//            output = 0.0;
-//        }
-        
-//            if(state == STATE_ERROR){
-//                output = 0;
-//            }
-            
+                 
         PWMA.write(output);//write to motor
         lastPulses = currentPulses;//update 
         lastError = error;
@@ -283,9 +296,14 @@
         lastErrorDot = errorDot;
         
         //exit when test has completed
-        if (Tnow >= SPIN_T[5]){
+        if(isTestComplete){
             //printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
-            state = STATE_READY;//change state
+            if(state = STATE_ERROR){
+                state = STATE_ERROR;//change state
+            } 
+            else {
+                state = STATE_READY;
+            }
             testTimer.stop(); //stop and reset timers
             testTimer.reset();
             Tnow = testTimer.read();