File content as of revision 4:d04afc466198:

/* 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:
- 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;
char _errorId = 0x00;
bool _isErrorMsg = 0;


double currentPulses;
double lastPulses;
double lastSpeedRPM;
double Tnow;
double _demandSpeed_RPM = 0.0;
double _currentSpeed_RPM;
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 PrintMessages(){
    while(1){
        semPrint.wait();
        
        mut1.lock();//lock mutex to prevent race condition.
        double localDemandSpeed = _demandSpeed_RPM;
        double localCurrentSpeed = _currentSpeed_RPM;
        char localErrorId = _errorId;
        bool localIsErrorMsg = _isErrorMsg;
        char localState = _state;
        mut1.unlock();
        
        if(localState == 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);

            printf("%f\t%f\r\n",localDemandSpeed, localCurrentSpeed);
        }
        if(localIsErrorMsg){
            localIsErrorMsg = 0;//clear local error message flag.
            mut1.lock();
            _isErrorMsg = 0;//clear global error message flag.
            mut1.unlock();
            
            switch (localErrorId){
                case ERROR_ACCEL:
                    printf("Excess vibration detected\r\n");
                    break;
                case ERROR_MAN_STOP:
                    printf("Manual stop detected\r\n");
                    break;
                default:
                    break;
            }
        }
    }
}

int CheckAccelerometer(){
    //read accelerometer
    double xAccel;
    double yAccel;
    
    double magnitude = xAccel*xAccel + yAccel*yAccel;
    magnitude = sqrt(magnitude);
    
    if (magnitude > VIBRATION_THRESHOLD){
        return 1;
    }
    else{
        return 0;
    }
}

void MotorControl(){
    while(1){
        semMotorControl.wait();//wait for a signal
        //grab global variables
        mut1.lock();//lock mutex to prevent race condition.
        char localErrorId = _errorId;
        bool localIsErrorMsg = _isErrorMsg;
        char localState = _state;
        mut1.unlock();
        
        Tnow = testTimer.read();
        //check accel. If problem, change state to ERROR
        if(CheckAccelerometer()){
            mut1.lock();
            _state= STATE_HALT;
            _errorId = ERROR_ACCEL;
            _isErrorMsg = 1;//set flag for sending an error message
            mut1.unlock();
        }
        //int deltaT = encoderTimer.read();//read current time in seconds //don't need this????
        static double demandSpeed_RPM;
        
        if(localState == 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;
            }*/
            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;
                demandSpeed_RPM += targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
            }
            else if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
                demandSpeed_RPM = targetSpeed_RPM;
            }
            else 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);
                demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
            }
            else {//((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
                demandSpeed_RPM = 0.0;
            }
        }
        if(localState == STATE_HALT){//if halt condition is set
            if(demandSpeed_RPM > 0.0){
                demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000;
            }
            else{
                mut1.lock();
                _state = STATE_ERROR;//once demand speed is 0, go to error state.
                mut1.unlock();
            }
        }
        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
        double currentSpeed_RPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
        
        mut1.lock();//update global variables 
        _demandSpeed_RPM = demandSpeed_RPM;
        _currentSpeed_RPM = currentSpeed_RPM;
        mut1.unlock();//end of global variable write
        
        double error = demandSpeed_RPM - currentSpeed_RPM;//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, currentSpeed_RPM, 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 variables
        lastError = error;
        lastSpeedRPM = currentSpeed_RPM;
        
        
        //exit when test has completed
        if ((Tnow >= SPIN_T[5]) || (localState == STATE_ERROR)){ //either if test time has expired, or if system has gone into an error state
            Timer timerMotorStopTimeout;
            bool isTimeout;
            timerMotorStopTimeout.start();
            if(timerMotorStopTimeout.read() > MOTOR_STOP_TIMEOUT){
                isTimeout = 1;
            }
            else{
                isTimeout = 0;
            }
            
                
            if((deltaPulses == 0) || isTimeout){//check if motor has come to a complete stop or if timeout has occured
                timerMotorStopTimeout.stop();
                timerMotorStopTimeout.reset();
                tickerPrint.detach();//stop the print thread from firing
                PrintThread.terminate();//terminate print thread???
                EN_FAULTA.write(0);//disable motor
                tickerMotorControl.detach(); //detach the semaphore release for motor control
                
                //ensure rotor has come to a complete stop.
                int deltaPulses;
                deltaPulses = encoder.getPulses();//calculate current change in pulses
                while(deltaPulses>0){//loop forever until system has come to complete stop
                    deltaPulses = encoder.getPulses();//calculate current speed
                    encoder.reset();//reset encoder count
                    ThisThread::sleep_for(10);
                }
                //Inform user why test has ended.
                if(localState == !STATE_ERROR){
                    printf("Test complete\r\n");//Test completed cleanly
                }
                else{
                    mut1.lock();
                    localErrorId = _errorId;
                    mut1.unlock();
                    switch (localErrorId){
                        case ERROR_ACCEL://accelerometer detected excess vibration
                            printf("Test terminated.\r\n Please check holder and restart system.\r\n");
                            break;
                        case ERROR_MAN_STOP://test was terminated manually
                            printf("Test terminated manually. \r\nPlease restart system to resume testing.\r\n");
                            break;
                        default:
                            break;
                    }
                }
                mut1.lock();
                _state = STATE_READY;//change state
                mut1.unlock();
                demandSpeed_RPM = 0.0;//update variables
                lastPulses = 0.0;
                lastError = 0.0;
                lastSpeedRPM = 0.0;
                testTimer.stop(); //stop and reset timers
                testTimer.reset();
                Tnow = testTimer.read();
                encoder.reset();//reset encoder
                //encoderTimer.stop();
                //encoderTimer.reset();
                
    
                //printf("state = %d\r\n",state);
                TestCompleteNotification();//send notification
                //deactivate motor
                //CentrifugeTestThread.terminate();//terminate threads
            }
        }
    }
}

void CentrifugeTestInit(){
//    while(1){    
//        semStartTest.wait();
        printf("\r\n Test starting \r\n");
        mut1.lock();
        _state = STATE_RUNNING;
        _errorId = 0x00;
        mut1.unlock();
        //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
        PrintThread.start(PrintMessages);
        printf("\r\n Test setup complete \r\n");
        EN_FAULTA.write(1);//enable motor
        //encoderTimer.start();
//        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();
        mut1.lock();//grab global variables
        char localErrorId = _errorId;
        bool localIsErrorMsg = _isErrorMsg;
        char localState = _state;
        mut1.unlock();
        
        int count = 0;
            
        while(!pinButton.read())
        {
            count++;
            if (count ==1){
                printf("button pressed\r\n");
            }
            ThisThread::sleep_for(10);
        }
        switch (localState)
        {
            case STATE_READY:
                if(count > countThreashold)
                {
                    printf("button released count = %d\r\n",count);
                    count = 0;
                    //CentrifugeTestThread.start(CentrifugeTest);
                    CentrifugeTestInit();
                    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);
                    mut1.lock();
                    _state = STATE_HALT;
                    mut1.unlock();
                }
                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);
*/
    mut1.lock();
    _state = STATE_READY;//set state to READY
    mut1.unlock();
    //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);
    }
}