#define MotOn OnFwd(OUT_A, 100);
#define MotOff Off(OUT_A);

#define LightSensor S2
#define Thr 50
#define Hyst 10
#define MaxTurns 6

#define VoltCorrect 1.02

#define MechGear 3


byte SensorPort  =  IN_1;
byte ADDR    = 0x12;
#include "PM-lib.nxc"


long Timer[MaxTurns];
string str = "";

task main()
{
  int Current=0;
  int i;

  SetSensorLowspeed(SensorPort);
  Wait(10);

	ShowSensorInfo(ADDR, SensorPort);
	
	PM_SendCommand(SensorPort, ADDR, 'R');

  Wait(1000);
  ClearScreen();

  SetSensorLight(LightSensor);
  MotOn;

  for(i=0; i<MaxTurns ; i++)
  {
    until(Sensor(LightSensor) > Thr+Hyst);
    until(Sensor(LightSensor) < Thr-Hyst);
    Timer[i]=CurrentTick();
  }


  for(i=0; i<10; i++)
  {
    Current += PM_ReadPresentCurrent(SensorPort,ADDR);
  }
  int Voltage = PM_ReadPresentVoltage(SensorPort,ADDR)/VoltCorrect;
  MotOff;
  Current/=10;

  str = StrCat(NumToStr(Voltage), "mV ", NumToStr(Current), "mA");
  TextOut(0,LCD_LINE1, str);

  float rpm = 60000/(Timer[MaxTurns-1] - Timer[MaxTurns - 4])*MechGear*3;

  str = StrCat(NumToStr(rpm), " rpm" );
  TextOut(0,LCD_LINE2, str);
  
  for(i=1; i<MaxTurns; i++)
  {
    NumOut(40, LCD_LINE3-8*i, Timer[i]-Timer[i-1]);
  }
  
  until(ButtonPressed(BTNCENTER, false));

}