/* 
 * layered.c
 *
 * Control motors using proportional feedback with stall detection
 *
 * Version:
 *   0.1: initial version for 0.2.4 legOS kernel
 *
 * Assumes:
 *   Not much; motor definitions are held in an array  so you
 *   can set them up at run time.
 *
 * External Interfaces:
 *    motor_init
 *    detectStallThread
 *    measureSpeedThread
 *    
 * Internal Interfaces:
 *    initModule
 *
 * Description:
 *   This first version contains the speed measurement code and the
 *   stall detection thread. Most of the base variable and structure
 *   definitions are in place.
 *
 *   A sample test main loop is in place to exercise the code
 *
 * TO DO:
 *   Seperate the layered motor driver code into a library module,
 *   split up the header file to expose the interfaces and keep the
 *   module internals hidden. Modify the Makefile to link the main
 *   module with the library module.
 *
 * Mark Crosbie  1/23/2001  mark@mastincrosbie.com
 */

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

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

#include "layered.h"

#define DEBUG

#ifdef DEBUG
static int progress = 0;   /* for debugging */
#define MAKEPROGRESS lcd_int(progress++);
#else
#define MAKEPROGRESS
#endif

/*********************************************************************
 * Contruction specific definitions
 ********************************************************************/
#define WHEEL_CIRC	256   /* mm Circumference of the wheel */
#define MOTOR_WHEEL_RATIO 8   /* Ratio of sensor/wheel turns 
			       * Rotation sensor turns 8 times for
			       * every one wheel revolution */

/*********************************************************************
 * static global variables for this module
 ********************************************************************/
motor_t motors[MAXMOTOR];  /* all the motors we can control */
int errno;  /* your friend errno; what was the last error value? */
char *args[2]; /* for passing to threads */


/*********************************************************************
 * local thread declarations (if any)
 ********************************************************************/


/********************************************************************* * local function declarations for this module (if any)
 ********************************************************************/
static void initModule(void);    /* basic module initialization */
void handleStall(motor_t *m);

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

  MAKEPROGRESS;

  initModule();

  MAKEPROGRESS;

  /* define a motor */
  initMotor(&A_MOTOR,       /* motor output A */
	    256,            /* circumference is 256 mm */
	    8,              /* ratio of motor/wheel turns is 8 */
	    &ROTATION_3,    /* rotation sensor is on input 3 */
	    &SENSOR_3,      /* for initialization */
	    handleStall     /* stall function */
	    );           

  MAKEPROGRESS;

  motor_a_speed(MAX_SPEED);
  motor_a_dir(fwd);

  setDist(&A_MOTOR, 0L);

  while(1) {
#if 1
    lcd_int(getDist(&A_MOTOR));
#endif

    msleep(100);
  }
   
}

void handleStall(motor_t *m) {
  m->stalled = 0; /* reset stalled flag */
  return;
}

/*
 * panic
 *
 * Input Parameters:
 *    None
 *
 * Output Parameters:
 *    None
 *
 * Globals:
 *    errno
 *
 * Algorithm: 
 *    Halt in an infinite loop displaying errno and progress values
 */
void panic(void) {

  while(1) {
    lcd_int(errno);
    sleep(1);
#ifdef DEBUG
    lcd_int(progress);
    sleep(1);
#endif
  }
}

/*
 * initModule
 *
 * Input Parameters:
 *    None
 *
 * Output Parameters:
 *    None
 *
 * Globals:
 *
 * Algorithm: 
 *
 */
static void initModule(void) {

  errno = 0;

  /* initialize the sensors */
  ds_init(); 

  /* start task processing */
  tm_start();
}

