// export RCXTTY=COM1
// cd ..; make; cd util; ./dll ../demo/sample_code.lx
// cd ..; make; cd util; ./firmdl3 -s ../boot/brickOS.srec

/* file:    sample_code.c
 * author:  Bob Kojima - bob@fial.com - www.bong69.com
 * date:    01-10-05
 *
 * This example code demonstrates how to use the Nystrom Engineering
 * sensor Multiplexor.  A total of 6 touch sensors and 3 rotation 
 * sensors are used. In this program there are 3 identical 
 * threads.  Each tread has a motor, 2 touch sensors and 1
 * rotation sensor.  The rotation sensor controls the speed
 * of the motor while the touch sensors control the motor direction.
 * Pressing the view button terminates the program.
 *
 * If you have questions about the Nystrom Engineering Sensor Multiplexor
 * please send an email to pat Nystrom - pat@nystromengineering.com
 */

#include <unistd.h>
#include <dmotor.h>
#include <conio.h>
#include <pic_mux.h>
#include <dsensor.h>
#include <dkey.h>

int thread_one(int argc, char *argv[])
{
  int speed;
  motor_a_dir(fwd);

  speed = 0;
  
  while(1)
    {
      speed = SENSOR_3_A1;
      if(speed > 255)
        speed = 255;
      if(speed < 0)
        speed = 0; 	
      motor_a_speed(speed);	
      if(SENSOR_3_P1)
        motor_a_dir(fwd);
      if(SENSOR_3_P2)
        motor_a_dir(rev);
    }
}

int thread_two(int argc, char *argv[])
{
  int speed;
  motor_b_dir(fwd);

  speed = 0;

  while(1)
    {
      speed = SENSOR_3_A2;
      if(speed > 255)
        speed = 255;
      if(speed < 0)
        speed = 0; 	
      motor_b_speed(speed);	
      if(SENSOR_3_P3)
        motor_b_dir(fwd);
      if(SENSOR_3_P4)
        motor_b_dir(rev);
    }
}

int thread_three(int argc, char *argv[])
{
  int speed;
  motor_c_dir(fwd);

  speed = 0;

  while(1)
    {
      speed = SENSOR_3_A3;
      if(speed > 255)
        speed = 255;
      if(speed < 0)
        speed = 0; 	
      motor_c_speed(speed);	
      if(SENSOR_3_P5)
        motor_c_dir(fwd);
      if(SENSOR_3_P6)
        motor_c_dir(rev);
    }
}

int main(int argc, char *argv[])
{
  tid_t thread1, thread2, thread3;
  //char tmp[10];
  unsigned char key = 0;
  
  ds_active(&SENSOR_3);
  
  /* start the 3 children threads going */
  thread1=execi(&thread_one, 0, 0, PRIO_NORMAL, DEFAULT_STACK_SIZE);
  thread2=execi(&thread_two, 0, 0, PRIO_NORMAL, DEFAULT_STACK_SIZE);
  thread3=execi(&thread_three, 0, 0, PRIO_NORMAL, DEFAULT_STACK_SIZE);

  while (1)
    {
      cputw(SENSOR_3_A1);
    
      //stop if view button is pressed
      key = dkey_multi;
      if (key == 4)
      {
        /* now kill the kids */
        kill (thread1);
        kill (thread2);
        kill (thread3);
        
        /* cleanup */
        motor_a_speed(MIN_SPEED);
        motor_b_speed(MIN_SPEED);
        motor_c_speed(MIN_SPEED);
        motor_a_dir(off);
        motor_b_dir(off);
        motor_c_dir(off);
        cls();
        
        return 0;
      }
    }
  return 0;
}