Diff: serwo.cpp

Revision:

1:b8d65b5745d1

Child:

2:0fcc9b85c8f6

diff -r e69a0b7f4b41 -r b8d65b5745d1 serwo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serwo.cpp	Fri May 05 20:01:41 2017 +0000
@@ -0,0 +1,92 @@
+//#include <LPC21xx.H>
+#include "serwo.h"
+#include "led.h"
+#include "mbed.h"
+//#include "timer_interrupts.h"
+
+#define DETECTOR_bm (1<<10)
+DigitalIn det(PA_0);
+Ticker SerwoTim;
+
+
+
+
+enum ServoState {CALLIB, IDDLE, IN_PROGRESS};
+struct Servo
+{
+enum ServoState eState;
+unsigned int uiCurrentPosition;
+unsigned int uiDesiredPosition;
+};
+struct Servo sServo;
+
+enum State {ACTIVE, INACTIVE};
+void DetectorInit(void){
+
+	//IO0DIR=IO0DIR&(~DETECTOR_bm);
+}
+
+enum State eReadDetector (void){
+
+	//if((IO0PIN&DETECTOR_bm) == 0){
+		if(det){
+		return(INACTIVE);
+	}else{
+		return(ACTIVE);
+	}
+}
+
+void Servo_Callib(void){
+
+	sServo.eState = CALLIB;
+	while(eReadDetector()==INACTIVE);
+}
+
+void Servo_GoTo(unsigned int uiPosition){
+
+	sServo.eState = IN_PROGRESS;
+	sServo.uiDesiredPosition = uiPosition;
+}
+
+void SerwoAutomat(){
+
+	 switch(sServo.eState){
+		case IDDLE: 	
+			if(sServo.uiCurrentPosition != sServo.uiDesiredPosition){
+				sServo.eState = IN_PROGRESS;
+			}else{
+				sServo.eState = IDDLE;
+			}
+			break;
+		case IN_PROGRESS: 
+			if(sServo.uiCurrentPosition > sServo.uiDesiredPosition){
+				Led_StepLeft();
+				sServo.eState = IN_PROGRESS;
+				sServo.uiCurrentPosition--;
+			}else if(sServo.uiCurrentPosition < sServo.uiDesiredPosition){
+				sServo.eState = IN_PROGRESS;
+				Led_StepRight();
+				sServo.uiCurrentPosition++;
+			}else{
+				sServo.eState = IDDLE;
+			}
+			break;
+		case CALLIB: 
+			if(eReadDetector()==INACTIVE){
+				Led_StepRight();
+			}else{
+				sServo.eState = IDDLE;
+				sServo.uiCurrentPosition = 0;
+				sServo.uiDesiredPosition = 0;				
+			}	
+			break;
+	 }
+}
+void Servo_Init(unsigned int uiServoFrequency){
+
+	//Timer0Interrupts_Init(1000000/uiServoFrequency, &SerwoAutomat);
+	SerwoTim.attach(&SerwoAutomat,float(1/float(uiServoFrequency)));
+	LedInt();
+	DetectorInit();
+	Servo_Callib();
+}