1 files changed, 23 insertions, 0 deletions
diff --git a/Lab4.X/lab4_servo.c b/Lab4.X/lab4_servo.c
new file mode 100644
index 0000000..9ff5b42
--- /dev/null
+++ b/Lab4.X/lab4_servo.c
@@ -0,0 +1,23 @@
+#include "xc.h"
+#include "lab4_servo.h"
+void setServo(int Val)
+{
+    OC1RS=Val;
+}
+void initServo() {
+    TRISB &= 1111111111011111;
+    //TIMER 3
+    T3CON = 0x0010; //Stop Timer, Tcy clk source, PRE 1:8
+    T3CONbits.TCKPS = 0b01;
+    T3CONbits.TCS=0;
+    TMR3 = 0;     // Initialize to zero
+    PR3 = 39999; // Set period in order to make 20ms cycle
+    T3CONbits.TON = 1;
+    
+    //OC1
+    OC1CON = 0;
+    OC1R = 1234;   // servo start position. We won?t touch OC1R again
+    OC1RS = 1234;  // We will only change this once PWM is turned on
+    OC1CONbits.OCM = 0b110;
+    OC1CONbits.OCTSEL = 1;
+}