Diff: main.cpp

Revision:

0:64442857169c

Child:

1:6e3f1776be82

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Aug 08 12:25:06 2021 +0000
@@ -0,0 +1,360 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "Settings.h"
+#include "QEI.h"
+//#include "LIS3DH.h"
+
+/*
+Open issues:
+- need to figure out how to handle transitions to error states
+- Need to test how breaking is performed. 
+-Test onboard current limitation
+*/
+
+Timer encoderTimer;
+Timer buttonTimer;
+Timer testTimer;
+
+Timeout buttonTimeout;
+
+Thread PrintThread(osPriorityAboveNormal);
+Thread CentrifugeTestThread(osPriorityHigh);
+Thread ReadButtonThread(osPriorityNormal);
+Thread MotorControlThread(osPriorityRealtime);
+
+double SPIN_T[6]; //times for spin cycle
+
+QEI encoder(PA_14, PA_13, PA_15, 256, QEI::X4_ENCODING);
+
+//LIS3DH accel = LIS3DH(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS,LIS3DH_DR_NR_LP_400HZ, LIS3DH_FS_2G);
+
+DigitalOut  EN_FAULTA   (D2);
+DigitalOut  STBY        (D8);
+DigitalOut  PHA         (D3);
+DigitalOut  PHB         (D7);
+PwmOut      PWMA        (D5);
+PwmOut      PWMB        (D4);
+PwmOut      REF_PWM_A   (D15);
+PwmOut      REF_PWM_B   (D14);
+
+Serial pc(USBTX,USBRX);
+
+DigitalOut      pinStatusLED    (D13);
+DigitalIn       pinButton       (PC_13);
+
+/* Still to allocate
+
+DigitalOut pinBuzzer
+DigitalOut pinPowerLED
+
+*/
+
+//global variables
+double speedFilterConstant;
+char state;
+double currentPulses;
+double lastPulses;
+double lastSpeedRPM;
+double Tnow;
+double demandSpeed_RPM = 0.0;
+double currentSpeedRPM;
+double deltaError;
+double lastError;
+double integralTerm;
+double output;
+
+Ticker tickerMotorControl;
+Ticker tickerPrint;
+Ticker tickerReleaseButton;
+
+Semaphore semMotorControl(0);
+Semaphore semPrint(0);
+Semaphore semStartTest(0);
+Semaphore semButton(0);
+
+Mutex mut1;
+
+double LimitDouble(double input, double min, double max){
+    double output;
+    output = input;
+    if (input > max){
+        output = max;
+    }
+    if (input < min){
+        output = min;
+    }
+    return output;
+}
+/*
+double GetSpeed(int pulses, int lastPulses, double currentSpeed){
+    double filteredSpeedRPM;
+    
+    //find change in pulses.
+    double speedRPM = (double) (pulses - lastPulses); //get change in angle
+    speedRPM /= encoderTimer.read_us(); //calculate speed in pulses/us
+    encoderTimer.reset();
+    speedRPM *= 60000000.0/PULSES_PER_REV/4.0;//convert to RPM;
+    
+    //filter speed
+    //filteredSpeedRPM = speedFilterConstant * speedRPM + currentSpeed - speedFilterConstant * currentSpeed;
+    filteredSpeedRPM = speedRPM;
+    return filteredSpeedRPM;
+}*/
+
+
+void PIDControl(){
+    
+}
+
+void TestCompleteNotification(){
+    //send message 
+    printf("\r\nTest complete\r\n");
+    //sound buzzer and blink 3 times
+}
+
+void MotorControlRelease(){
+    semMotorControl.release();
+}
+void PrintRelease(){
+    semPrint.release();
+}
+
+void PrintTimeRemaining(){
+    while(1){
+        semPrint.wait();
+        if(state == STATE_RUNNING){    
+            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",demandSpeed_RPM, currentSpeedRPM);
+        }
+    }
+}
+
+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.
+            
+            Tnow = testTimer.read();
+            //check accel. If problem, change state to ERROR
+            
+            //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;
+                //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
+                demandSpeed_RPM = 0.0;
+                //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
+            
+            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);
+        
+            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;
+            
+            //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
+            }
+        }
+    }
+}
+
+void CentrifugeTest(){
+    while(1){    
+        semStartTest.wait();
+        printf("\r\n Test starting \r\n");
+        state = STATE_RUNNING;
+        //set up test
+        testTimer.reset();
+        testTimer.start();//start timer
+        
+        char spinState;
+        
+        //set up ticker to allow motor control thread to run periodically
+        
+        encoder.reset();//reset encoder
+        lastPulses = 0;//reset previous encoder reading
+        encoderTimer.start();
+        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);
+        }
+        
+        
+    }
+}
+
+void ReleaseReadButton(){
+    semButton.release();
+}
+
+void ReadButton()
+{
+    int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S);
+    while (1)
+    {
+        semButton.wait();
+        int count = 0;
+            
+        while(!pinButton.read())
+        {
+            count++;
+            if (count ==1){
+                printf("button pressed\r\n");
+            }
+            ThisThread::sleep_for(10);
+        }
+        switch (state)
+        {
+            case STATE_READY:
+                if(count > countThreashold)
+                {
+                    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);
+                }
+                break;
+            case STATE_RUNNING:
+                if(count >1){
+                    EN_FAULTA.write(0);
+                }
+                break;
+                
+            case STATE_ERROR:
+            
+                break;
+            default:
+                break;
+        }
+        count = 0;
+        ThisThread::sleep_for(100);
+    }
+}
+
+int main(){
+    printf("\r\nSystem Online\r\n");
+    //set up system
+    
+    //set up motor
+    //EN_FAULTA.mode(PullDown);
+    //STBY.mode(PullDown);
+    
+    REF_PWM_A.period_us(100);
+    PWMA.period_us(100);
+    REF_PWM_A.write(0.5);//set current reference
+    
+    EN_FAULTA.write(0);
+    STBY.write(1);
+    //calculate filter constant
+    speedFilterConstant = 2*3.141593*SAMPLE_TIME_US/1000000*FILTER_CUTOFF_FREQ;
+    speedFilterConstant /= (1 + speedFilterConstant);
+    
+    SPIN_T[0] = 0.0;
+    SPIN_T[1] = T_WARMUP;
+    SPIN_T[2] = T_WARMUP + T_RAMP;
+    SPIN_T[3] = T_WARMUP + T_RAMP + T_TEST;
+    SPIN_T[4] = T_WARMUP + T_RAMP + T_TEST + T_RAMP;
+    SPIN_T[5] = T_WARMUP + T_RAMP + T_TEST + T_RAMP + T_WARMUP;
+    
+/*    //set up button interrupts
+    pinButton.rise(ISR_Button_Rise);
+    pinButton.fall(ISR_Button_Fall);
+*/
+    
+    state = STATE_READY;//set state to READY
+    //start all threads
+    MotorControlThread.start(MotorControl);
+    
+    CentrifugeTestThread.start(CentrifugeTest);
+    //start print thread.
+    
+    ReadButtonThread.start(ReadButton);
+    tickerReleaseButton.attach(ReleaseReadButton,BUTTON_READ_SAMPLETIME_S);
+    
+    
+    printf("\r\nSetup Complete\r\n");
+    while (true) {
+        //check state of button
+        ThisThread::sleep_for(osWaitForever);
+    }
+}