/* preliminary version of a progrma to test robot sensors
 *
 * This program and all MyroC software is licensed under the Creative Commons
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * Details may be found at http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 */

#include <stdio.h>
#include <libc.h>     // needed for sleep command
#include "MyroC.h"

int main ()
{
  printf ("test of sensor functions\n");

  //rConnect ("/dev/rfcomm0");
  rConnect ("");
  //rConnect ("00:1E:19:01:0E:8C");  // IPRE365827 - post
  rSetForwardness ("scribbler-forward");
  int sensorData [3];

  /* scribbler sensors */
  printf ("light sensors\n");
  printf ("\tleft light sensor:    %d\n", rGetLightTxt("left", 10));
  printf ("\tmiddle light sensor:  %d\n", rGetLightTxt("middle", 10));
  printf ("\tright light sensor:   %d\n", rGetLightTxt("right", 10));
  rGetLightsAll (sensorData, 10);
  printf ("\tarray data:  left: %d, middle: %d, right: %d\n",
          sensorData[0], sensorData[1], sensorData[2]);

  printf ("\nir sensors\n");
  int power;
  for (power = 5; power <= 255; power+=50)
    {
      printf ("setting IR power to %d\n", power);
      rSetIRPower (power);
      printf ("\tleft ir sensor:    %d\n", rGetIRTxt("left", 10));
      printf ("\tright ir sensor:   %d\n", rGetIRTxt("right", 10));
      rGetIRAll (sensorData, 10);
      printf ("\tarray data:  left: %d, right: %d\n",
              sensorData[0], sensorData[1]);
    }

  printf ("\nline sensors\n");
  rGetLine (sensorData, 10);
  printf ("\tarray data:  left: %d, right: %d\n",
          sensorData[0], sensorData[1]);

  /* fluke sensors */
  printf ("\nobstacle sensors\n");
  printf ("\tleft obstacle sensor:    %d\n", rGetObstacleTxt("left", 10));
  printf ("\tmiddle obstacle sensor:  %d\n", rGetObstacleTxt("middle", 10));
  printf ("\tright obstacle sensor:   %d\n", rGetObstacleTxt("right", 10));
  rGetObstacleAll (sensorData, 10);
  printf ("\tarray data:  left: %d, middle: %d, right: %d\n",
          sensorData[0], sensorData[1], sensorData[2]);

  printf ("\nbright sensors\n");
  printf ("\tleft average %d\n",    rGetBrightTxt("left", 10));
  printf ("\tmiddle average %d\n",  rGetBrightTxt("middle", 10));
  printf ("\tright average %d\n",   rGetBrightTxt("right", 10));

  rGetBrightAll (sensorData, 10);
  printf ("\tarray data:  left: %d, middle: %d, right: %d\n",
          sensorData[0], sensorData[1], sensorData[2]);

  printf ("\nstall sensors\n");
  printf ("\tstalled data (1=stalled):  %d\n", rGetStall(10));
  printf ("\tstart motors\n");
  rMotors (0.5, 0.5);
  sleep (1);
  printf ("\tstalled data (1=stalled):  %d\n", rGetStall(10));
  rMotors (0.0, 0.0);

  /* wrap up with rGetName, to be sure communication in sync */
  printf ("finishing up with robot %s\n", rGetName());

  rDisconnect ();
  printf ("test completed\n");
  return 0;

}