   //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);
+  Serial.println("Enter frequency, amplitude");
+  pos = 16*mid;
+  
+  while ( Serial.available() == 0 ) {
+       TCNT1 = pos;
+       digitalWrite2(8, HIGH);
+       while(TCNT1>0);
+       digitalWrite2(8, LOW);
+       delay(20);
+  }
+  
+   int n = Serial.readBytesUntil(',',buf,BUFLEN);
+   buf[n] = '\0';
+   freq = atof(buf);
+   n = Serial.readBytes(buf,BUFLEN);
+   buf[n] = '\0';
+   amp = atof(buf);
+   if( !(freq > 0 && freq <= MAXF) )
+   {
+     Serial.print("Frequency must be between 0 and ");
+     Serial.println(MAXF);
+   }
+   else if( !(amp > 0 && amp <= MAXA) )
+   {
+     Serial.print("Amplitude must be between 0 and ");
+     Serial.println(MAXA);
+   }
+   else
+   {
+     Serial.print("Platform oscillating at ");
+     Serial.print(freq);
+     Serial.print("Hz, ");
+     Serial.print(amp);
+     Serial.println("Deg");
+     
+     float ampus = amp*deg2us;
+     long start = micros();
+     
+     while ( Serial.available() == 0 ) {
+       long time = micros();
+       pos = round(16*(ampus*sin(2*PI*freq*(time-start)/1000000.0) + mid));
+       TCNT1 = pos;
+       digitalWrite2(8, HIGH);
+       while(TCNT1>0);
+       digitalWrite2(8, LOW);
+       delay(20);
+     }
+     
+     bool above = pos > 16*mid;    
+   
+     while ( ( above && pos > 16*mid ) || ( !above && pos < 16*mid ) ) {
+       long time = micros();
+       pos = round(16*(ampus*sin(2*PI*freq*(time-start)/1000000.0) + mid));
+       TCNT1 = pos;
+       digitalWrite2(8, HIGH);
+       while(TCNT1>0);
+       digitalWrite2(8, LOW);
+       delay(20);
+     }
+   }
+  //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);
+  //delay(20);
 }

					// and the first one is in the middle of a cycle
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(38400);
  pinMode2(8, OUTPUT);
  timerSetup(512);
  start = micros();
}

void loop() {
  // put your main code here, to run repeatedly:
  //long t1 = micros();

/************/
/* 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