/* 
 * Name:
 *    initMotor
 *
 * Description:
 *    Initialize a motor's entry in the motor definition array
 *
 * Parameters:
 *    motor (in):      ptr to motor entry in motors[] array
 *    wheel_circ (in): circumference of wheel in mm
 *    ratio (in):      ratio of motor/wheel turns
 *    rotSensorPort (in): what port the rotation sensor for this motor
 *                         is connected to
 *    rotSensor (in):  to initialize rotational sensor processing
 *    stallHandler (in): function to call if motor stalls
 *                       may be NULL
 *
 * Returns:
 *    0 if all values valid and changes are made
 *    -1 an error occurred. errno is set
 *
 * Algorithm:
 *    Each motor's speed, circumference and ratio settings are initialized
 *    to 0. Direction is set to brake. The feedback setting is not changed
 *    unless the caller needs to fine tune a motor's response. The
 *    stallHandler function pointer points to a callback function to execute
 *    if the motor stalls.
 * 
 *    Start a thread to measure the distance and speed of the motor
 *    given its rotation sensor, and a thread to detect stalls on the
 *    motor.
 *
 *    The mapping of motors to motors[] array entries is simple: motor
 *    output A is in entry 0, B in 1 and C in 2
 *    The constants A_MOTOR, B_MOTOR and C_MOTOR are pre-defined in
 *    the layered.h file to simplify calling this function
 *
 * Example:
 *    initMotor(&A_MOTOR,       motor on output A
 *              256,            circumference is 256 mm
 *              8,              ratio of motor/wheel turns is 8
 *              &ROTATION_1,    rotation sensor is on input 1
 *              &SENSOR_1,      must agree with prev argument
 *              foo             call foo if the motor stalls
 *             );           
 */
int initMotor(motor_t *motor, 
	      unsigned int wheel_circ, 
	      unsigned int ratio, 
	      volatile int *rotSensorPort,
              volatile int *rotSensor,
	      void (*stallHandler)(struct motor *motor)) {


  errno = 0;     /* no errors yet */

  motor->distance = 0;
  motor->desired_speed = 0;
  motor->current_speed = 0;
  motor->ratio = ratio;
  motor->wheel_circ = wheel_circ;
  motor->dir = off;
  motor->feedback = 10;
  motor->stalled = 0;
  motor->rotSensorPort = rotSensorPort;
  motor->stallHandler = stallHandler;

  /* initialize the mutex on the structure */
  sem_init(&(motor->mutex), 0, 1);

  /* initialize the rotation sensor for this motor */
  ds_init(); 
  ds_active(rotSensor);

  /* ok, start sensor processing */
  ds_rotation_set(rotSensor, 0);
  ds_rotation_on(rotSensor);

  /* put the motor ptr into argv[0] for the thread */
  args[0] = (char *)motor;
  args[1] = (char *)NULL;

  /* start an instance of the distance thread for this motor */
  motor->distancePid = execi(distSpeedThread, 2,
				    args, 
				    PRIO_NORMAL,DEFAULT_STACK_SIZE); 

  /* start an instance of the stall detection thread for this motor */
  motor->stallPid = execi(stallThread, 2,
				    args, 
				    PRIO_NORMAL,DEFAULT_STACK_SIZE); 

  return 0;
}


/* 
 * Name:
 *    stallThread
 *
 * Description:
 *    Monitor the rotation sensor and detect if it has stopped
 *    changing. If it has, set the flag in the motor's array entry and
 *    call the stall handler function if defined.
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ptr to array entry of wheel to monitor for a stall
 *
 * Returns:
 *    Does not return - runs until killed
 */
int stallThread(int argc, char **argv) {

  int value;
  int sampleCount;
  int sensorReading;
  motor_t *motor;
  volatile int *sensor;

  motor = (motor_t *)(argv[0]);
  sensor = motor->rotSensorPort; /* take an initial reading */
  value = *sensor;
  sampleCount = 0;      /* no samples have been taken so far */

  /* we live forever, or at least until the main loop kills us :-) */
  while(1) {

    sensorReading = *sensor;   /* take a reading */

    msleep(STALL_INTERVAL);

    /* if the motor is currently disabled then don't bother to process
     * stall data
     */
    if( (motor->dir == off) || (motor->desired_speed == 0) )
      continue;

    if(value != sensorReading) {
      /* change is good. It means we're moving, so update value and
       * reset sampleCount
       */
      value = sensorReading;
      sampleCount = 0;
    } else {
      /* ok, time to take samples. If this is the last sample of the
       * set, then we call it a stall
       */
      if(sampleCount == STALL_SAMPLES) {
	cputs("stall");
	value = sensorReading;
	sampleCount = 0;
	motor->stalled = 1;
	/* call the stall handler */
	motor->stallHandler(motor);
      } else {
	/* update the sampleCount */
	sampleCount++;
      }
    }
  }
}


