/* File name: machine_auto.ch
 * The purpose of this demo is to demonstrate the CH Mindstorms
 * Control Package's ability to control the machine robot model
 * autonomously, as well as demonstrate how to collect and plot
 * sensor data from the robot. */

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

CPlot       plot;
CMindstorms  robot;
double  speedRatio = 0.30;
int     quit = 0,             //used to exit for loop
        i;                    //counter variable
double  ultraValue, position;
double  gearratio = (8.0/56)*(1.0/24);
int     numpoints = 10;       //desired number of data points
int     anglestep = 2;	      //angle moved between steps
double  angle[numpoints];     //angle retrieved from the tachometer
double  distance[numpoints];  //data received from the ultrasonic sensor

/* Initialize arm. (Set sensor types and initialize variables) */
printf("\nInitializing arm for autonomous control...\n");

robot.setSensorUltrasonic(PORT4, "Centimeter");
for(i = 0; i < numpoints; i++){
    angle[i] = 0;
    distance[i] = 0;
}

/* print usage information to the user*/
printf("\n%d Data points will be collected with a"
       "step size of %d.", numpoints, anglestep);
printf("\nPlease ensure that the arm can rotate"
       "%d degrees from its current position.", 
       (numpoints*anglestep));
printf("\nPress any key to continue. Press q at any time to quit.\n");

if(getch() == 'q'){
    printf("\nQuitting program.");
    exit(0);
}

/* begin Autonomous loop*/
for(i = 0; i < numpoints; i++){
    /* get sensor data, if success print data, else print error*/
    robot.getSensorUltrasonic(PORT4, ultraValue);
    distance[i] = ultraValue;
    robot.getJointAngle(JOINT1, angle[i]);
    printf("\nSample: %d,  distance: %lf,  Angle: %lf",
                   i, distance[i], angle[i]);

    /* check if q was pressed and if so exit program*/
    if (!_kbhit) {
        if (getch() == 'q') {
            printf("\nQuitting program.");
            break;
        }
    }

    /* rotate arm by anglestep (rotate motor anglestep/gear ratio)*/
    robot.moveJoint(JOINT1, anglestep / gearratio);
}

/* plot data in CH */
plot.data2DCurve(angle, distance, numpoints);
plot.grid(PLOT_ON);
plot.plotting();