diff --git a/main.c b/main.c
index aa716b9..f52afb7 100644
--- a/main.c
+++ b/main.c
@@ -3,7 +3,7 @@
 #include <pic16f676.h>
 
 // CONFIGURATION
-#pragma config FOSC = INTRCIO   // Internal oscillator
+#pragma config FOSC = INTRCCLK   // Internal oscillator
 #pragma config WDTE = OFF       // Watchdog timer off
 #pragma config PWRTE = ON       // Power-up timer enabled
 #pragma config MCLRE = OFF
@@ -20,23 +20,20 @@ void __interrupt() ISR(void) {
     if (PIR1bits.TMR1IF) {
         PIR1bits.TMR1IF = 0;    // Clear interrupt flag
         T1CONbits.TMR1ON = 0;
-        static uint16_t off_ticks; // Retains value between interrupts
-        static uint16_t timer;
+        // Calculate pulse parameters
+        // 3000 ticks = 20ms   
+        uint8_t postoms= (uint16_t)(servo_pos*126) / 255;
+        uint16_t on_ticks = 16 + postoms * 2;
+        uint16_t off_ticks = 3000 - on_ticks;
         if (pulse_state == 0) {
-            // Calculate pulse parameters
-            uint16_t on_ticks = 125 + ((uint16_t)servo_pos * 125) / 255; // Map to 0.5ms-2.5ms
-            off_ticks = 2500 - on_ticks; // 2500 ticks = 20ms            
-                        
-            timer = 65535 - on_ticks;
-            TMR1L = (unsigned char)(timer & 0xFF);
-            TMR1H = (unsigned char)(timer >> 8);
-            RC5 = 1; // Start pulse
+            RC5 = 1; // Start pulse           
+            TMR1L = (0xFFFF - on_ticks)& 0xFF;//on_ticks;
+            TMR1H = (0xFFFF - on_ticks) >> 8;            
             pulse_state = 1;
         } else {            
-            timer = 65535 - off_ticks;
-            TMR1L = (unsigned char)(timer & 0xFF);
-            TMR1H = (unsigned char)(timer >> 8);
-            RC5 = 0;            // End pulse
+            RC5 = 0; // End pulse
+            TMR1L = (0xFFFF - off_ticks) & 0xFF;
+            TMR1H = (0xFFFF - off_ticks) >> 8;
             pulse_state = 0;
         }
         T1CONbits.TMR1ON = 1;
@@ -56,8 +53,8 @@ void setup() {
     
     // Configure Timer1
     T1CON = 0b00110000; // Timer1: prescaler 1:8, internal clock, OFF
-    TMR1H = (65536 - 188) >> 8; // Initial 1.5ms pulse (high byte)
-    TMR1L = (65536 - 188) & 0xFF; // Initial 1.5ms pulse (low byte)
+    TMR1H = (65535 - 188) >> 8; // Initial 1.5ms pulse (high byte)
+    TMR1L = (65535 - 188) & 0xFF; // Initial 1.5ms pulse (low byte)
     PIR1bits.TMR1IF = 0; // Clear interrupt flag
     PIE1bits.TMR1IE = 1; // Enable Timer1 interrupt
     INTCONbits.PEIE = 1; // Enable peripheral interrupts
@@ -119,18 +116,18 @@ void main() {
                 __delay_us(100);
                 break;
             case 1:
-                smooth_servo(128);
+                smooth_servo(94);
                 __delay_us(100);
                 break;
             case 2:
                 if(isincreasing) {                                            
                     smooth_servo(servo_pos + 1);
-                     __delay_ms(10);
+                     __delay_ms(5);
                     if(servo_pos + 1 >= 254) isincreasing = 0;
                 } else
                 {
                     smooth_servo(servo_pos - 1);
-                     __delay_ms(10);
+                     __delay_ms(5);
                     if(servo_pos - 1  <= 0) isincreasing = 1;
                 }
                 break;