Diff: ServoGUI.cpp

Revision:

0:befccd954577

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoGUI.cpp	Tue Jun 09 10:21:57 2020 +0000
@@ -0,0 +1,117 @@
+#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;
+    }  
+    
\ No newline at end of file