/* 
 * Name:
 *    distSpeedThread
 *
 * Description:
 *    Given the circumference of a wheel and the ratio of rotation
 *    sensor turns to wheel turns, compute the distance travelled by a
 *    wheel, and the speed at which it is turning.
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) argv[0] contains ptr to motor to measure
 *
 * Returns:
 *    Does not return - runs until killed
 *
 * Algorithm:
 *    Within each SPEED_INTERVAL milliseconds, count the number of
 *    rotation sensor pulses and compute the distance traveled.
 *
 *    Computational efficiency is gained by using local variables so
 *    that the motors[] array is not continually defreferenced.
 *    
 *    NOTE: distance is *not* vectored so travelling forwards by x 
 *    distance and then reverse by x results in 2x distance, not 0 
 *    distance
 *
 *    LIMITATIONS: 
 *      Will underflow if wheel circumference < 2mm
 *      If rate of rotation is less than 1/16th of a revolution every
 *      SPEED_INTERVAL milliseconds then zero is computed. Adjust
 *      SPEED_INTERVAL to a lower value if this is a problem.
 */
int distSpeedThread(int argc, char **argv) {

  motor_t *motor;
  int val1, val2, speed, rpm, circumference;
  unsigned int x, avgspeed, avgrpm;
  unsigned long avgdist;
  volatile int *sensor;

  /* pre-compute some values */
  motor = (motor_t *)argv[0];
  circumference = motor->wheel_circ;
  sensor = motors->rotSensorPort;
  avgdist = avgspeed = avgrpm = 0;

  while(1) {

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

    /* 
     * each unit of difference represents 1/16 th of a revolution if
     * we get 16 units of absolute difference, that equals one
     * revolution. Multiply by 1000/SPEED_INTERVAL to scale to seconds
     * 
     * Fixed point math is used to avoid division underflow. 
     * The RPM calculation is scaled by 60 in the numerator so that
     * division by 16 does not lead to an underflow error.
     *
     */
    x = ABS(val2 - val1); /* take absolute difference */

    if(x) { /* avoid division by zero! */
      speed = ((x * circumference) * (1000/SPEED_INTERVAL))/16;
      rpm = (x * 60 * (1000/SPEED_INTERVAL))/16; /* revs per minute */
    } else {
      rpm = speed = 0;
    }

    /* compute averages over two readings for smoother results */
    avgspeed = (avgspeed + speed)/2;
    avgrpm = (avgrpm + rpm) / 2;

    /* distance = speed * time */
    avgdist = avgdist + ((x * circumference)/16);

    /* update the motor's entry and only acquire the lock once */
    sem_wait(&(motor->mutex));  /* get the mutex */
    motor->current_rpm = avgrpm;
    motor->current_speed = avgspeed;
    motor->distance = avgdist;
    sem_post(&(motor->mutex));  /* release the mutex */
  }
}

/* 
 * Name:
 *    getRPM
 *
 * Description:
 *    Return current rotational speed of a motor in RPM
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured rotational speed
 *
 * Algorithm:
 *    Return the current value for this motor. No need to acquire mutex.
 */
unsigned int getRPM(motor_t *motor) {

  return(motor->current_rpm);
}

/* 
 * Name:
 *    getSpeed
 *
 * Description:
 *    Return current rotational velocity of the motor in mm/s
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured speed value
 *
 * Algorithm:
 *    Return the current value for this motor. No need to acquire mutex.
 */
unsigned int getSpeed(motor_t *motor) {

  return(motor->current_speed);
}

/* 
 * Name:
 *    getDist
 *
 * Description:
 *    Return current measured distance traveled by the wheel
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured distance value
 *
 * Algorithm:
 *    Return the current value for this motor.  No need to acquire mutex.
 */
unsigned long getDist(motor_t *motor) {

  return(motor->distance);
}

/* 
 * Name:
 *    setDist
 *
 * Description:
 *    Set the distance measurement for a wheel to a fixed value 
 *
 * Parameters:
 *    motor (in): ptr to motor to set
 *    value (in): value to set current distance to
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Nothing.
 *
 * Algorithm:
 *    Sets the current distance measurement. Acquires mutex on array entry
 */
void setDist(motor_t *motor, long value) {

  sem_wait(&(motor->mutex));  /* get the mutex */
  motor->distance = value;
  sem_post(&(motor->mutex));  /* release the mutex */

}