/* File name: vehicle_auto.ch
   The purpose of this demo is to demonstrate the CH Mindstorms
   Control Package's ability to control the vehicle robot model
   autonomously, as well as demonstrate how use sensor data from
   the robot to controle it's actions. */

#include <conio.h>
#include <stdio.h>
#include <mindstorms.h>

CMindstorms robot;
int touchValue;             //used to store touch sensor data
double ultraValue;          //used to store ultrasonic sensor data
char key = 'x',             //stoes user input
     movemode = 'x';	      //stores last movement command

/* Set sensor types */
robot.setSensorTouch(PORT1, "Touch");
robot.setSensorUltrasonic(PORT4, "Centimeter");

while(1){
    /* check user input 'q' to quit */
    if(kbhit()){
        if(getch() == 'q'){
            printf("\nExiting.");
            break;
        }
    }

    /* get touch sensor. If pressed reverse and turn left */
    robot.getSensorTouch(PORT1, touchValue);
    if (touchValue){
      robot.driveAngle(-720);
      robot.turnLeft(90, 1.1, 4.5);
    }

    /* Get distance from UltraSonic sensor.
     * Turn left if really close.*/
    robot.getSensorUltrasonic(PORT4, ultraValue);
    if (ultraValue < 10){
      robot.turnLeft(90, 1.1, 4.5);
    } 

    /* drive vehicle forward forever */
    robot.driveForeverNB();
}