/* 
 * tacho.c
 *
 * A tachometer for the motor
 * Measure the speed of a motor using a rotation sensor
 * 
 * Assumes: rotation sensor on sensor port 1 
 * and motor is connected to Output A
 *
 * This program was more tricky than it first appeared, mainly thanks
 * to the speed of the motors relative to the rate of measurement of
 * the sensors.
 * A simple algorithm: take two measurements seperated by INTERVAL
 * milliseconds. This gives the number of 1/16th revolutions. Divide
 * by 16 for the number of whole revolutions and scale up for RPM.
 *
 * Possible improvements: don't reset the sensor each loop iteration and
 * compute off the previous value. Only update the display every
 * second to give more time over to measuring. Run the whole thing in
 * an interrupt driven mode.
 *
 * NOTE: if you get negative values on the display then the motor is
 * rotating "backwards". Just switch the connector block on the motor
 * around.
 *
 * Mark Crosbie  4/8/00  mark@mastincrosbie.com
 */


#include <conio.h>
#include <unistd.h>

#include <sys/dsensor.h>
#include <dmotor.h>
#include <dsound.h>

#define ROTSENSOR1 SENSOR_1
#define MOTOR MOTOR_A

/* interval in milliseconds to sample at. 1000ms = 1 sec */
#define INTERVAL 500

int main(int argc, char *argv[]) {

  long int rpm;
  long int val1;
  long int val2; 
  long int avg = 0;

  /* initialize the sensors */
  ds_init(); 
  ds_active(&ROTSENSOR1);

  /* ok, start sensor processing */
  ds_rotation_on(&ROTSENSOR1);

  /* and turn on the motor */
  motor_a_speed(MAX_SPEED);	   
  motor_a_dir(fwd);

  while(1) {

    /* take a sensor measurement */
    val1 = ROTATION_1;
    msleep(INTERVAL);
    val2 = ROTATION_1;

    /* 
     * each unit of difference represents 1/16 th of a revolution
     * if we get 16 units of difference, that equals one revolution
     */
    rpm = ((val2 - val1)/16) * (1000/INTERVAL) * 60;

    /* update running average */
    avg = (avg + rpm) / 2;

    /* print the difference to the screen */
    lcd_int(avg);
  }
}