/* MULTI-THREAD WALL FOLLOWING ALGORITHM FOR BIPED.

Filename: mazemulti2.c
Date: 14 May 2004 @ 20:42
Author: Konstantinos Akalestos kostisakalestos@hotmail.com

Part of the "Investigation Into Robotic Movements Final Year Project.

Configuration:
This program is to be used with biped R.O.B.B.I v2 onwards.
It requires the Lego Light Sensor Mounted on port 1, the Techno-Stuff DIRPD-T
Sensor mounted on port 2 and the Lego Touch sensor mounted on port 3.
Also the front motor should be connected to output port A while the rear motor
should be connected to output port C.
*/

#include <config.h>
#include <conio.h>
#include <unistd.h>
#include <tm.h>
#include <dsensor.h>
#include <dmotor.h>
#include <mem.h>
#include <dsound.h>

void walk_forward()// This function makes the robot walk forward
     {
       motor_a_speed(0);
       motor_c_speed(0);
       motor_c_speed(MAX_SPEED); // tilt to the right
       motor_c_dir(2);
       msleep(2000);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED);
       motor_a_dir(2); // move left foot forward
       msleep(600);
       motor_a_speed(0);
       motor_c_speed(MAX_SPEED); // tilt to the left
       motor_c_dir(1);
       msleep(2000);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED);  // move right foot forward
       motor_a_dir(1);
       msleep(600);
       motor_a_speed(0);
       }
       
void walk_backward() // This function makes the robot walk backwards
     {
       motor_c_speed(MAX_SPEED); // tilt to the right
       motor_c_dir(2);
       msleep(2000);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED);
       motor_a_dir(1); // move left foot backwards
       msleep(600);
       motor_a_speed(0);
       motor_c_speed(MAX_SPEED); // tilt to the left
       motor_c_dir(1);
       msleep(2000);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED);  // move right foot backwards
       motor_a_dir(2);
       msleep(600);
       motor_a_speed(0);
       }

void spin_right() // This function makes the robot turn clockwise standing on
                  // its right foot.
{
       motor_a_speed(0);
       motor_c_speed(MAX_SPEED); // tilt to the right
       motor_c_dir(2);
       msleep(2000);
       motor_c_speed(0);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED); // spin right
       motor_a_dir(2);
       msleep(300);
}

void spin_left() // This function makes the robot turn anti-clockwise standing
                 // on its left foot.
{
       motor_a_speed(0);
       motor_c_speed(MAX_SPEED); // tilt to the left
       motor_c_dir(1);
       msleep(2000);
       motor_c_speed(0);
       motor_c_speed(0);
       motor_a_speed(MAX_SPEED); // spin left
       motor_a_dir(1);
       msleep(300);
}

void stop()
{
motor_a_speed(0);
motor_c_speed(0);
motor_a_dir(3);
motor_c_dir(3);
dsound_system(DSOUND_BEEP);
}

/* Here we declare the detect object, detect wall and detect light functions */

int detect_wall(int argc, char **argv);
int detect_light(int argc, char **argv);
int detect_object(int argc, char **argv);

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

/* These integers will hold the processes id's (pid) of the different threads */

int wallpid;
int lightpid;
int objectpid;

ds_active(&SENSOR_1); // Set Light Sensor in ACTIVE MODE
ds_active(&SENSOR_2); // Set IR Sensor in ACTIVE MODE
#define  TOUCH_3 TOUCH(SENSOR_3) // Define Touch Sensor


/* Here we initialise the different threads. The int argc and char
**argv are required for the thread setup but are not used. */

lightpid = execi(detect_light, 0, (char **)NULL, 10,
DEFAULT_STACK_SIZE);
wallpid = execi(detect_wall, 0, (char **)NULL, 20, DEFAULT_STACK_SIZE);
objectpid = execi(detect_object, 0, (char **)NULL, 10,
DEFAULT_STACK_SIZE);
/* This command starts the pre-emptive round-robin scheduler */

tm_start();

}

int detect_object(int argc, char **argv)
{
while (!shutdown_requested())
      {
      if (TOUCH_3!=0)
       {
        walk_backward();
        spin_left();
        exit;
        }
       else
           {
           exit;
           }
       }
}

int detect_light(int argc, char **argv)
{


while (!shutdown_requested()) // While the off button is not pressed

      {


       if ( LIGHT_1<35)
          {
          stop();
          }

       else
          {
           exit;
           }
       }
}

int detect_wall(int argc, char **argv)
{

while (!shutdown_requested()) // While the off button is not pressed.
       {

       if (LIGHT_2<50)
          {
          walk_forward();
          exit;
          }



      else
         {
         spin_right();
         exit;
         }
      }
}