-#include "PWM.h"
-
 const int encoderRightData1 = 3;
 const int encoderRightData2 = 5;
 const int encoderLeftData1 = 2;

  PWM_R = min(255, max(-255, PWM_R) );

  // Deadband
  if( abs(PWM_L) < 0 )
	PWM_L = 0;
  if( abs(PWM_R) < 0 )
	PWM_R = 0;

  if(PWM_L < 0) {
	//pwmWrite(leftForwardSpeed, 0);
	//pwmWrite(leftBackwardSpeed, -PWM_L);
  } else {
	pwmWrite(leftForwardSpeed, PWM_L);
	//pwmWrite(leftBackwardSpeed, 0);
  }

  if(PWM_R < 0) {
	//pwmWrite(rightForwardSpeed, 0);
	//pwmWrite(rightBackwardSpeed, -PWM_R);
  } else {
	//pwmWrite(rightForwardSpeed, PWM_R);
	//pwmWrite(rightBackwardSpeed, 0);
  }
  
  lastErrorL = ErrorL;
  lastErrorR = ErrorR;
}

  gotCommand = false;
  while(Serial.available())
  {
  if(Serial.read() == '$')
  {
    desiredSpeedL = Serial.parseFloat();
    desiredSpeedR = Serial.parseFloat();
    lastCmdTime = millis();
    gotCommand = true;
  }
  }
  if(gotCommand)
  {
  Serial.print('$');
  Serial.print(actualSpeedL);
  Serial.print(',');
  Serial.print(actualSpeedR);
  Serial.print(',');
  Serial.print(dT_sec);
  Serial.print('\n');
  }
  if( millis() - lastCmdTime > 500)
  {
  Serial.println("TIMEOUT");
  desiredSpeedL = 0;
  desiredSpeedR = 0;
  PWM_L = 0;
  PWM_R = 0;
  }

  dT_sec = (float)( millis() - lastLoopTime ) / 1000.0;
