File content as of revision 36:9ca4125e526e:

#include "mbed.h"
#include "Screens.hpp"


void GUIThread();
void Init();

void ZeroPlate();
void Manual();
void Automatic();
void Calibrate();

int currentScreen = 0;
int nextScreen = 0;
Thread guiThread;
Screen *activeScreen = NULL;

float zeroAlpha = 0;
float alpha = 0;
float xSP = -20;
float x = 0;

float zeroBeta = 0;
float beta = 0;
float ySP = 50;
float y = 0;

float P = 0;
float I = 0;
float D = 0;
float Ts = 4;

float tCal[3][2];
int calPoint = 0;
bool calStarted = false;
Timer calTimer;
float calHeldTime;

int measuredTs = 0;

bool started = true;

int ballOnPlate = 1;

bool saved = false;

// TODO Delete these variables
float t = 0;
Serial pc(USBTX,USBRX);
DigitalIn test(D3);

int main()
{
  Init();

  while(1)
  {
    switch (currentScreen)
    {
      case Screen::ZERO_PLATE_SCREEN :
        ZeroPlate();
        break;
      case Screen::MANUAL_CONTROL_SCREEN :
        Manual();
        break;
      case Screen::AUTOMATIC_CONTROL_SCREEN :
        Automatic();
        break;
      case Screen::CALIBRATE_SCREEN :
        Calibrate();
        break;
    }
  }
}

void GUIThread()
{
  while(1)
  {
    if( nextScreen != currentScreen )
    {
      if( activeScreen != NULL)
        delete activeScreen;

      switch (nextScreen)
      {
        case Screen::MAIN_MENU_SCREEN :
          activeScreen = new MainMenuScreen(&nextScreen);
          break;
        case Screen::MAIN_SETTINGS_SCREEN :
          activeScreen = new MainSettingsScreen(&nextScreen);
          break;
        case Screen::ZERO_PLATE_SCREEN :
          activeScreen = new ZeroPlateScreen(&nextScreen, &zeroAlpha, &zeroBeta, &saved);
          break;
        case Screen::MANUAL_CONTROL_SCREEN :
          activeScreen = new ManualControlScreen(&nextScreen, &alpha, &beta);
          break;
        case Screen::AUTOMATIC_CONTROL_SCREEN :
          activeScreen = new AutomaticControlScreen(&nextScreen, &x, &y, &measuredTs, &started, &ballOnPlate, &xSP, &ySP);
          break;
        case Screen::AUTOMATIC_MORE_SCREEN :
          activeScreen = new AutomaticMoreScreen(&nextScreen);
          break;
        case Screen::MANUAL_SETPOINT_SCREEN :
          activeScreen = new ManualSetpointScreen(&nextScreen, &xSP, &ySP, &ballOnPlate, &started);
          break;
        case Screen::AUTOMATIC_SETTINGS_SCREEN :
          activeScreen = new AutomaticSettingsScreen(&nextScreen, &P, &I, &D, &Ts);
          break;
        case Screen::CHANGE_P_SCREEN :
          activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &P, "Change P");
          break;
        case Screen::CHANGE_I_SCREEN :
          activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &I, "Change I");
          break;
        case Screen::CHANGE_D_SCREEN :
          activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &D, "Change D");
          break;
        case Screen::CHANGE_TS_SCREEN :
          activeScreen = new ChangeValWholeScreen(&nextScreen, currentScreen, &Ts, "Change Ts", false, 4);
          break;
        case Screen::CALIBRATE_SCREEN :
          activeScreen = new CalibrateScreen(&nextScreen, &ballOnPlate, &calStarted, &calPoint, &calHeldTime);
          break;
      }

      activeScreen->Draw();
      Thread::wait(200);
      currentScreen = nextScreen;
    }

    activeScreen->Process();

    Thread::wait(10);
  }
}

void Init()
{
  pc.baud(57600);
  nextScreen = Screen::MAIN_MENU_SCREEN;
  guiThread.start(GUIThread);
}

void ZeroPlate()
{

  pc.printf("Zero a : %f | Zero b : %f\n\r", zeroAlpha, zeroBeta);


  if( saved)
  {
    // TODO Save zero position on servos
    nextScreen = Screen::MAIN_SETTINGS_SCREEN;
    saved = false;
  }

  Thread::wait(20);
}

void Manual()
{
  // TODO Add manual control logic
}

void Automatic()
{
  //TODO Add automatic control mode logic
  ballOnPlate = test.read();
  Thread::wait(4);
}

void Calibrate()
{
  ballOnPlate = test.read();

  if( calStarted )
  {
    if( calPoint < 3 )
    {
      if( ballOnPlate )
      {
        calTimer.start();
        calHeldTime = calTimer.read();

        if( calHeldTime >= 5 && ballOnPlate )
        {
          while( test.read() );

          calPoint++;
          calTimer.stop();
          calTimer.reset();
        }
      }
    }
  }

  while( calStarted )
  {
    while( calPoint < 3 )
    {
      //TODO Reaplace test with plate
      if( test.read() )
      {
        calTimer.start();
        //TODO measure and save screen values;

        while( calTimer.read() < 5 )
        {
          calHeldTime = calTimer.read();
        }

        while(test.read());

        calPoint++;
        calTimer.stop;
        calTimer.reset();
      }
      else
      {
        calTimer.stop;
        calTimer.reset();
      }
    }

    calStarted = false;
    calPoint = 0;
  }
}