Adrian Nowak
/
__M2__5b____copy
ll
servo.cpp@0:a8e3f4b25a52, 2017-05-09 (annotated)
- Committer:
- adrianow795
- Date:
- Tue May 09 11:59:23 2017 +0000
- Revision:
- 0:a8e3f4b25a52
siema;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
adrianow795 | 0:a8e3f4b25a52 | 1 | #include "servo.h" |
adrianow795 | 0:a8e3f4b25a52 | 2 | #include "led.h" |
adrianow795 | 0:a8e3f4b25a52 | 3 | #include "mbed.h" |
adrianow795 | 0:a8e3f4b25a52 | 4 | |
adrianow795 | 0:a8e3f4b25a52 | 5 | Ticker timer; |
adrianow795 | 0:a8e3f4b25a52 | 6 | DigitalIn detector(PG_6); |
adrianow795 | 0:a8e3f4b25a52 | 7 | |
adrianow795 | 0:a8e3f4b25a52 | 8 | enum eStan {ACTIVE, INACTIVE}; |
adrianow795 | 0:a8e3f4b25a52 | 9 | |
adrianow795 | 0:a8e3f4b25a52 | 10 | enum eStan eReadDetector(){ |
adrianow795 | 0:a8e3f4b25a52 | 11 | if(1==detector){ |
adrianow795 | 0:a8e3f4b25a52 | 12 | return INACTIVE; |
adrianow795 | 0:a8e3f4b25a52 | 13 | } |
adrianow795 | 0:a8e3f4b25a52 | 14 | else{ |
adrianow795 | 0:a8e3f4b25a52 | 15 | return ACTIVE; |
adrianow795 | 0:a8e3f4b25a52 | 16 | } |
adrianow795 | 0:a8e3f4b25a52 | 17 | } |
adrianow795 | 0:a8e3f4b25a52 | 18 | |
adrianow795 | 0:a8e3f4b25a52 | 19 | enum ServoState {CALLIB, IDDLE, IN_PROGRESS}; |
adrianow795 | 0:a8e3f4b25a52 | 20 | |
adrianow795 | 0:a8e3f4b25a52 | 21 | struct Servo |
adrianow795 | 0:a8e3f4b25a52 | 22 | { |
adrianow795 | 0:a8e3f4b25a52 | 23 | enum ServoState eState; |
adrianow795 | 0:a8e3f4b25a52 | 24 | unsigned int uiCurrentPosition; |
adrianow795 | 0:a8e3f4b25a52 | 25 | unsigned int uiDesiredPosition; |
adrianow795 | 0:a8e3f4b25a52 | 26 | }; |
adrianow795 | 0:a8e3f4b25a52 | 27 | |
adrianow795 | 0:a8e3f4b25a52 | 28 | struct Servo sServo; |
adrianow795 | 0:a8e3f4b25a52 | 29 | |
adrianow795 | 0:a8e3f4b25a52 | 30 | void Automat(){ |
adrianow795 | 0:a8e3f4b25a52 | 31 | switch(sServo.eState){ |
adrianow795 | 0:a8e3f4b25a52 | 32 | case CALLIB: |
adrianow795 | 0:a8e3f4b25a52 | 33 | if(eReadDetector()==INACTIVE){ |
adrianow795 | 0:a8e3f4b25a52 | 34 | Led_StepLeft(); |
adrianow795 | 0:a8e3f4b25a52 | 35 | sServo.eState=CALLIB; |
adrianow795 | 0:a8e3f4b25a52 | 36 | } |
adrianow795 | 0:a8e3f4b25a52 | 37 | else{ |
adrianow795 | 0:a8e3f4b25a52 | 38 | sServo.uiCurrentPosition=0; |
adrianow795 | 0:a8e3f4b25a52 | 39 | sServo.uiDesiredPosition=0; |
adrianow795 | 0:a8e3f4b25a52 | 40 | sServo.eState=IDDLE; |
adrianow795 | 0:a8e3f4b25a52 | 41 | } |
adrianow795 | 0:a8e3f4b25a52 | 42 | break; |
adrianow795 | 0:a8e3f4b25a52 | 43 | case IDDLE: |
adrianow795 | 0:a8e3f4b25a52 | 44 | if(sServo.uiCurrentPosition != sServo.uiDesiredPosition){ |
adrianow795 | 0:a8e3f4b25a52 | 45 | sServo.eState=IN_PROGRESS; |
adrianow795 | 0:a8e3f4b25a52 | 46 | } |
adrianow795 | 0:a8e3f4b25a52 | 47 | else{ |
adrianow795 | 0:a8e3f4b25a52 | 48 | sServo.eState=IDDLE; |
adrianow795 | 0:a8e3f4b25a52 | 49 | } |
adrianow795 | 0:a8e3f4b25a52 | 50 | break; |
adrianow795 | 0:a8e3f4b25a52 | 51 | case IN_PROGRESS: |
adrianow795 | 0:a8e3f4b25a52 | 52 | if(sServo.uiCurrentPosition == sServo.uiDesiredPosition){ |
adrianow795 | 0:a8e3f4b25a52 | 53 | sServo.eState=IDDLE; |
adrianow795 | 0:a8e3f4b25a52 | 54 | } |
adrianow795 | 0:a8e3f4b25a52 | 55 | else if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){ |
adrianow795 | 0:a8e3f4b25a52 | 56 | Led_StepRight(); |
adrianow795 | 0:a8e3f4b25a52 | 57 | sServo.uiCurrentPosition++; |
adrianow795 | 0:a8e3f4b25a52 | 58 | sServo.eState=IN_PROGRESS; |
adrianow795 | 0:a8e3f4b25a52 | 59 | } |
adrianow795 | 0:a8e3f4b25a52 | 60 | else { |
adrianow795 | 0:a8e3f4b25a52 | 61 | Led_StepLeft(); |
adrianow795 | 0:a8e3f4b25a52 | 62 | sServo.uiCurrentPosition--; |
adrianow795 | 0:a8e3f4b25a52 | 63 | sServo.eState=IN_PROGRESS; |
adrianow795 | 0:a8e3f4b25a52 | 64 | } |
adrianow795 | 0:a8e3f4b25a52 | 65 | break; |
adrianow795 | 0:a8e3f4b25a52 | 66 | } |
adrianow795 | 0:a8e3f4b25a52 | 67 | } |
adrianow795 | 0:a8e3f4b25a52 | 68 | |
adrianow795 | 0:a8e3f4b25a52 | 69 | void Servo_Init(unsigned int uiServoFrequency){ |
adrianow795 | 0:a8e3f4b25a52 | 70 | float fServoFrequency; |
adrianow795 | 0:a8e3f4b25a52 | 71 | const float ucConstOne=1; |
adrianow795 | 0:a8e3f4b25a52 | 72 | fServoFrequency= ucConstOne / uiServoFrequency; |
adrianow795 | 0:a8e3f4b25a52 | 73 | Led_Init(); |
adrianow795 | 0:a8e3f4b25a52 | 74 | sServo.eState=CALLIB; |
adrianow795 | 0:a8e3f4b25a52 | 75 | timer.attach(&Automat,fServoFrequency); |
adrianow795 | 0:a8e3f4b25a52 | 76 | } |
adrianow795 | 0:a8e3f4b25a52 | 77 | |
adrianow795 | 0:a8e3f4b25a52 | 78 | void Servo_Callib(void){ |
adrianow795 | 0:a8e3f4b25a52 | 79 | sServo.eState=CALLIB; |
adrianow795 | 0:a8e3f4b25a52 | 80 | } |
adrianow795 | 0:a8e3f4b25a52 | 81 | |
adrianow795 | 0:a8e3f4b25a52 | 82 | void Servo_GoTo(unsigned int uiPosition){ |
adrianow795 | 0:a8e3f4b25a52 | 83 | sServo.uiDesiredPosition=uiPosition; |
adrianow795 | 0:a8e3f4b25a52 | 84 | } |
adrianow795 | 0:a8e3f4b25a52 | 85 | |
adrianow795 | 0:a8e3f4b25a52 | 86 | void Servo_Step(unsigned int uiStep){ |
adrianow795 | 0:a8e3f4b25a52 | 87 | sServo.uiDesiredPosition=sServo.uiCurrentPosition+uiStep; |
adrianow795 | 0:a8e3f4b25a52 | 88 | } |