Diff: main.cpp

Revision:

6:004dc33f4081

Parent:

5:e9bb800a3742

Child:

7:e36f61608c10

--- a/main.cpp	Mon Aug 09 18:59:04 2021 +0000
+++ b/main.cpp	Mon Aug 09 21:06:17 2021 +0000
@@ -136,7 +136,7 @@
             double printDemandSpeed = demandSpeed_RPM;
             double printCurrentSpeed = currentSpeedRPM;
             mut1.unlock();
-            printf("%f\t%f\r\n",demandSpeed_RPM, currentSpeedRPM);
+            printf("%f\t%f\r\n",printDemandSpeed, printCurrentSpeed);
         }
     }
 }
@@ -144,14 +144,30 @@
 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.
-            
+        
+        //check accel here
+        double Ax, Ay, Az, Mag;
+        static double MagFil;
+        //read accel
+        Mag = Ax*Ax + Ay*Ay;
+        Mag = sqrt(Mag);
+        Mag *=ACCEL_SCALE;
+        MagFil = 0.3*Mag + 0.7*MagFil;
+        if (Mag > VIBRATION_THREASHOLD_G){
+            mut1.lock();
+            state = STATE_ERROR;
+            mut1.unlock();
+        }
+        
+        mut1.lock();
+        char localState = state;
+        mut1.unlock();
+                       
+        if(localState == 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
+            mut1.lock();
             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);
@@ -177,59 +193,76 @@
                 //printf("cool down %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
-            
-            currentSpeedRPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
+        }
+        else{
+            Tnow = 1000000.0;
+        }
             
-            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
+        
+        currentSpeedRPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
+                
+        double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
+        mut1.unlock();
+        deltaError = error - lastError;
+        double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
+        
+        //filter error dot
+        errorDot = 0.2*errorDot + 0.8 * lastErrorDot;
+        
+        integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
+        integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
         
-            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;
+        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;
+        static double lastErrorDot = errorDot;
+        
+        //exit when test has completed
+        if (Tnow >= SPIN_T[5]){
+            printf("Test complete %f \t %f\r\n", Tnow, SPIN_T[5]);
             
-            //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();
-                encoderTimer.stop();
-                encoderTimer.reset();
-                EN_FAULTA.write(0);//disable motor
-                
-                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
-            }
+            mut1.lock();
+            state = STATE_READY;//change state
+            mut1.unlock();
+            testTimer.stop(); //stop and reset timers
+            testTimer.reset();
+            Tnow = testTimer.read();
+            encoderTimer.stop();
+            encoderTimer.reset();
+            EN_FAULTA.write(0);//disable motor
+            
+            lastPulses = 0;//update 
+            lastError = 0.0;
+            lastSpeedRPM = 0.0;
+            lastErrorDot = 0.0;
+            
+            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
         }
     }
 }
@@ -238,7 +271,9 @@
     while(1){    
         semStartTest.wait();
         printf("\r\n Test starting \r\n");
+        mut1.lock();
         state = STATE_RUNNING;
+        mut1.unlock();
         //set up test
         testTimer.reset();
         testTimer.start();//start timer
@@ -253,9 +288,9 @@
         PrintThread.start(PrintTimeRemaining);
         printf("\r\n Test setup complete \r\n");
         EN_FAULTA.write(1);
-        if(state == STATE_RUNNING){
-            printf("\r\n running %d\r\n",state);
-        }
+//        if(state == STATE_RUNNING){
+//            printf("\r\n running %d\r\n",state);
+//        }
         
         
     }
@@ -281,7 +316,10 @@
             }
             ThisThread::sleep_for(10);
         }
-        switch (state)
+        mut1.lock();
+        char localState = state;
+        mut1.unlock();
+        switch (localState)
         {
             case STATE_READY:
                 if(count > countThreashold)