File content as of revision 0:d7cc9bad311b:

#include "mbed.h"
#include "NOKIA_5110.h"
#include "PinDetect.h"
#include "Pattern.h"
#include "MovingAverage.h"

#define PWM_PERIOD_US 100 //10kHz
#define MAX_PWM 1.0f //Maximum duty cycle 100%
#define MIN_PWM 0.0f //Minimum duty cycle 0%
#define OPEN 17080.0f //No object sensor value
#define CLOSED 0.1f //Fully obstructed sensor value
#define REFERENCE 1150.0f //Setpoint (desired) sensor value
#define MAX_POWER 40.8f //Maximum power watts of coil
#define MAX_CURRENT 3.5f //Maximum current amps through coil (Used to calculate instantaneous current)
#define CONST_POWER 4.78f  //Const power drawn by fans and other hardware
#define COIL_VOLTAGE 12.0f //Voltage of the coil

//-- State variables

#define REF_PARAM 0
#define KP_PARAM 1
#define KD_PARAM 2

#define TEST 1
#define IDLE 2
#define LEVITATE 3
#define ERROR 4
#define CALIBRATION 5

//-- Function prototypes
void Initialise();
void Idle();
void Calibrate();
void Test();
void Error();
void SetRGB(bool,bool,bool);
void SetState(char);
void SelectButtonPress();
void UpButtonPress();
void DownButtonPress();
void SelectButtonHeld();
void UpButtonHeld();
void DownButtonHeld();
void UpdateScreen();
void PDController();
void Levitate();
void SetErrorLine(int,char*);
float RoundDigits(float,int);
void ResetTime();
void UpdateTime();


//-- Mbed objects
AnalogIn PosSensor(PC_0);
AnalogIn AmbSensor(PC_1);

NokiaLcd *Display;
LcdPins DisplayPins;

PinDetect SelectButton(PB_3);
PinDetect UpButton(PA_10);
PinDetect DownButton(PB_5);

Ticker ScreenUpdateTimer;
Timer CycleTimer;

DigitalOut RedLed(PA_8);
DigitalOut GreenLed(PA_9);
DigitalOut BlueLed(PB_10);

MovingAverage <float>AveragePower(10,0.0f);
MovingAverage <float>AverageCurrent(10,0.0f);

PwmOut CoilPWM(PWM_OUT);

//Variable decleration and initialisation

float ref = 0.0f; //Reference value
float pos = 0.0f; //Position value
float amb = 0.0f; //Ambient sensor value
float err = 0.0f; //Difference between setpoint and measured value
float prev_err = 0.0f; //Previous error
float d_err = 0.0f; //Rate of error change
float duty = 0.0f; //Current duty cycle

float current = 0.0f;
float power_consumed = 0.0f;

char line_buffer[14];

char error_line_1[14];
char error_line_2[14];
char error_line_3[14];
char error_line_4[14];
char error_line_5[14];

int state = 0; //State variable
float T = 0.000016129f; //Cycle time (measured)
float cycle_t = 0.0f; 
float open_val = 0.0f;

int seconds = 0;
int minutes = 0;
int hours = 0;
int delay = 260;

int prev_seconds = 0;
int prev_minutes = 0;
int prev_hours = 0;

bool levitating = false;


volatile float Kp = 11000.0; //Proportional value of PD controller
volatile float Kd = 65.0f; //Derivative value of PD controller
volatile float offset = 20.0f; //Control bias
volatile int selected_parameter = 0;

volatile bool select_button_flag = false;
volatile bool down_button_flag = false;
volatile bool up_button_flag = false;
volatile bool select_button_held_flag = false;
volatile bool down_button_held_flag = false;
volatile bool up_button_held_flag = false;


int main()
{
    Initialise();
    
    while(1)
    {
        switch(state)
        {
            case IDLE:
                Idle();
                break;
            case CALIBRATION:
                Calibrate();
                break;
            case TEST:
                Test();
                break;
            case LEVITATE:
                Levitate();
                break;
            case ERROR:
                Error();
                break;
        }
    }
}

