serwo.cpp

00001 //#include <LPC21xx.H>
00002 #include "serwo.h"
00003 #include "led.h"
00004 #include "mbed.h"
00005 //#include "timer_interrupts.h"
00006 
00007 #define DETECTOR_bm (1<<10)
00008 DigitalIn det(PA_0);
00009 Ticker SerwoTim;
00010 
00011 
00012 
00013 
00014 enum ServoState {CALLIB, IDDLE, IN_PROGRESS};
00015 struct Servo
00016 {
00017 enum ServoState eState;
00018 unsigned int uiCurrentPosition;
00019 unsigned int uiDesiredPosition;
00020 };
00021 struct Servo sServo;
00022 
00023 enum State {ACTIVE, INACTIVE};
00024 void DetectorInit(void){
00025 
00026     //IO0DIR=IO0DIR&(~DETECTOR_bm);
00027 }
00028 
00029 enum State eReadDetector (void){
00030 
00031     //if((IO0PIN&DETECTOR_bm) == 0){
00032         if(det){
00033         return(INACTIVE);
00034     }else{
00035         return(ACTIVE);
00036     }
00037 }
00038 
00039 void Servo_Callib(void){
00040 
00041     sServo.eState = CALLIB;
00042     while(eReadDetector()==INACTIVE);
00043 }
00044 
00045 void Servo_GoTo(unsigned int uiPosition){
00046 
00047     sServo.eState = IN_PROGRESS;
00048     sServo.uiDesiredPosition = uiPosition;
00049 }
00050 
00051 void SerwoAutomat(){
00052 
00053      switch(sServo.eState){
00054         case IDDLE:     
00055             if(sServo.uiCurrentPosition != sServo.uiDesiredPosition){
00056                 sServo.eState = IN_PROGRESS;
00057             }else{
00058                 sServo.eState = IDDLE;
00059             }
00060             break;
00061         case IN_PROGRESS: 
00062             if(sServo.uiCurrentPosition > sServo.uiDesiredPosition){
00063                 Led_StepLeft();
00064                 sServo.eState = IN_PROGRESS;
00065                 sServo.uiCurrentPosition--;
00066             }else if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){
00067                 sServo.eState = IN_PROGRESS;
00068                 Led_StepRight();
00069                 sServo.uiCurrentPosition++;
00070             }else{
00071                 sServo.eState = IDDLE;
00072             }
00073             break;
00074         case CALLIB: 
00075             if(eReadDetector()==INACTIVE){
00076                 Led_StepRight();
00077             }else{
00078                 sServo.eState = IDDLE;
00079                 sServo.uiCurrentPosition = 0;
00080                 sServo.uiDesiredPosition = 0;               
00081             }   
00082             break;
00083      }
00084 }
00085 void Servo_Init(unsigned int uiServoFrequency){
00086 
00087     //Timer0Interrupts_Init(1000000/uiServoFrequency, &SerwoAutomat);
00088     SerwoTim.attach(&SerwoAutomat,float(1/float(uiServoFrequency)));
00089     LedInt();
00090     DetectorInit();
00091     Servo_Callib();
00092 }