File content as of revision 18:5e9b288793bb:

/* 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);
QEI encoder(PA_15, PB_7, PC_13, 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_11);//(PC_8);
DigitalOut      pinLedWhite     (PC_2);//(PC_6);
DigitalOut      pinLedRed       (PC_3);//(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;
}


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;
            
            //mut1.lock();
            double printDemandSpeed = demandSpeed_RPM;
            double printCurrentSpeed = currentSpeedRPM;
            int intCurrentSpeed = int(printCurrentSpeed);
            //printf("Current Speed %d RPM.\t Time remaining %d s\r\n",intCurrentSpeed, displayTime);
            //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);
        }
    }
}

bool isSpinning(){
    int pulses1 = encoder.getPulses();
    wait(0.1);
    int pulses2 = encoder.getPulses();
    int difference = pulses2 - pulses1;
    if (abs(difference) > 10){
        return 1;
    }else{
        return 0;
    }
}

void MotorControl(){
    double Ax, Ay, Az, Mag;
    static double MagFil, lastAx, lastAy, lastAz;
    static double lastErrorDot;
    while(1){
        semMotorControl.wait();//wait for a signal
        
        
        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(demandSpeed_RPM = 0.0){
//            output = 0.0;
//        }
        
//            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
            //printf("Wating for motor to stop.\r\n");
            while(isSpinning()){
                ThisThread::sleep_for(200);
            }
            //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);//enable motor

        //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;
        if(state == STATE_READY){
            //pinLedRed = 0;
        }
        while(pinButton.read())
        {
            count++;
            if(count < countThreashold){
                blinkCount++;
                if(blinkCount>=10){
                    blinkCount = 0;
                    pinLedRed = !pinLedRed;
                    if(count == 1){
                        pinLedRed = 1;
                    }
                }
            }
            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);
                    if(!isSpinning()){
                        semStartTest.release();
                        tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency
                        tickerPrint.attach(&PrintRelease,PRINT_TIME_S);
                    }else{
                        //printf("Holder still spinning. Wait until it has stopped.\r\n");
                    }
                }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");
                pinLedRed = !pinLedRed;
                ThisThread::sleep_for(400);
//                errorBlinkCount++;
//                if(errorBlinkCount >=50){
//                    errorBlinkCount = 0;
//                    
//                }
                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.05);//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(1);//remove incase of restart?
    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);
    }
}