void Initialise()
{
    DisplayPins.sce  = PB_8;
    DisplayPins.rst  = PB_9;
    DisplayPins.dc   = PB_6;
    DisplayPins.mosi = PA_7;
    DisplayPins.miso = NC;
    DisplayPins.sclk = PA_5;
    Display = new NokiaLcd(DisplayPins);
    Display->InitLcd();
    Display->ClearLcdMem();
    Display->SetXY(0,0);
    wait(0.1);
    CoilPWM.period_us(PWM_PERIOD_US);
    ref = REFERENCE;
    
    CoilPWM.period_us(PWM_PERIOD_US);
    CoilPWM = 0.0f;
    
    SelectButton.mode(PullDown);
    SelectButton.attach_asserted(&SelectButtonPress);
    SelectButton.attach_asserted_held(&SelectButtonHeld);
    SelectButton.setSampleFrequency();
    
    UpButton.mode(PullDown);
    UpButton.attach_asserted(&UpButtonPress);
    UpButton.attach_asserted_held(&UpButtonHeld);
    UpButton.setSampleFrequency();
    
    DownButton.mode(PullDown);
    DownButton.attach_asserted(&DownButtonPress);
    DownButton.attach_asserted_held(&DownButtonHeld);
    DownButton.setSampleFrequency();
    
    DisplayPattern(Display,1);
    
    
    SetRGB(true,false,false);
    wait(0.5);
    SetRGB(false,true,false);
    wait(0.5);
    SetRGB(false,false,true);
    wait(0.5);
    SetRGB(true,true,true);
    wait(2);
    Display->ClearLcdMem();
    SetRGB(false,false,false);
    
    ScreenUpdateTimer.attach(&UpdateScreen,1);
    
    SetState(TEST);
    
}

void Levitate()
{
    PDController();
    
    if( down_button_held_flag && up_button_held_flag && select_button_held_flag )
    {
        down_button_held_flag = false;
        up_button_held_flag = false;
        select_button_held_flag = false;
        
        SetState(CALIBRATION);
    }
    
    
    if( select_button_held_flag )
    {
        select_button_held_flag = false;
        select_button_flag = false;
        SetState(IDLE);
    }    
}

void PDController()
{
    pos = PosSensor.read();
    pos *= 3461.5f;
    
    err = pos - ref;
    
    d_err = (err-prev_err)/T;
    
    duty = 17*(Kp*err+Kd*d_err)+10000.0f;
    prev_err = err;

    
    if( pos < CLOSED || pos > OPEN )
    {
        CoilPWM.pulsewidth(0.0f);
        AveragePower.Insert(0.0f);
        AverageCurrent.Insert(0.0f);
    }
    else
    {
        if( duty > PWM_PERIOD_US )
        {
            CoilPWM.pulsewidth_us(MAX_PWM*PWM_PERIOD_US);
            //AveragePower.Insert()
            AverageCurrent.Insert(MAX_PWM*MAX_CURRENT);
        }
        else if( duty < 0.0f )
        {
            CoilPWM.pulsewidth_us(0.0f);
            AveragePower.Insert(0.0f);
            AverageCurrent.Insert(0.0f);
        }
    }
    
    
    
    wait_us(260); //Delay longer than PWM period needed to ensure the set duty cycle completes atleast one cycle
}

void Error()
{
    CoilPWM = 0.0f;
    
    if( select_button_held_flag )
    {
        select_button_held_flag = false;
        select_button_flag = false;
        SetState(TEST);
    }
}

void Test()
{
    bool test_passed = false;
    open_val = 3461.5f*PosSensor.read();
    amb = 3461.5f*AmbSensor.read();
    
    test_passed = ( open_val > amb );
    
    Display->ClearLcdMem();
    Display->SetXY(0,0);
    
    if( test_passed )
    {    
        Display->DrawString("Test Passed");
    }
    else
    {
        Display->DrawString("Test Failed");
    }
    
    wait(2);
    
    if( test_passed )
    {
        SetState(IDLE);
    }
    else
    {
        SetErrorLine(1,"Error");
        SetErrorLine(2,"!!!!!!!!!!!!!!");
        SetErrorLine(3,"Sensor Blocked");
        SetErrorLine(4,"or");
        SetErrorLine(5,"Disconnected");
        SetState(ERROR);
    }
}

