/* 
 * layered.h
 *
 * Version:
 *   0.1: initial version for 0.2.4 legOS kernel
 *
 * Definitions for layered.c
 * Control motors using proportional feedback with stall detection
 *
 * Mark Crosbie  1/23/2001  mark@mastincrosbie.com
 */

#ifndef __LAYERED_H
#define __LAYERED_H

#include <semaphore.h>

/*********************************************************************
 * constants
 ********************************************************************/
#define MID_SPEED ((MAX_SPEED - MIN_SPEED) / 2)
#define HIGH_SPEED ((2*MAX_SPEED)/3)

/* interval in seconds to sample in feedback loop */
#define INTERVAL 1

/* damping factor for feedback loop */
#define DAMPING 0.5

/* motor direction constants */
#define FORW 0
#define REV 1

/* how many samples should we take before calling a stall? 
 * Derived by observing the robot in action
 */
#define STALL_SAMPLES 7   

/* time interval between each sample point for stall detection in ms */
#define STALL_INTERVAL 100

/* time interval between each sample point for speed calculations ms */
#define SPEED_INTERVAL 100

/* thresholds for feedback loop */
#define MIN_THRESHOLD 3 /* don't adjust if difference is <= 3 */

/* errno values */
#define ERANGE 1

/* a handy abs() function */
#define ABS(x) ((x)>=0)?(x):-(x)

/* a handy debug function */
#define VAL(m, c) cputs(m); sleep(1); cputw(c); sleep(2);

/* all the information we need to know about a motor */
struct motor;
typedef struct motor {

  sem_t mutex;            /* mutex around structure */

  unsigned int current_rpm;     /* motor revolutions per second */
  unsigned int desired_speed;	/* in mm/s */
  unsigned int current_speed;	/* in mm/s */
  unsigned int wheel_circ;	/* circumference of wheel mm */
  unsigned int ratio;		/* sensor/wheel ratio */

  unsigned long distance;       /* distance travelled by the wheel in mm */
  pid_t distancePid;            /* thread to measure distance */

  MotorDirection dir;		/* motor direction */
  int feedback;			/* feedback proportion */

  int stalled;			/* is this motor stalled ? */
  pid_t stallPid;               /* stall handler thread */
  void (*stallHandler)(struct motor *);    /* stall handler callback */

  volatile int *rotSensorPort;  /* port to which rot sensor is connected */
} motor_t;

/* some convenient definitions */
#define MAXMOTOR 3
#define A_MOTOR motors[0]
#define B_MOTOR motors[1]
#define C_MOTOR motors[2]

/*********************************************************************
 * global function declarations visible to all modules
 ********************************************************************/
int initMotor(motor_t *motor, 
	      unsigned int wheel_circ, 
	      unsigned int ratio, 
	      volatile int *rotSensorPort, 
	      volatile int *rotSensor,
	      void (*stallHandler)(struct motor *));

int stallThread(int argc, char **argv);
int distSpeedThread(int argc, char **argv);

unsigned int getRPM(motor_t *motor);
unsigned int getSpeed(motor_t *motor);
unsigned long getDist(motor_t *motor);
void setDist(motor_t *motor, long value);

/*********************************************************************
 * global variable declarations
 ********************************************************************/
extern int errno;  /* global errno, just like in UNIX */

#endif