Distance Sensing

and Servo Control

Introduction

Having previously calibrated our GP2D12 and PING))) sensors, we were asked to implement a function that would stop the robot when it had reached a certain distance from the wall.

VID_20170131_155459.mp4

Here is an example of the robot using one our distance sensors to stop a certain distance from the wall.

Using our PING))) sensor, we got very good results.

We struggled getting accurate results with our GP2D12, so we implemented an average filter to help minimize the errors.

Code for PING))) Sensor

void pingStop(int distance2) {

  //stops a certain distance from the wall

  delay(500);

  leftMotorSpeed=leftMotorDefaultSpeed-100;

  rightMotorSpeed=rightMotorDefaultSpeed-100;

  distance2+=5;                                           //distance to edge of Gizmo

  int pingDistance = microsecondsToCentimeters(ping());   //initial distance from wall

  int stopped = 0;

  while (stopped<3) {

    //accelerates the robot until it reaches a specified top speed

    accelerate();

    if (pingDistance>distance2) {                         //too far? go forward

        Forward(0);

        stopped = 0;

    else if (pingDistance==distance2) {                   //just right? stop, and wait for momentum

        Stop(100);

        leftMotorSpeed=80;

        rightMotorSpeed=80;

        stopped += 1;

    else if (pingDistance<distance2) {                    //too close? back up.. slowly

        BackwardSlowly(0);

        stopped = 0;

    pingDistance = microsecondsToCentimeters(ping());     //gets current distance from wall.

    delay(5);     //a slight delay so that previous pings do not interfere with one another.

  //returns motor speeds to default value

  leftMotorSpeed=leftMotorDefaultSpeed;

  rightMotorSpeed=rightMotorDefaultSpeed;

  currentMenu=draw;                                       //back to main menu

Code for GP2D12 Sensor

void gp2d12Stop( float distance2) {

  delay(500);

  distance2+=3;

  leftMotorSpeed=leftMotorDefaultSpeed-100;

  rightMotorSpeed=rightMotorDefaultSpeed-100;

  gp2d12CurrentDistance = gp2d12();                    //returns distance from wall.

  int stopped=0;

  while (stopped<=3) {

    accelerate();

    gp2d12CurrentDistance = gp2d12();

    if (useSave) saveData(useSave);                  //logs the data

    if (gp2d12CurrentDistance < (distance2-1))  {    //too close? back up

        BackwardSlowly(0); stopped=0;

    else if (gp2d12CurrentDistance > distance2+1) {  ///too far? go closer

        Forward(0); stopped=0;

    else  {                                          //just right? stop and wait for a momentum

        Stop(100);                                   //100 is delay of 100ms.

        stopped++;

        leftMotorSpeed=leftMotorDefaultSpeed-100;

        rightMotorSpeed=rightMotorDefaultSpeed-100;

  Stop(0);

  currentMenu = draw;                                //returns to main menu

The stopped value in both examples above basically waits for momentum. If the robot has been at that location for about 300ms, we know the robot is in the right range. Waiting too long can result in a blip that triggers the robot to act on a false positive. If we had more time, adding a type of filter would help minimize errors.

Servo Control

Here we demonstrate the servo on our robot.

received_411518079230249.mp4

Google Sites

Report abuse