 					// and the first one is in the middle of a cycle
 }
 
+/*********/
+/* SETUP */
+/*********/
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(38400);
   pinMode2(8, OUTPUT);
   timerSetup(512);
-  start = micros();
+  //start = micros();
 }
 
+/********/
+/* LOOP */
+/********/
 void loop() {
   // put your main code here, to run repeatedly:
   //long t1 = micros();

  //long t1 = micros();
  //cli();
  //TCNT1=16*1500;
  long time = micros();
  TCNT1 = round(16*(200*sin(2*PI*0.5*(time-start)/1000000.0) + 1500));
  digitalWrite2(8, HIGH);
  while(TCNT1>0);
  digitalWrite2(8, LOW);
  //long t2 = micros();
  //Serial.println(t2-time);
  //delay(20);
  //long t2 = micros();
  //Serial.println(t2-t1);
  delay(20);
}

/************/
/* INCLUDES */
/************/
#include <digitalWriteFast.h>
//#include <TimerOne.h>
#define	GPIO2_PREFER_SPEED	1
#include <arduino2.h>

/*************/
/* CONSTANTS */
/*************/
#define RESOLUTION 65536
#define BUFLEN 100
#define MAXF 5
#define MAXA 45

/********************/
/* GLOBAL VARIABLES */
/********************/
int maxus = 2390; // 20 ms
int mid = 1545;
double deg2us = 845 / 90.0;
long start = 0;
unsigned int pwmPeriod;
unsigned char clockSelectBits;
char oldSREG;
char buf[BUFLEN+1];
float freq, amp;
int pos;

/*************/
/* FUNCTIONS */
/*************/
void timerSetup(int duty) {
  TCCR1A = 0;                 // clear control register A 
  TCCR1B = 0; //_BV(WGM13);        // set mode 8: phase and frequency correct pwm, stop the timer
  TCNT1 = 0x00;
  long cycles = (F_CPU / 1000000) * maxus;                                      // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2
  if(cycles < RESOLUTION)              clockSelectBits = _BV(CS10);              // no prescale, full xtal
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11);              // prescale by /8
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10);  // prescale by /64