void Idle()
{
    if( select_button_held_flag && down_button_held_flag && up_button_held_flag )
    {
        select_button_held_flag = false;
        down_button_held_flag = false;
        up_button_held_flag = false;
        
        SetState(CALIBRATION);
    }
    
    if( select_button_flag )
    {
        select_button_flag = false;
        SetState(LEVITATE);
    }
    
    CoilPWM = 0.0f;
}

void Calibrate()
{
    CycleTimer.start();
    
    if( select_button_held_flag )
    {
        select_button_held_flag = false;
        select_button_flag = false;
        SetState(IDLE);
    }
    
    if( up_button_held_flag )
    {
        up_button_held_flag = false;
        
        if( selected_parameter == KP_PARAM )
            Kp += 10.0f;
        else if( selected_parameter == KD_PARAM )
            Kd += 10.0f;
        else if( selected_parameter == REF_PARAM )
            ref += 10.0f;
    }
    
    if( down_button_held_flag )
    {
        down_button_held_flag = false;
        
        if( selected_parameter == KP_PARAM )
            Kp -= 10.0f;
        else if( selected_parameter == KD_PARAM )
            Kd -= 10.0f;
        else if( selected_parameter == REF_PARAM )
            ref -= 10.0f;
    }
    
    if( select_button_flag )
    {
        select_button_flag = false;
        
        if( selected_parameter < 2 )
            selected_parameter++;
        else
            selected_parameter = 0;
        
        
    }
    
    if( up_button_flag )
    {
        up_button_flag = false;
        
        if( selected_parameter == KP_PARAM )
            Kp += 5.0f;
        else if( selected_parameter == KD_PARAM )
            Kd += 5.0f;
        else if( selected_parameter == REF_PARAM )
            ref += 5.0f;
    }
    
    if( down_button_flag )
    {
        down_button_flag = false;
        
        if( selected_parameter == KP_PARAM )
            Kp -= 5.0f;
        else if( selected_parameter == KD_PARAM )
            Kd -= 5.0f;
        else if( selected_parameter == REF_PARAM )
            ref -= 5.0f;
    }
    
    PDController();
    
    CycleTimer.stop();
    cycle_t = CycleTimer.read()*1000000.0f;
    CycleTimer.reset();
}

void SetRGB(bool r, bool g, bool b)
{
    RedLed = (r ? 1 : 0 );
    GreenLed = (g ? 1 : 0 );
    BlueLed = (b ? 1 : 0 );
}

float RoundDigits(float x, int numdigits)
{ 
    return ceil(x * pow(10.0f,numdigits))/pow(10.0f,numdigits); 
}

void SetState(char to_state)
{
    Display->ClearLcdMem();
    Display->SetXY(0,0);
    CoilPWM = 0.0f;
    
    if( to_state == IDLE )
    {
        state = IDLE;
        SetRGB(false,true,false);
        DisplayPattern(Display,2);
    }
    else if( to_state == CALIBRATION)
    {
        state = CALIBRATION;
        SetRGB(true,true,false);
    }
    else if( to_state == TEST )
    {
        state = TEST;
        SetRGB(false,true,true);
        Display->DrawString("Testing");
    }
    else if( to_state == ERROR )
    {
        SetRGB(true,false,false);
        Display->DrawString(error_line_1);
        Display->SetXY(0,1);
        Display->DrawString(error_line_2);
        Display->SetXY(0,2);
        Display->DrawString(error_line_3);
        Display->SetXY(0,3);
        Display->DrawString(error_line_4);
        Display->SetXY(0,4);
        Display->DrawString(error_line_5);
        state = ERROR;
    }
    else if( to_state == LEVITATE )
    {
        Display->DrawString("Levitate");
        SetRGB(false,false,true);
        state = LEVITATE;
    }
}

