File content as of revision 62:58e846621435:

#include "mbed.h"
#include "Screens.hpp"
#include "ResistiveTouchController.hpp"
#include "CYS8218Controller.hpp"
#include "Map.hpp"
#include "MedianFilter.hpp"

//--- Function Prototypes
void GUIThread();
void Init();

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

//-----------------------

//-- GUI Thread Variables and Objects
int currentScreen = 0;
int nextScreen = 0;
Thread guiThread;
Screen *activeScreen = NULL;
//----------------------------------

//-- X Axis variables
float zeroAlpha = 0;
float alpha = 0;
float xSP = 0;
float x = 0;
float xError = 0;
float xPrevError = 0;
float xSummation = 0;
MedianFilter xFilter(7);

//-- Y Axis Variables
float zeroBeta = 0;
float beta = 0;
float ySP = 0;
float y = 0;
float yError = 0;
float yPrevError = 0;
float ySummation = 0;
MedianFilter yFilter(7);


//-- Controller Variables
float P = 0.1;
float I = 0;
float D = 9;
float Ts = 4;
Timer cycleTimer;
//-----------------------

float t[3][2];
float d[3][2];


//-- Sensors and Actuator objects
ResistiveTouchController plate(A0,A1,A3,A4,A5);
Map xMapper(0,1024, -170,170);
Map yMapper(0,1024, -136,136);
CYS8218Controller xMotor(D8);
CYS8218Controller yMotor(D9);


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

int measuredTs = 0;

bool started = false;

int ballOnPlate = 0;

bool saved = false;


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;
      case Screen::MANUAL_SETPOINT_SCREEN :
        Automatic();
        break;
      case Screen::SQUARE_SCREEN :
        Automatic();
        break;
      default :
        xMotor.Set(0);
        yMotor.Set(0);
        Thread::wait(20);
        break;
    }
  }
}

void GUIThread()
{
  while(1)
  {
    if( nextScreen != currentScreen )
    {
      started = false;
      xSummation = 0;
      ySummation = 0;
      Thread::wait(100);

      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, &calFinished, &calPoint, &calHeldTime);
          break;
        case Screen::SQUARE_SCREEN :
          activeScreen = new SquareScreen(&nextScreen,&xSP, &ySP, &ballOnPlate, &started, &xError, &yError );
          break;
      }
      activeScreen->Draw();
      currentScreen = nextScreen;
    }

    activeScreen->Process();

    Thread::wait(10);
  }
}

void Init()
{
  
  d[0][0] = 921.6;
  d[1][0] = 512;
  d[2][0] = 102.4;
  
  d[0][1] = 512;
  d[1][1] = 921.6;
  d[2][1] = 102.4;
  
  ImageFromSD splash("/sd/Splash.txt");
  xMotor.SetZero(28.0);
  yMotor.SetZero(25.0);
  nextScreen = Screen::MAIN_MENU_SCREEN;

  Thread::wait(5000);

  guiThread.start(GUIThread);
}

void ZeroPlate()
{

  xMotor.Set(zeroAlpha);
  yMotor.Set(-zeroBeta);

  if( saved)
  {
    xMotor.SetZero(zeroAlpha);
    yMotor.SetZero(-zeroBeta);
    zeroAlpha = 0;
    zeroBeta = 0;
    nextScreen = Screen::MAIN_SETTINGS_SCREEN;
    saved = false;
  }

  Thread::wait(20);
}

void Manual()
{
  xMotor.Set(-alpha);
  yMotor.Set(beta);
  Thread::wait(10);
}

void Automatic()
{
  //TODO Add automatic control mode logic
  ballOnPlate = plate.TouchDetected();

  if( ballOnPlate && started )
  {
    cycleTimer.start();
    plate.Measure();

    //x = xMapper.Calculate(floor(plate.X()));
    //y = yMapper.Calculate(floor(plate.Y()));

    x = xMapper.Calculate(floor(xFilter.Process(plate.X())));
    y = yMapper.Calculate(floor(yFilter.Process(plate.Y())));

    xError = xSP - x;
    yError = ySP - y;

    xSummation += xError;
    ySummation += yError;



    alpha = P*xError + D*(xError-xPrevError) + I*xSummation;
    beta = P*yError + D*(yError-yPrevError) + I*ySummation;

    xPrevError = xError;
    yPrevError = yError;

    alpha = ( alpha > 10 ? 10 : (alpha < -10 ? -10 : alpha));
    beta = ( beta > 10 ? 10 : (beta < -10 ? -10 : beta) );

    xMotor.Set(alpha);
    yMotor.Set(beta);

    float waitTime = Ts/1000.0;

    while( cycleTimer.read() < waitTime);

    cycleTimer.stop();
    measuredTs = cycleTimer.read_ms();
    cycleTimer.reset();
  }
  else
  {
    xMotor.Off();
    yMotor.Off();
  }

}

void Calibrate()
{
  ballOnPlate = plate.TouchDetected();

  if( calStarted )
  {
    if( calPoint < 3 )
    {
      if( ballOnPlate )
      {
        calTimer.start();
        calHeldTime = calTimer.read();
        plate.Measure();
        
        t[calPoint][0] = plate.RawX();
        t[calPoint][1] = plate.RawY();

        if( calHeldTime >= 5 )
        {
          while( plate.TouchDetected() );
          
          calPoint++;
          calTimer.stop();
          calTimer.reset();
        }
      }
      else
      {
        calTimer.stop();
        calTimer.reset();
      }
    }
    else
    {
      plate.Calibrate(t,d);
      calFinished = true;
      calPoint = 0;
      calStarted = false;
    }
  }
}