servo_gui.cpp

00001 #include "servo_gui.h"
00002 
00003 
00004 DigitalIn MyCallibButton(USER_BUTTON);
00005 
00006 ServoGui::ServoGui(){
00007     lcd.Clear(LCD_COLOR_WHITE);
00008     ui8ActualLedPoint = 0;
00009     DrawDisk();
00010     LedOn(0);
00011     
00012 }
00013 
00014 enum eState ServoGui::eReadDetector(){
00015     if(MyCallibButton == 1){
00016         return ON;
00017     }
00018     else {
00019         return OFF;
00020     }    
00021 }    
00022 
00023 void ServoGui::ServoCallib(){
00024     while(eReadDetector() == OFF){
00025         LedStep(RIGHT);
00026         wait(DELAY);    
00027     }
00028     sServo.uiCurrentPosition = ui8ActualLedPoint;
00029     sServo.uiDesiredPosition = ui8ActualLedPoint;
00030     
00031 }
00032 
00033 void ServoGui::ServoGoTo(uint8_t ui8Position){
00034   sServo.uiDesiredPosition = ui8Position;
00035   while(sServo.uiCurrentPosition != sServo.uiDesiredPosition){
00036       if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){
00037             LedStep(LEFT);
00038             sServo.uiCurrentPosition++;
00039        }
00040        else if(sServo.uiCurrentPosition > sServo.uiDesiredPosition){
00041            LedStep(RIGHT);
00042            sServo.uiCurrentPosition--;   
00043        } 
00044        wait(DELAY);  
00045   }   
00046 }
00047 
00048 void ServoGui::DrawLed(uint8_t ui8LedNumber){
00049     
00050     if(ui8LedNumber <= 3){
00051         lcd.SetTextColor(LCD_COLOR_BLACK);
00052         lcd.DrawCircle(LED_X + LED_DISTANCE*ui8LedNumber,LED_Y,LED_RADIUS); 
00053     }
00054     if(eLedStates[ui8LedNumber] == ON){
00055         lcd.SetTextColor(LCD_COLOR_BLUE);
00056     }
00057     else{
00058         lcd.SetTextColor(LCD_COLOR_WHITE);
00059     }
00060     
00061     lcd.FillCircle(LED_X+LED_DISTANCE*ui8LedNumber,LED_Y ,LED_RADIUS-2);
00062 }  
00063 
00064 
00065 void ServoGui::LedOn(uint8_t ui8LedNumber){
00066       for(uint8_t ui8LedCounter = 0; ui8LedCounter <= 3; ui8LedCounter++){
00067             eLedStates[ui8LedCounter] = OFF;
00068             DrawLed(ui8LedCounter);
00069       }
00070           if(ui8LedNumber <= 3){
00071             eLedStates[ui8LedNumber] = ON;
00072             DrawLed(ui8LedNumber);
00073           }    
00074 }  
00075 
00076 void ServoGui::LedStep(enum eDirection eWhere){
00077     if(eWhere == RIGHT){
00078         ui8ActualLedPoint = ui8ActualLedPoint + 1;
00079     }
00080     else if(eWhere == LEFT){
00081         ui8ActualLedPoint = ui8ActualLedPoint - 1;
00082     }
00083     LedOn(ui8ActualLedPoint%4);
00084     DrawDisk();   
00085 }
00086 
00087 void ServoGui::DrawDisk(){
00088     double dAngleOfPointer = (ui8ActualLedPoint%MAX_STEPS);
00089     dAngleOfPointer = dAngleOfPointer/ MAX_STEPS;
00090     dAngleOfPointer = dAngleOfPointer * 2 * PI;
00091     
00092     lcd.SetTextColor(LCD_COLOR_BLACK);
00093     lcd.FillCircle(DISK_X,DISK_Y,DISK_RADIUS);
00094     lcd.SetTextColor(LCD_COLOR_WHITE);
00095     lcd.DrawLine(DISK_X,DISK_Y,DISK_X+(DISK_RADIUS * cos(dAngleOfPointer)), DISK_Y + (DISK_RADIUS * sin(dAngleOfPointer)));
00096     lcd.FillCircle(DISK_X,DISK_Y,SMALL_DISK_RADIUS);    
00097 }