/* This is a very simple left-wall-follower. It uses two touch sensors to */

/* test for walls to the left and in front of the robot. It has to be     */

/* started with contact to the left wall and attempts to maintain the     */

/* contact.                                                               */

/* The configuration of the robot is as follows:                          */

/*    - The left bumper is connected to sensor port 3                     */

/*    - The front bumper is connected to sensor port 2                    */

/*    - The left wheel is connected to motor port C                       */

/*    - The right wheel is connected to motor port A                      */

/*  for an image see the code file                                                */

/*    The motors are connected to the large wheels via a transmission     */

/*    with a gear ratio of approximately 25:3 .                           */

/*    Motor A (the right wheel) drives the robot forwards if the motor    */

/*    direction is set to  fwd  while motor C (the left wheel) drives the */

/*    robot forwards if the direction is set to  rev .                    */

/*                                                                        */

/*                                                                        */

/* This program uses timing to determine the distance traveled and is     */

/* therefore sensitive to the charge level of the batteries and the exact */

/* gear ratio of the transmission. You might have to play with the timing */

/* constants (used in msleep) and the motor velocity settings to make it  */

/* on your robot.                                                         */

/*========================================================================*/

#include <conio.h>

#include <unistd.h>

#include <dmotor.h>

#include <dsensor.h>

#include <dkey.h>

int main()

{

  unsigned short front_bumper, left_bumper;

  unsigned short drive_speed = 70;

  /* Initially set the motor speed to 0 */

  motor_a_speed(0);

  motor_c_speed(0);

  while(1) {

    /*------------------------------*/

    /* Read the two bumper switches */

    /*------------------------------*/

    front_bumper = SENSOR_2;

    left_bumper = SENSOR_3;

    /*------------------------------------*/

    /* If the front bumper is not pressed */

    /*------------------------------------*/

    if (front_bumper > 0xf000) {

      if (left_bumper < 0xf000) {

        /*-----------------------------------------------------------------*/

        /* If the left bumper is in contact and the front bumper does not  */

        /* have contact, then drive and turn away from the left wall (turn */

        /* right).                                                         */

        /*-----------------------------------------------------------------*/

        motor_a_speed(5);

        motor_c_speed(drive_speed);

        motor_a_dir(fwd);

        motor_c_dir(fwd);

      }

      else {

        /*-----------------------------------------------------------------*/

        /* If neither the left nor the front bumper are in contact with a  */

        /* wall, then drive and turn towards the left wall to reestablish  */

        /* contact with the left bumper (turn left).                       */

        /*-----------------------------------------------------------------*/

        motor_a_speed(drive_speed);

        motor_c_speed(5);

        motor_a_dir(rev);

        motor_c_dir(rev);

      }

    }

    else {

      /*-------------------------------------------------------------------*/

      /* If the front bumper has hit the wall, then break, back up, and    */

      /* turn right approximately 90 degrees. The distance to back up and  */

      /* amount to rotate are determined here by the amount of time for    */

      /* which the motors are turned on. (The use of the encoder would be  */

      /* much more reliable.)                                              */ 

      /*-------------------------------------------------------------------*/

      motor_a_speed(drive_speed);

      motor_c_speed(drive_speed);

      motor_a_dir(brake);

      motor_c_dir(brake);

      msleep(100);

      cputs("bump");

      motor_a_dir(fwd);

      motor_c_dir(rev);

      msleep(450);

      cputs("right");

      motor_a_speed(10);

      motor_a_dir(fwd);

      motor_c_dir(fwd);

      msleep(1300);

    }

  }

}