Diff: main.cpp

Revision:

9:b1f53e4eb453

Parent:

8:41cd1fc8cbbe

Child:

10:8a41143a9f52

--- a/main.cpp	Tue Aug 10 15:52:39 2021 +0000
+++ b/main.cpp	Thu Aug 12 13:42:41 2021 +0000
@@ -37,15 +37,20 @@
 DigitalOut  EN_FAULTA   (D2);
 DigitalOut  STBY        (D8);
 DigitalOut  PHA         (D3);
-DigitalOut  PHB         (D7);
+//DigitalOut  PHB         (D7);
 PwmOut      PWMA        (D5);
-PwmOut      PWMB        (D4);
+//PwmOut      PWMB        (D4);
 PwmOut      REF_PWM_A   (D15);
 PwmOut      REF_PWM_B   (D14);
 AnalogIn pinAx(A0);
 AnalogIn pinAy(A1);
 AnalogIn pinAz(A2);
 
+DigitalOut  PHB          (D7);
+DigitalOut  EN_FAULTB    (D11);
+PwmOut      PWMB         (D4);
+PwmOut      REF_PWMB     (D14);
+
 DigitalOut pinSt(A3);
 
 Serial pc(USBTX,USBRX);
@@ -77,6 +82,8 @@
 double integralTerm;
 double output;
 
+double _MagFil;
+
 Ticker tickerMotorControl;
 Ticker tickerPrint;
 Ticker tickerReleaseButton;
@@ -140,26 +147,30 @@
             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",demandSpeed_RPM, currentSpeedRPM);
+            //printf("%f\t%f\r\n", currentSpeedRPM,_MagFil);
+            //printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed);
+            //printf("%f\r\n",_MagFil);
         }
     }
 }
 
 void MotorControl(){
+    double Ax, Ay, Az, Mag;
+    static double MagFil, lastAx, lastAy, lastAz;
+    static double lastErrorDot;
     while(1){
         semMotorControl.wait();//wait for a signal
         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
-            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;
@@ -175,11 +186,12 @@
             Mag = Ax*Ax + Ay*Ay;
             Mag = sqrt(Mag);
             //Mag *=ACCEL_SCALE;
-            MagFil = 0.3*Mag + 0.7*MagFil;
+            MagFil = 0.1*Mag + 0.9*MagFil;
+            _MagFil = MagFil;
             if (MagFil > VIBRATION_THRESHOLD){
                 //mut1.lock();
-                state = STATE_ERROR;
-                
+                //state = STATE_ERROR;
+                printf("Excess vibration detected\r\n");
                 //mut1.unlock();
             }
             //int deltaT = encoderTimer.read();//read current time in seconds
@@ -217,17 +229,19 @@
             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
+            currentSpeedRPM = 0.1*speed_RPM + 0.9*lastSpeedRPM;//filter speed
             
             double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
             deltaError = error - lastError;
             double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
+            errorDot = 0.1*errorDot + 0.9*lastErrorDot;
             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 += Ko*demandSpeed_RPM;
             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);
         
@@ -242,29 +256,59 @@
             lastPulses = currentPulses;//update 
             lastError = error;
             lastSpeedRPM = currentSpeedRPM;
+            lastErrorDot = errorDot;
             
             //exit when test has completed
             if (Tnow >= SPIN_T[5]){
-                printf("Terminating Test %f \t %f\r\n", 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();
+                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("state = %d\r\n",state);
+                //printf("state = %d\r\n",state);
                 pinLedRed = 0;
                 TestCompleteNotification();//send notification
+                lastErrorDot = 0.0;
+                lastError = 0.0;
+                integralTerm = 0.0;
                 //deactivate motor
                 //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;
+        } 
     }
 }
 
@@ -287,8 +331,9 @@
         PrintThread.start(PrintTimeRemaining);
         printf("\r\n Test setup complete \r\n");
         EN_FAULTA.write(1);
+        //EN_FAULTB.write(1);
         if(state == STATE_RUNNING){
-            printf("\r\n running %d\r\n",state);
+            //printf("\r\n running %d\r\n",state);
         }
         
         
@@ -306,12 +351,26 @@
     {
         semButton.wait();
         int count = 0;
-            
+        int blinkCount = 0;
+        pinLedRed = 0;
         while(pinButton.read())
         {
             count++;
+            if(count < countThreashold){
+                blinkCount++;
+                if(blinkCount>=10){
+                    blinkCount = 0;
+                    pinLedRed = !pinLedRed;
+                    if(count == 1){
+                        pinLedRed = 0;
+                    }
+                }
+            }
+            else{
+                pinLedRed = 1;
+            }
             if (count ==1){
-                printf("button pressed\r\n");
+                printf("Button pressed. Hold for %f s\r\n",BUTTON_HOLD_TIME_S);
             }
             ThisThread::sleep_for(10);
         }
@@ -320,21 +379,28 @@
             case STATE_READY:
                 if(count > countThreashold)
                 {
-                    printf("button released count = %d\r\n",count);
+                    pinLedRed = 1;
+                    //printf("button released count = %d\r\n",count);
                     count = 0;
                     //CentrifugeTestThread.start(CentrifugeTest);
                     semStartTest.release();
                     tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency
                     tickerPrint.attach(&PrintRelease,PRINT_TIME_S);
+                }else{
+                    pinLedRed = 0;
                 }
                 break;
             case STATE_RUNNING:
                 if(count >1){
                     EN_FAULTA.write(0);
+                    EN_FAULTB.write(0);
+                    state = STATE_ERROR;
+                    //EN_FAULTB.write(0);
                 }
                 break;
                 
             case STATE_ERROR:
+                //printf("Please restart the system. \r\n");
             
                 break;
             default:
@@ -354,8 +420,14 @@
     //STBY.mode(PullDown);
     
     REF_PWM_A.period_us(100);
-    PWMA.period_us(100);
-    REF_PWM_A.write(0.5);//set current reference
+    PWMA.period_us(33);
+    REF_PWM_A.write(0.1);//set current reference
+    
+    REF_PWMB.period_us(100);
+    PWMB.period_us(33);
+    REF_PWMB.write(0.0);
+    PHB.write(0);
+    EN_FAULTB.write(0);
     
     EN_FAULTA.write(0);
     STBY.write(1);