void SelectButtonPress()
{
    select_button_flag = true;
}

void UpButtonPress()
{
    up_button_flag = true;
}

void DownButtonPress()
{
    down_button_flag = true;
}

void SelectButtonHeld()
{
    select_button_held_flag = true;
}

void UpButtonHeld()
{
    up_button_held_flag = true;
}

void DownButtonHeld()
{
    down_button_held_flag = true;
}

void UpdateScreen()
{
    power_consumed += (CONST_POWER + AverageCurrent.GetAverage()*COIL_VOLTAGE)/1000.0f;
    
    if( state == CALIBRATION )
    {
        Display->ClearLcdMem();
        Display->SetXY(0,0);
        Display->DrawString("Calibrate");
        
        Display->SetXY(0,1);
        sprintf(line_buffer,"Kp=%.2f",Kp);
        
        if( selected_parameter == KP_PARAM )
            Display->DrawChar('>');
        
        Display->DrawString(line_buffer);
        
        Display->SetXY(0,2);
        sprintf(line_buffer,"Kd=%.2f",Kd);
        
        if( selected_parameter == KD_PARAM )
            Display->DrawChar('>');
        
        Display->DrawString(line_buffer);
        
        Display->SetXY(0,3);
        sprintf(line_buffer,"T=%.2f",ref);
        
        if( selected_parameter == REF_PARAM )
            Display->DrawChar('>');
        
        Display->DrawString(line_buffer);   
        
        Display->SetXY(0,4);
        sprintf(line_buffer,"T=%.2fus",cycle_t);
        Display->DrawString(line_buffer);
        
        Display->SetXY(0,5);
        sprintf(line_buffer,"Pos=%.2f",pos);
        Display->DrawString(line_buffer);
    }
    
    if( state == LEVITATE )
    {
        if( pos < open_val-100)
        {
            UpdateTime();
            levitating = true;
        }
        else
        {
            prev_seconds = seconds;
            prev_minutes = minutes;
            prev_hours = hours;
            levitating = false;
            ResetTime();
        }
        
        
        Display->ClearLcdMem();
        Display->SetXY(0,0);
        Display->DrawString("Current:");
        //Display->SetXY(0,1);
        current = AverageCurrent.GetAverage();
        
        if( current < 0.0f )
            current = 0.0f;
        
        sprintf(line_buffer,"%.2fA",current);
        Display->DrawString(line_buffer);
        
        sprintf(line_buffer,"%.2fkWs",power_consumed);
        
        Display->SetXY(0,1);
        Display->DrawString("Consumption");
        Display->SetXY(0,2);
        Display->DrawString(line_buffer);
        
        if( levitating )
        {
            SetRGB(false,false,true);
            sprintf(line_buffer,"%dh%dm%ds",hours,minutes,seconds);
        }
        else
        {
            SetRGB(true,false,true);
            sprintf(line_buffer,"%dh%dm%ds",prev_hours,prev_minutes,prev_seconds);
        }
        
        Display->SetXY(0,3);
        Display->DrawString("Time:");
        //Display->SetXY(0,5);
        Display->DrawString(line_buffer);
        
        Display->SetXY(0,4);
        Display->DrawString("Airgap:15mm");
    }
    
    
}

void SetErrorLine(int line,char *message)
{
    char *temp;
    
    switch(line)
    {
        case 1:
            temp = error_line_1;
            break;
        case 2:
            temp = error_line_2;
            break;
        case 3:
            temp = error_line_3;
            break;
        case 4:
            temp = error_line_4;
            break;
        case 5:
            temp = error_line_5;
            break;
    }
    
    
    for(int i = 0; i < 14 ; i++ )
    {
        *temp = message[i];
        temp++;
    }
    
}

void ResetTime()
{
    seconds = 0;
    minutes = 0;
    hours = 0;
}

void UpdateTime()
{
    if( seconds < 60 )
        seconds++;
    else
    {
        seconds = 0;
        
        if( minutes < 60 )
            minutes++;
        else
        {
            minutes = 0;
            hours++;
        }
    }
        
}