Robert Labuz / Mbed 2 deprecated mbed5b

mbed5b

Dependencies:   mbed

serwo.cpp@0:d546b014a1e3, 2017-05-05 (annotated)

Committer:
Robsonik16
Date:
Fri May 05 20:00:16 2017 +0000
Revision:
0:d546b014a1e3
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Robsonik16 0:d546b014a1e3 1 //#include <LPC21xx.H>
Robsonik16 0:d546b014a1e3 2 #include "serwo.h"
Robsonik16 0:d546b014a1e3 3 #include "led.h"
Robsonik16 0:d546b014a1e3 4 #include "mbed.h"
Robsonik16 0:d546b014a1e3 5 //#include "timer_interrupts.h"
Robsonik16 0:d546b014a1e3 6
Robsonik16 0:d546b014a1e3 7 #define DETECTOR_bm (1<<10)
Robsonik16 0:d546b014a1e3 8 DigitalIn det(PA_0);
Robsonik16 0:d546b014a1e3 9 Ticker SerwoTim;
Robsonik16 0:d546b014a1e3 10
Robsonik16 0:d546b014a1e3 11
Robsonik16 0:d546b014a1e3 12
Robsonik16 0:d546b014a1e3 13
Robsonik16 0:d546b014a1e3 14 enum ServoState {CALLIB, IDDLE, IN_PROGRESS};
Robsonik16 0:d546b014a1e3 15 struct Servo
Robsonik16 0:d546b014a1e3 16 {
Robsonik16 0:d546b014a1e3 17 enum ServoState eState;
Robsonik16 0:d546b014a1e3 18 unsigned int uiCurrentPosition;
Robsonik16 0:d546b014a1e3 19 unsigned int uiDesiredPosition;
Robsonik16 0:d546b014a1e3 20 };
Robsonik16 0:d546b014a1e3 21 struct Servo sServo;
Robsonik16 0:d546b014a1e3 22
Robsonik16 0:d546b014a1e3 23 enum State {ACTIVE, INACTIVE};
Robsonik16 0:d546b014a1e3 24 void DetectorInit(void){
Robsonik16 0:d546b014a1e3 25
Robsonik16 0:d546b014a1e3 26 //IO0DIR=IO0DIR&(~DETECTOR_bm);
Robsonik16 0:d546b014a1e3 27 }
Robsonik16 0:d546b014a1e3 28
Robsonik16 0:d546b014a1e3 29 enum State eReadDetector (void){
Robsonik16 0:d546b014a1e3 30
Robsonik16 0:d546b014a1e3 31 //if((IO0PIN&DETECTOR_bm) == 0){
Robsonik16 0:d546b014a1e3 32 if(det){
Robsonik16 0:d546b014a1e3 33 return(INACTIVE);
Robsonik16 0:d546b014a1e3 34 }else{
Robsonik16 0:d546b014a1e3 35 return(ACTIVE);
Robsonik16 0:d546b014a1e3 36 }
Robsonik16 0:d546b014a1e3 37 }
Robsonik16 0:d546b014a1e3 38
Robsonik16 0:d546b014a1e3 39 void Servo_Callib(void){
Robsonik16 0:d546b014a1e3 40
Robsonik16 0:d546b014a1e3 41 sServo.eState = CALLIB;
Robsonik16 0:d546b014a1e3 42 while(eReadDetector()==INACTIVE);
Robsonik16 0:d546b014a1e3 43 }
Robsonik16 0:d546b014a1e3 44
Robsonik16 0:d546b014a1e3 45 void Servo_GoTo(unsigned int uiPosition){
Robsonik16 0:d546b014a1e3 46
Robsonik16 0:d546b014a1e3 47 sServo.eState = IN_PROGRESS;
Robsonik16 0:d546b014a1e3 48 sServo.uiDesiredPosition = uiPosition;
Robsonik16 0:d546b014a1e3 49 }
Robsonik16 0:d546b014a1e3 50
Robsonik16 0:d546b014a1e3 51 void SerwoAutomat(){
Robsonik16 0:d546b014a1e3 52
Robsonik16 0:d546b014a1e3 53 switch(sServo.eState){
Robsonik16 0:d546b014a1e3 54 case IDDLE:
Robsonik16 0:d546b014a1e3 55 if(sServo.uiCurrentPosition != sServo.uiDesiredPosition){
Robsonik16 0:d546b014a1e3 56 sServo.eState = IN_PROGRESS;
Robsonik16 0:d546b014a1e3 57 }else{
Robsonik16 0:d546b014a1e3 58 sServo.eState = IDDLE;
Robsonik16 0:d546b014a1e3 59 }
Robsonik16 0:d546b014a1e3 60 break;
Robsonik16 0:d546b014a1e3 61 case IN_PROGRESS:
Robsonik16 0:d546b014a1e3 62 if(sServo.uiCurrentPosition > sServo.uiDesiredPosition){
Robsonik16 0:d546b014a1e3 63 Led_StepLeft();
Robsonik16 0:d546b014a1e3 64 sServo.eState = IN_PROGRESS;
Robsonik16 0:d546b014a1e3 65 sServo.uiCurrentPosition--;
Robsonik16 0:d546b014a1e3 66 }else if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){
Robsonik16 0:d546b014a1e3 67 sServo.eState = IN_PROGRESS;
Robsonik16 0:d546b014a1e3 68 Led_StepRight();
Robsonik16 0:d546b014a1e3 69 sServo.uiCurrentPosition++;
Robsonik16 0:d546b014a1e3 70 }else{
Robsonik16 0:d546b014a1e3 71 sServo.eState = IDDLE;
Robsonik16 0:d546b014a1e3 72 }
Robsonik16 0:d546b014a1e3 73 break;
Robsonik16 0:d546b014a1e3 74 case CALLIB:
Robsonik16 0:d546b014a1e3 75 if(eReadDetector()==INACTIVE){
Robsonik16 0:d546b014a1e3 76 Led_StepRight();
Robsonik16 0:d546b014a1e3 77 }else{
Robsonik16 0:d546b014a1e3 78 sServo.eState = IDDLE;
Robsonik16 0:d546b014a1e3 79 sServo.uiCurrentPosition = 0;
Robsonik16 0:d546b014a1e3 80 sServo.uiDesiredPosition = 0;
Robsonik16 0:d546b014a1e3 81 }
Robsonik16 0:d546b014a1e3 82 break;
Robsonik16 0:d546b014a1e3 83 }
Robsonik16 0:d546b014a1e3 84 }
Robsonik16 0:d546b014a1e3 85 void Servo_Init(unsigned int uiServoFrequency){
Robsonik16 0:d546b014a1e3 86
Robsonik16 0:d546b014a1e3 87 //Timer0Interrupts_Init(1000000/uiServoFrequency, &SerwoAutomat);
Robsonik16 0:d546b014a1e3 88 SerwoTim.attach(&SerwoAutomat,float(1/float(uiServoFrequency)));
Robsonik16 0:d546b014a1e3 89 LedInt();
Robsonik16 0:d546b014a1e3 90 DetectorInit();
Robsonik16 0:d546b014a1e3 91 Servo_Callib();
Robsonik16 0:d546b014a1e3 92 }