   TCCR1B = (capture_mode << 6) | ((mode & 0xC) << 1) | clock_mode ;
 }
 
-// Setup the one-shot pulse generator
-
-void osp_setup() {
-
-  timer1_setup(0x0, 1, 0x0, 0x0, 0x0);
-
-}
-
 // Fire a one-shot pulse. Use the most recently set width. 
 
-#define OSP_FIRE() (TCNT1 = OCR1B - 1)
+#define OSP_FIRE() (TCNT2 = OCR2B - 1)
 
 // Test there is currently a pulse still in progress
 
-#define OSP_INPROGRESS() (TCNT1>0)
+#define OSP_INPROGRESS() (TCNT2>0)
 
 // Fire a one-shot pusle with the specififed width. 
 // Order of operations in calculating m must avoid overflow of the unint8_t.
-// TCNT1 starts one count lower than the match value becuase the chip will block any compare on the cycle after setting a TCNT. 
+// TCNT2 starts one count lower than the match value becuase the chip will block any compare on the cycle after setting a TCNT. 
 
-#define OSP_SET_AND_FIRE(cycles) {uint16_t m=0xffff-(cycles-1); OCR1B=m;TCNT1 =m-1;}
+#define OSP_SET_AND_FIRE(cycles) {uint8_t m=0xff-(cycles-1); OCR2B=m;TCNT2 =m-1;}
 
 
 void setup()
 {
   Serial.begin(38400);
-  osp_setup();
+  timer1_setup();
   //OSP_SET_WIDTH(254);
   start = micros();
+  //Serial.println(F_CPU);
+  //noInterrupts();
 }
 
 void loop()
 {
-  long time = micros();
-  //pos = round(amp*sin(2*PI*0.5*(time-start)/1000000.0) + mid);
-  noInterrupts();
-  OSP_SET_AND_FIRE(250);
+  
+  
+  //pos = round(amp*sin(2*PI*0.5*(time1-start)/1000000.0) + mid);
+  //double pulseLength = 900*sin(2*PI*0.5*(time1-start)/1000000.0) + 1500;
+  for (int i = 1; i < 3; i++)
+  {
+    TCCR1A = 0;     // disable all PWM on Timer1 whilst we set it up
+    OCR1A = ICR1 * i / 20; // 2ms pulse to left motor on PB1
+    TCCR1A |= 2 <<  6;  // enable PWM on port B1 in non-inverted compare mode 2
+  }
+  //noInterrupts();
+  //OSP_SET_AND_FIRE(250);
+  long time1 = micros();
   while (OSP_INPROGRESS()){};
-  interrupts();
+  //interrupts();
   long time2 = micros();
-  Serial.println(time2-time);
-  _delay_ms(20);
+  Serial.println(time2-time1);
+  delay(500);
+  //_delay_ms(20);
   
   // Step though 0-19 cycle long pulses for demo purposes 
   /*long start = micros();


// Setup the one-shot pulse generator and initialize with a pulse width that is (cycles) clock counts long

void osp_setup(uint16_t cycles) {


  TCCR1B =  0;      // Halt counter by setting clock select bits to 0 (No clock source).
              // This keeps anyhting from happeneing while we get set up

  TCNT1 = 0x0000;     // Start counting at bottom. 
  OCR1A = 0;      // Set TOP to 0. This effectively keeps us from counting because the counter just keeps reseting back to 0.
          // We break out of this by manually setting the TCNT higher than 0, in which case it will count all the way up to MAX and then overflow back to 0 and get locked up again.
  OSP_SET_WIDTH(cycles);    // This also makes new OCR values get loaded frm the buffer on every clock cycle. 

  TCCR1A = _BV(COM2B0) | _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // OC2B=Set on Match, clear on BOTTOM. Mode 7 Fast PWM.
  TCCR1B = _BV(WGM22) | _BV(CS20);         // Start counting now. WGM22=1 to select Fast PWM mode 7

  DDRD |= _BV(3);     // Set pin to output (Note that OC2B = GPIO port PD3 = Arduino Digital Pin 3)
}

void timer1_setup (byte mode, int prescale, byte outmode_A, byte outmode_B, byte capture_mode)
{
  // enforce field widths for sanity
  mode &= 15 ;

#define OFFSET -40


#define OSP_SET_WIDTH(cycles) (OCR2B = 0xff-(cycles-1))

int pos;
int maxt = MAX + OFFSET;
