/* 
 * straight-diff.c
 *
 * Drive a two-motor robot in a dead straight line (or die trying :-)
 * Third version: A differential is installed on the robot and it
 * continually updates the rotation sensor with the delta between the speeds.
 * Slave the right motor to the left: the right motor tries to exactly
 *  match the left speed
 *
 * Assumes:
 *   Left motor on Output A
 *   Right motor on Output C
 *   Light sensor on sensor input 1
 *   Rotation sensor on input 3
 *
 * Differential is built to output difference between wheel
 * rotations. The rotation sensor returns a POSITIVE value if the
 * right motor is SLOWER than the left, else it returns a NEGATIVE
 * value. The difference value is scaled and fed into the right motor
 * speed
 *
 * The algorithm consists of a single thread:
 *    slaveControlThread
 *
 * Mark Crosbie  20/8/00  mark@mastincrosbie.com
 */


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

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

/*********************************************************************
 * inputs and outputs
 ********************************************************************/
#define SENSOR SENSOR_3
#define LIGHTSENSOR SENSOR_2

#define ROTSENSOR ROTATION_3

#define MOTORLEFT MOTOR_A
#define MOTORRIGHT MOTOR_C

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

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

/* proportional constant */
#define Kpro 5

#define THRESHOLD 1

/* how often to run the feedback loop - in ms */
#define FEEDBACKFREQ 500

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

/*********************************************************************
 * thread declarations
 ********************************************************************/
int slaveControlThread(int argc, char **argv);

/*********************************************************************
 * function declarations
 ********************************************************************/
void initAll(void);
wakeup_t rotation_change(wakeup_t data);

void setLeftSpeed(int speed);
void setRightSpeed(int speed);


/*********************************************************************
 * global variables that each thread will access 
 ********************************************************************/
int leftSpeed, rightSpeed;

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

  initAll();

  setLeftSpeed(MID_SPEED);
  setRightSpeed(MID_SPEED);

  while(1) {
    lcd_int(rightSpeed);
    msleep(FEEDBACKFREQ);
  }

}

void initAll(void) {

    ds_init(); 
    ds_active(&SENSOR);
    ds_active(&LIGHTSENSOR);

    /* reset sensor values */
    ds_rotation_set(&SENSOR, 0);

    /* start sensor processing */
    ds_rotation_on(&SENSOR);

    /* motor direction */
    motor_a_dir(fwd);
    motor_c_dir(fwd);

    /* start the slave thread */
    execi(&slaveControlThread, 0, (char **)NULL, PRIO_NORMAL,
	  DEFAULT_STACK_SIZE);

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


void setLeftSpeed(int speed) {
  if(speed < MIN_SPEED)
    speed = MIN_SPEED;
  if(speed > MAX_SPEED)
    speed = MAX_SPEED;
  motor_a_speed(speed);
  leftSpeed = speed;
}

void setRightSpeed(int speed) {
  if(speed < MIN_SPEED)
    speed = MIN_SPEED;
  if(speed > MAX_SPEED)
    speed = MAX_SPEED;
  motor_c_speed(speed);
  rightSpeed = speed;
}

/*
 * slaveControlThread
 *
 * Slave the right motor to the left motor so it runs at the same speed
 *
 * Inputs
 *    Not used
 *
 * Returns
 *    None
 */
int slaveControlThread(int argc, char **argv) {

  int diff;

  while(1) {
    if( (diff = ROTSENSOR) > THRESHOLD) {  /* read the speed difference */
      setRightSpeed(rightSpeed + (Kpro * diff));
      /* reset sensor values */
      ds_rotation_set(&SENSOR, 0);
    }
    msleep(FEEDBACKFREQ);
  }
}