Adrian Nowak / Mbed 2 deprecated __M2__5b____copy

ll

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }