File content as of revision 1:4fd32c2e7975:

#include "ServoGUI.h"
#include "mbed.h"
#include "mbed.h"

DigitalIn UserButton(USER_BUTTON);

enum eStep{RIGHT, LEFT};
enum eDetectorState{ACTIVE, OFF};

ServoGUI::ServoGUI(){
    LCD_Display.Clear(LCD_COLOR_WHITE);
    sPosition.X=120;
    sPosition.Y=210;
    ucCurrentPosition=0;
    ucStepPosition=0;
    ucCallibPosition=12;
    
    DrawServo();
    LedOn(0);
    Callib();
}

void ServoGUI::DrawServo(){
    LCD_Display.SetTextColor(LCD_COLOR_BLACK);
    LCD_Display.FillCircle(120,110,102);
    LCD_Display.SetTextColor(LCD_COLOR_BLUE);
    LCD_Display.FillCircle(120, 110, 100);
    LCD_Display.SetTextColor(LCD_COLOR_YELLOW);
    LCD_Display.DrawLine(120, 110, sPosition.X, sPosition.Y);
    LCD_Display.SetTextColor(LCD_COLOR_WHITE);
    LCD_Display.FillCircle(120, 110, 20);
    }  

void ServoGUI::DrawLed(unsigned char ucCurrentLed, uint32_t uiLedColor){
            LCD_Display.SetTextColor(LCD_COLOR_BLACK);
            LCD_Display.FillCircle(30+60*ucCurrentLed,270,20);
            LCD_Display.SetTextColor(uiLedColor);
            LCD_Display.FillCircle(30+60*ucCurrentLed,270,18);
    }
    
void ServoGUI::LedOn(unsigned char ucLed){
    unsigned char ucCurrentLed;
    for(ucCurrentLed=0; ucCurrentLed<NUMBER_OF_LEDS; ucCurrentLed++){
       if(ucCurrentLed==ucLed){
            DrawLed(ucCurrentLed,LCD_COLOR_BLUE);
            }
       else{
           DrawLed(ucCurrentLed, LCD_COLOR_WHITE);
           }
       }
    }  
    
void ServoGUI::Step(void){
    double dStep;
    dStep=((((ucCurrentPosition+ucCallibPosition)%48))%48*(3.1415/24));    
    sPosition.X=(120+100*cos(dStep));
    sPosition.Y=(110+100*sin(dStep));
    DrawServo();
    LedOn(ucCurrentPosition%4);
    wait(0.1);  
    }

void ServoGUI::MovePos(enum eStep eDirection){
     while(ucCurrentPosition!=ucStepPosition){
            if(eDirection==LEFT){    
                ucCurrentPosition--;      
                }
            else if(eDirection==RIGHT){
                ucCurrentPosition++;
                }
            else;
            ucCurrentPosition=ucCurrentPosition%48;
            Step();
            }
        }
   
  
    
void ServoGUI::GoTo(unsigned char ucPosition){
    ucStepPosition=ucPosition;
    
    if(ucCurrentPosition!=ucStepPosition){
        enum eStep eDirection= ucPosition < ucCurrentPosition ? LEFT : RIGHT;
        MovePos(eDirection);
        }
    else;
    }        
      
    
void ServoGUI::Shift(unsigned char ucLed){
    ucStepPosition+=ucLed;
    if(ucCurrentPosition!=ucStepPosition){
        enum eStep eDirection= ucStepPosition < ucCurrentPosition ? LEFT : RIGHT;
        ucStepPosition=ucStepPosition%48;
        MovePos(eDirection);
        }
    else;
    }   
    
enum eDetectorState ServoGUI::GetDetector(){  
    if(UserButton==1)
        return ACTIVE;
    else
        return OFF;
}  
    
void ServoGUI::Callib(){
    unsigned char ucCallibCounter=12;
    while(GetDetector()!=ACTIVE){
        Shift(1);
        ucCallibCounter++;
        }
    ucCallibPosition=ucCallibCounter%48;
    ucCurrentPosition=0;
    ucStepPosition=0;
    }