Diff: main.cpp

Revision:

0:d7cc9bad311b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Sep 09 18:45:52 2018 +0000
@@ -0,0 +1,663 @@
+#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++;
+        }
+    }
+        
+}
+