Diff: main.cpp

Revision:

7:e36f61608c10

Parent:

6:004dc33f4081

Child:

8:41cd1fc8cbbe

--- a/main.cpp	Mon Aug 09 21:06:17 2021 +0000
+++ b/main.cpp	Tue Aug 10 11:00:13 2021 +0000
@@ -7,6 +7,7 @@
 #include "platform/mbed_thread.h"
 #include "Settings.h"
 #include "QEI.h"
+#include "math.h"
 //#include "LIS3DH.h"
 
 /*
@@ -41,6 +42,11 @@
 PwmOut      PWMB        (D4);
 PwmOut      REF_PWM_A   (D15);
 PwmOut      REF_PWM_B   (D14);
+AnalogIn pinAx(A0);
+AnalogIn pinAy(A1);
+AnalogIn pinAz(A2);
+
+DigitalOut pinSt(A3);
 
 Serial pc(USBTX,USBRX);
 
@@ -132,11 +138,11 @@
             double timeRemaining = SPIN_T[5] - timeElapsed;
             int displayTime = int(timeRemaining)+1;
             //printf("Time remaining %d s\r\n", displayTime);
-            mut1.lock();
+            //mut1.lock();
             double printDemandSpeed = demandSpeed_RPM;
             double printCurrentSpeed = currentSpeedRPM;
-            mut1.unlock();
-            printf("%f\t%f\r\n",printDemandSpeed, printCurrentSpeed);
+            //mut1.unlock();
+            //printf("%f\t%f\r\n",demandSpeed_RPM, currentSpeedRPM);
         }
     }
 }
@@ -144,30 +150,38 @@
 void MotorControl(){
     while(1){
         semMotorControl.wait();//wait for a signal
-        
-        //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.
+        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
+            //check accel here
+            double Ax, Ay, Az, Mag;
+            static double MagFil, lastAx, lastAy, lastAz;
+            /*
+            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.3*Mag + 0.7*MagFil;
+            if (MagFil > VIBRATION_THRESHOLD){
+                //mut1.lock();
+                state = STATE_ERROR;
+                
+                //mut1.unlock();
+            }
+            //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);
@@ -193,76 +207,59 @@
                 //printf("cool down %f\r\n", demandSpeed_RPM);
                 //deactivate motor?
             }
-        }
-        else{
-            Tnow = 1000000.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
             
-        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);
+            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);
         
-        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]);
+            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;
             
-            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
+            //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
+            }
         }
     }
 }
@@ -271,9 +268,7 @@
     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
@@ -288,9 +283,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);
+        }
         
         
     }
@@ -316,10 +311,7 @@
             }
             ThisThread::sleep_for(10);
         }
-        mut1.lock();
-        char localState = state;
-        mut1.unlock();
-        switch (localState)
+        switch (state)
         {
             case STATE_READY:
                 if(count > countThreashold)