/* 
 * stall.c
 *
 * Detect a robot stalling when it hits an object
 * Constantly monitors the rotation sensor as a background thread and
 * if it detects a stall then beep and stop and reverse the motors
 *
 * Assumes: rotation sensor on sensor port 1
 *          motor output on port A
 *
 * Mark Crosbie  4/8/00  mark@mastincrosbie.com
 */


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

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

#define ROTSENSOR SENSOR_1
#define DRIVEMOTOR MOTOR_A
#define FORW 0
#define REV 1

/* how many samples should we take before calling a stall? */
#define STALL_SAMPLES 3   

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

int detect_stall(int argc, char **argv);
void stop_and_reverse(void);

int dir;  /* motor direction */

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

  int stallpid;

  /* we start out going forward */
  dir = FORW;

  /* initialize the sensor */
  ds_init(); 
  ds_active(&ROTSENSOR);

  /* set the sensor's count to 0 */
  ds_rotation_set(&ROTSENSOR, 0);

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

  /* declare the thread to detect a stall */
  stallpid = execi(detect_stall, 0, (char **)NULL, PRIO_NORMAL,
		   DEFAULT_STACK_SIZE);

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

  motor_a_speed(MAX_SPEED);
  motor_a_dir(fwd);
  cputs("fwwd ");

  while(1) {

  }
}

/* 
 * Name:
 *    detect_stall
 *
 * Description:
 *    Monitor the rotation sensor and detect if it has stopped
 *    changing. If it has, stop the motors and reverse direction
 *
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ignored
 *
 * Returns:
 *    0 if no change in value
 *    1 if value of ROTSENSOR has changed since last poll
 */
int detect_stall(int argc, char **argv) {

  int value;
  int sampleCount;

  value = ROTATION_1;   /* take an initial reading */

  sampleCount = 0;      /* no samples have been taken */

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

    msleep(STALL_INTERVAL);

    if(value != ROTATION_1) {
      /* change is good. It means we're moving, so update value and
       * reset sampleCount
       */
      value = ROTATION_1;
      sampleCount = 0;
    } else {
      /* ok, time to take samples. If this is the last sample of the
       * set, then we call it a stall and reverse out of here
       */
      if(sampleCount == STALL_SAMPLES) {
	/* beep so we know it  */
	dsound_system(DSOUND_BEEP);
	stop_and_reverse();
	value = ROTATION_1;
	sampleCount = 0;
      } else {
	/* update the sampleCount */
	sampleCount++;
      }
    }
  }
}

/* 
 * Name:
 *    stop_and_reverse
 *
 * Description:
 *    Stop the motor and reverse direction
 *
 * Parameters:
 *
 * Returns:

 */
void stop_and_reverse(void) {

  motor_a_dir(brake);

  /* toggle direction */
  if(dir == FORW) {
    dir = REV;
    motor_a_dir(rev);
    cputs("rev ");
  } else {
    dir = FORW;
    motor_a_dir(fwd);
    cputs("fwwd ");

  }

  motor_a_speed(MAX_SPEED);
}