File content as of revision 2:da51e13f4ddf:

/* 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();//this is the first change
            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);
    }
}