Luca Jel / Mbed 2 deprecated mbed5a_testy

sdsd

Dependencies:   mbed

Fork of mbed5a_testy by Robert Labuz

serwo.cpp@2:0fcc9b85c8f6, 2017-06-20 (annotated)

Committer:
deerjelen
Date:
Tue Jun 20 16:02:06 2017 +0000
Revision:
2:0fcc9b85c8f6
Parent:
1:b8d65b5745d1 
xcx

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
deerjelen 2:0fcc9b85c8f6 31 if(det){
Robsonik16 1:b8d65b5745d1 32 return(INACTIVE);
Robsonik16 1:b8d65b5745d1 33 }else{
Robsonik16 1:b8d65b5745d1 34 return(ACTIVE);
Robsonik16 1:b8d65b5745d1 35 }
Robsonik16 1:b8d65b5745d1 36 }
Robsonik16 1:b8d65b5745d1 37
Robsonik16 1:b8d65b5745d1 38 void Servo_Callib(void){
Robsonik16 1:b8d65b5745d1 39
Robsonik16 1:b8d65b5745d1 40 sServo.eState = CALLIB;
Robsonik16 1:b8d65b5745d1 41 while(eReadDetector()==INACTIVE);
Robsonik16 1:b8d65b5745d1 42 }
Robsonik16 1:b8d65b5745d1 43
Robsonik16 1:b8d65b5745d1 44 void Servo_GoTo(unsigned int uiPosition){
Robsonik16 1:b8d65b5745d1 45
Robsonik16 1:b8d65b5745d1 46 sServo.eState = IN_PROGRESS;
Robsonik16 1:b8d65b5745d1 47 sServo.uiDesiredPosition = uiPosition;
Robsonik16 1:b8d65b5745d1 48 }
Robsonik16 1:b8d65b5745d1 49
Robsonik16 1:b8d65b5745d1 50 void SerwoAutomat(){
Robsonik16 1:b8d65b5745d1 51
Robsonik16 1:b8d65b5745d1 52 switch(sServo.eState){
Robsonik16 1:b8d65b5745d1 53 case IDDLE:
Robsonik16 1:b8d65b5745d1 54 if(sServo.uiCurrentPosition != sServo.uiDesiredPosition){
Robsonik16 1:b8d65b5745d1 55 sServo.eState = IN_PROGRESS;
Robsonik16 1:b8d65b5745d1 56 }else{
Robsonik16 1:b8d65b5745d1 57 sServo.eState = IDDLE;
Robsonik16 1:b8d65b5745d1 58 }
Robsonik16 1:b8d65b5745d1 59 break;
Robsonik16 1:b8d65b5745d1 60 case IN_PROGRESS:
Robsonik16 1:b8d65b5745d1 61 if(sServo.uiCurrentPosition > sServo.uiDesiredPosition){
Robsonik16 1:b8d65b5745d1 62 Led_StepLeft();
Robsonik16 1:b8d65b5745d1 63 sServo.eState = IN_PROGRESS;
Robsonik16 1:b8d65b5745d1 64 sServo.uiCurrentPosition--;
Robsonik16 1:b8d65b5745d1 65 }else if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){
Robsonik16 1:b8d65b5745d1 66 sServo.eState = IN_PROGRESS;
Robsonik16 1:b8d65b5745d1 67 Led_StepRight();
Robsonik16 1:b8d65b5745d1 68 sServo.uiCurrentPosition++;
Robsonik16 1:b8d65b5745d1 69 }else{
Robsonik16 1:b8d65b5745d1 70 sServo.eState = IDDLE;
Robsonik16 1:b8d65b5745d1 71 }
Robsonik16 1:b8d65b5745d1 72 break;
Robsonik16 1:b8d65b5745d1 73 case CALLIB:
Robsonik16 1:b8d65b5745d1 74 if(eReadDetector()==INACTIVE){
Robsonik16 1:b8d65b5745d1 75 Led_StepRight();
Robsonik16 1:b8d65b5745d1 76 }else{
Robsonik16 1:b8d65b5745d1 77 sServo.eState = IDDLE;
Robsonik16 1:b8d65b5745d1 78 sServo.uiCurrentPosition = 0;
Robsonik16 1:b8d65b5745d1 79 sServo.uiDesiredPosition = 0;
Robsonik16 1:b8d65b5745d1 80 }
Robsonik16 1:b8d65b5745d1 81 break;
Robsonik16 1:b8d65b5745d1 82 }
Robsonik16 1:b8d65b5745d1 83 }
Robsonik16 1:b8d65b5745d1 84 void Servo_Init(unsigned int uiServoFrequency){
Robsonik16 1:b8d65b5745d1 85
Robsonik16 1:b8d65b5745d1 86 //Timer0Interrupts_Init(1000000/uiServoFrequency, &SerwoAutomat);
Robsonik16 1:b8d65b5745d1 87 SerwoTim.attach(&SerwoAutomat,float(1/float(uiServoFrequency)));
Robsonik16 1:b8d65b5745d1 88 LedInt();
Robsonik16 1:b8d65b5745d1 89 DetectorInit();
Robsonik16 1:b8d65b5745d1 90 Servo_Callib();
Robsonik16 1:b8d65b5745d1 91 }