Robert Labuz / Mbed 2 deprecated mbed5a_testy

mbed5a_testy

Dependencies:   mbed

serwo.cpp@1:b8d65b5745d1, 2017-05-05 (annotated)

Committer:
Robsonik16
Date:
Fri May 05 20:01:41 2017 +0000
Revision:
1:b8d65b5745d1
a

Who changed what in which revision?

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