/* 
 * straight2.c
 *
 * Drive a two-motor robot in a dead straight line (or die trying :-)
 * Second version: uses the Feedback-loop control mechanism as
 * described in Ch 2 of Jones and Flynn "Mobile Robots"
 *
 * Assumes:
 *   Left motor on A, left rotation sensor on 1
 *   Right motor on C, right rotation sensor on 3
 *   Light sensor on sensor 2
 *
 * The algorithm consists of a single thread:
 *  diffThread: computes the differences between the rotation counts
 *              and feeds that into the speed settings for each motor
 *
 * Initially we can try proportional feedback.
 *
 * Note: motor speed is represented by an int between 0..255
 * Response may not be linear across this range.
 *
 * This first attempt slaves the two wheels together so that they move
 * in a straight line
 *
 * Mark Crosbie  5/3/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 SENSORLEFT SENSOR_1
#define SENSORRIGHT SENSOR_3

#define LIGHTSENSOR SENSOR_2
#define ROTSENSORLEFT ROTATION_1
#define ROTSENSORRIGHT 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

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

/* how many pulses do we get at full speed in each time period
 * measure this empirically
 */
#define CLICKS 6

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

/*********************************************************************
 * thread declarations
 ********************************************************************/
int leftVelocityThread(int argc, char **argv);
int rightVelocityThread(int argc, char **argv);
int speedControlThread(int argc, char **argv);

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

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

float integrate(float left_vel, float right_vel, float bias);
void set_velocity(float vel, float bias);

/*********************************************************************
 * global variables that each thread will access 
 ********************************************************************/

float desiredVelocity;         /* desired velocity for *both* motors */
float desiredBias;             /* desired bias */

int leftSign, rightSign;

float k_clicks = CLICKS / 100.0;
float integral = 0.0;

int leftVelocity, rightVelocity; /* current readings on each sensor */

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

  initAll();

  setLeftSpeed(MAX_SPEED);
  setRightSpeed(MAX_SPEED);

  while(1) {
    lcd_int(leftVelocity + rightVelocity);
    msleep(FEEDBACKFREQ);
  }
    
}

void initAll(void) {

    ds_init(); 
    ds_active(&SENSORRIGHT);
    ds_active(&SENSORLEFT);
    ds_active(&LIGHTSENSOR);

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

    /* start sensor processing */
    ds_rotation_on(&SENSORLEFT);
    ds_rotation_on(&SENSORRIGHT);

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

    /* initialize the current velocity and bias variables */
    desiredVelocity = 0;
    desiredBias = 0;
      
    /* reset the rotation sensor reading vars */
    leftVelocity = 0;
    rightVelocity = 0;

    /* start the threads to track each wheel's velocity */
    execi(&leftVelocityThread, 0, (char **)NULL, PRIO_NORMAL,
	  DEFAULT_STACK_SIZE);

    execi(&rightVelocityThread, 0, (char **)NULL, PRIO_NORMAL,
	  DEFAULT_STACK_SIZE);

    execi(&speedControlThread, 0, (char **)NULL, PRIO_NORMAL,
	  DEFAULT_STACK_SIZE);

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

/* 
 * Name:
 *    leftVelocityThread
 *
 * Description:
 *    Continually measure the rotational speed of the left motor and
 *    update leftVelocity
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ignored
 *
 * Returns:
 *    None - runs continually
 * 
 * Globals affected:
 *    leftVelocity
 *
 * Algorithm:
 *    
 */
int leftVelocityThread(int argc, char **argv) {

  while(1) {
    /* has the sensor changed value? */
    if(leftVelocity != ROTSENSORLEFT) {
      leftVelocity = ROTSENSORLEFT;
      ds_rotation_set(&SENSORLEFT, 0);
    }
    msleep(FEEDBACKFREQ);
  }
}

/* 
 * Name:
 *    rightVelocityThread
 *
 * Description:
 *    Continually measure the rotational speed of the left motor and
 *    update rightVelocity
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ignored
 *
 * Returns:
 *    None - runs continually
 * 
 * Globals affected:
 *    rightVelocity
 *
 * Algorithm:
 *    
 */
int rightVelocityThread(int argc, char **argv) {
  while(1) {
    /* has the sensor changed value? */
    if(rightVelocity != ROTSENSORRIGHT) {
      rightVelocity = ROTSENSORRIGHT;
      ds_rotation_set(&SENSORRIGHT, 0);
    }
    msleep(FEEDBACKFREQ);
  }
}

/* 
 * Name:
 *    speedControlThread
 *
 * Description:
 *    Run the feedbck control loop that slaves the two motors together
 *    and provides for turning bias
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ignored
 *
 * Returns:
 *    None - runs continually
 * 
 * Globals:
 *    leftVelocity, rightVelocity, desiredVelocity, desiredBias, k_clicks
 *
 * Algorithm:
 *    
 */
int speedControlThread(int argc, char **argv) {

  while(1) {

  }
}

float integrate(float left_vel, float right_vel, float bias) {
  integral = integral + left_vel + bias - right_vel;
}


void set_velocity(float vel, float bias) {
  desiredVelocity = k_clicks * vel;
  desiredBias = k_clicks * vel;

  leftSign = get_sign(vel - bias);
  rightSign = get_sign(vel - bias);
}

int get_sign(float x) {
  if( x > 0.0 )
    return 1;
  else 
    return -1;
}

void setLeftSpeed(int speed) {
  motor_a_speed(speed);
}

void setRightSpeed(int speed) {
  motor_c_speed(speed);
}