File content as of revision 12:bc34f264e2f2:

/* 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 "math.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);
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);

DigitalOut      pinStatusLED    (D13);

DigitalIn       pinButton       (PC_8);
DigitalOut      pinLedWhite     (PC_6);
DigitalOut      pinLedRed       (PC_5);

/* 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;

double _MagFil;

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", currentSpeedRPM,_MagFil);
            printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed);
            //printf("%f\r\n",_MagFil);
        }
        if(state == STATE_ERROR){    
            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", 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
        
        pinLedRed = 1;
        Tnow = testTimer.read();
        //check accel. If problem, change state to ERROR
        //check accel here
        
        /*
        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.1*Mag + 0.9*MagFil;
        _MagFil = MagFil;
        if (MagFil > VIBRATION_THRESHOLD){
            //mut1.lock();
            //state = STATE_ERROR;
            printf("Excess vibration detected\r\n");
            //mut1.unlock();
        }
            //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
                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?
            }
        }
            
            if(state == STATE_ERROR){
                demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
            }
        
            
            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.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);
        
            if(output >=0){//Set direction
                PHA.write(0);
            }else{
                PHA.write(1);
                output = -1*output;
            }
            
//            if(state == STATE_ERROR){
//                output = 0;
//            }
                
            PWMA.write(output);//write to motor
            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]);
                state = STATE_READY;//change state
                testTimer.stop(); //stop and reset timers
                testTimer.reset();
                Tnow = testTimer.read();
                encoderTimer.stop();
                encoderTimer.reset();
                PWMA.write(0.0);
                
                //add check if motor has stopped
                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);
                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
 
    }//end while
}

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;
        pinLedRed = 1;
        //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, State:%d \r\n", state);
        EN_FAULTA.write(1);
        //EN_FAULTB.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);
    int errorBlinkCount;
    while (1)
    {
        semButton.wait();
        
        if(state == STATE_ERROR){
            
        }
        
        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){
                if(state == STATE_READY){
                    printf("Button pressed. Hold for %f s\r\n",BUTTON_HOLD_TIME_S);
                }
                else if(state == STATE_RUNNING){
                    printf("Test terminated by user. Slowing to stop. Please restart system when safe.\r\n");
                    state = STATE_ERROR;
                }
            }
            ThisThread::sleep_for(10);
        }
        switch (state)
        {
            case STATE_READY:
                if(count > countThreashold)
                {
                    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");
                errorBlinkCount++;
                if(errorBlinkCount >=50){
                    errorBlinkCount = 0;
                    pinLedRed = !pinLedRed;
                }
                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(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);
    //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);
*/
    pinButton.mode(PullDown);
    pinLedWhite = 1;


    
    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);
    }
}