/*========================================================================*/
/* 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:                          */
/*                                                                        */
/*                                 ____________                           */
/*               Left Bumper -->  |___________ \                          */
/*                                  ||    ||  \ \                         */
/*                        ||        ||    ||   \_\  __                    */
/*                 ================ ||____||__      \ \                   */
/* Left Wheel -->  ================ |         |      \ \                  */
/*              __________||________|_________|      | |                  */
/*  Caster     |                              |      | |                  */
/*   |         |                              |      | |                  */
/*   V         |                              |______| |                  */
/* ====________|                                       | <-- Front Bumper */
/* ====~~~~~~~~|                               ______  |                  */
/*             |                              |      | |                  */
/*             |                              |      | |                  */
/*             |______________________________|      | |                  */
/*                        ||                         | |                  */
/*  Right Wheel -> ================                  / /                  */ 
/*                 ================                 /_/                   */
/*                        ||                                              */
/*                                                                        */
/*                                                                        */
/*    - 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                      */
/*                                                                        */
/*    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);
    }
  }
}