Sonar.nxc

Go to the documentation of this file.

/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
    Sonar.nxc
    Copyright (C) 2008 Naba Kumar   <naba@gnome.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef _SONAR_H_
#define _SONAR_H_

#include "CaliberateMotors.nxc"
#include "Circle.nxc"

#include "Angle.nxc"
#include "Vector.nxc"
#include "PID.nxc"
#include "Motor.nxc"

struct Sonar
{
    int sensorPort;         
    Vector sensorLocation;  
    int sensorOffset;       
    /* Thresholds */
    int sensorCutOffMin;    
    int sensorCutOffMax;    
    int toleranceAngle;     
    long toleranceDistance; 
    /* Sensor motor on which it is mounted */
    int sensorMotor;        
    int gearRatio;          
    int sensorMinAngle;     
    int sensorMaxAngle;     
    int motorMovePower;     
    int motorScanPower;     
    /* Current orientation of sensor motor relative to it's rest position */
    int currentViewAngle;   
    long neutralTachoCount; 
    PIDControl pidControl;  
};

void SonarInit(Sonar &sonar, int sensorPort,
               Vector &sensorLocation, int sensorOffset,
               int sensorCutOffMin, int sensorCutOffMax,
               int toleranceAngle, long toleranceDistance,
               int sensorMotor, int gearRatio,
               int motorMovePower, int motorScanPower,
               int sensorMinAngle, int sensorMaxAngle)
{
    sonar.sensorPort = sensorPort;
    sonar.sensorMotor = sensorMotor;
    sonar.gearRatio = gearRatio;
    sonar.sensorMinAngle = sensorMinAngle;
    sonar.sensorMaxAngle = sensorMaxAngle;
    sonar.sensorCutOffMin = sensorCutOffMin;
    sonar.sensorCutOffMax = sensorCutOffMax;
    sonar.sensorLocation = sensorLocation;
    sonar.sensorOffset = sensorOffset;
    sonar.toleranceAngle = toleranceAngle;
    sonar.toleranceDistance = toleranceDistance;
    sonar.motorMovePower = motorMovePower;
    sonar.motorScanPower = motorScanPower;
    sonar.currentViewAngle = 0;
    sonar.neutralTachoCount = GetOutput(sonar.sensorMotor, TachoCount);
    PIDControlInit(sonar.pidControl,
                   1000,     /* PID scale */
                   450,      /* P value */
                   0,        /* I value */
                   0,        /* D value */
                   5,        /* Steady state counts threshold */
                   2,        /* Output gain */
                   motorMovePower);   /* Output absolute maximum */

}

int SonarGetCurrentViewAngle(Sonar &sonar, Vector &robotLocation)
{
    int absoluteAngle;

    absoluteAngle = sonar.currentViewAngle;
    absoluteAngle = AngleAbsAdd(absoluteAngle, sonar.sensorLocation.theta);
    absoluteAngle = AngleAbsAdd(absoluteAngle, robotLocation.theta);
    return absoluteAngle;
}

int SonarGetMinViewAngle(Sonar &sonar, Vector &robotLocation)
{
    int absoluteAngle;

    absoluteAngle = AngleRelToAbs(sonar.sensorMinAngle);
    absoluteAngle = AngleAbsAdd(absoluteAngle, sonar.sensorLocation.theta);
    absoluteAngle = AngleAbsAdd(absoluteAngle, robotLocation.theta);
    return absoluteAngle;
}

int SonarGetMaxViewAngle(Sonar &sonar, Vector &robotLocation)
{
    int absoluteAngle;

    absoluteAngle = AngleRelToAbs(sonar.sensorMaxAngle);
    absoluteAngle = AngleAbsAdd(absoluteAngle, sonar.sensorLocation.theta);
    absoluteAngle = AngleAbsAdd(absoluteAngle, robotLocation.theta);
    return absoluteAngle;
}

void SonarGetPosition(Sonar &sonar, Vector &robotLocation, Vector &retVec)
{
    /* Orient sonar to robot orientation */
    retVec = sonar.sensorLocation;
    VectorRotate(retVec, robotLocation.theta);
    VectorAdd(retVec, robotLocation);
    retVec.theta = robotLocation.theta + sonar.sensorLocation.theta;
}

bool SonarSweep(Sonar &sonar, int angleFrom, int angleTo, long &rangeData[])
{
     int samplingPeriod = 20; /* msec */
     long motorTacho, output, lastError, tachoFrom, tachoTo;
     long senseDistance;
     int lastViewAngle, senseAngle, dataSize, dataIndex;
     int i;
     
     /* Run the motor. Speed is evaluated later during positioning */
     MotorStart(sonar.sensorMotor, 0);

     lastViewAngle = sonar.currentViewAngle;

     dataSize = ((Abs(angleTo - angleFrom) + 1) * 2);
     ArrayInit(rangeData, 0, dataSize);
     
     /* Initialize rangeData array with angles and unread values (-1) */
     if (angleTo > angleFrom)
     {
         dataIndex = 0;
         for (i = angleFrom; i <= angleTo; i++)
         {
             rangeData[dataIndex] = i;
             dataIndex = dataIndex + 1;
             rangeData[dataIndex] = -1;
             dataIndex = dataIndex + 1;
         }
     }
     else
     {
         dataIndex = 0;
         for (i = angleFrom; i >= angleTo; i--)
         {
             rangeData[dataIndex] = i;
             dataIndex = dataIndex + 1;
             rangeData[dataIndex] = -1;
             dataIndex = dataIndex + 1;
         }
     }
     
     tachoFrom = ((angleFrom - sonar.currentViewAngle) * sonar.gearRatio)/100;
     tachoTo = ((angleTo - sonar.currentViewAngle) * sonar.gearRatio)/100;

     /* Find the closest point either 'From' and 'To' angles from current angle,
      * to start scanning
      */
     if (Abs((lastViewAngle - angleFrom)) > Abs((lastViewAngle - angleTo)))
     {
         long tmp;
         tmp = tachoFrom;
         tachoTo = tachoFrom;
         tachoFrom = tmp;
     }

     /* Move to the closest point */
     sonar.pidControl.absMaxOutput = sonar.motorMovePower;
     PIDControlSetPoint(sonar.pidControl, tachoFrom);
     while(!PIDControlCheckEnd(sonar.pidControl))
     {
         motorTacho = GetOutput(sonar.sensorMotor, BlockTachoCount);
         senseAngle = ((motorTacho * 100)/sonar.gearRatio);
         sonar.currentViewAngle = lastViewAngle + senseAngle;
         output = PIDControlStep(sonar.pidControl, motorTacho);
         MotorSetPower(sonar.sensorMotor, output);
         Wait(samplingPeriod);
     }
     
     /* Rotate to the 'to' angle and at the same time take sonar readings
      * and fill up the rangeData array
      */
     sonar.pidControl.absMaxOutput = sonar.motorScanPower;
     PIDControlSetPoint(sonar.pidControl, tachoTo);
     while(!PIDControlCheckEnd(sonar.pidControl))
     {
         motorTacho = GetOutput(sonar.sensorMotor, BlockTachoCount);

         /* Read sensor */
         senseAngle = ((motorTacho * 100)/sonar.gearRatio);
         sonar.currentViewAngle = lastViewAngle + senseAngle;
         senseDistance = SensorUS(sonar.sensorPort);      /* cm */
         senseDistance = senseDistance * 10;              /* mm */
         senseDistance += sonar.sensorOffset; /* Reading should be from mount */
         dataIndex = 2 * Abs(sonar.currentViewAngle - angleFrom);
         if (dataIndex >= 0 && dataIndex < dataSize)
         {
             if ((senseDistance >= sonar.sensorCutOffMin) &&
                 (senseDistance <= sonar.sensorCutOffMax))
             {
                 rangeData[dataIndex] = sonar.currentViewAngle;
                 rangeData[(dataIndex + 1)] = senseDistance;
             }
         }
         /* Adjust output */
         output = PIDControlStep(sonar.pidControl, motorTacho);
         MotorSetPower(sonar.sensorMotor, output);
         Wait(samplingPeriod);
     }
     MotorStop(sonar.sensorMotor);
}

void SonarSweepPrint(Sonar &sonar, long &rangeData[], int waitTime)
{
    int arrLen = ArrayLen(rangeData);
    /*
    for (int i = 0; i < arrLen; i++)
    {
        int x = (i/8) * 40;
        int y = (i % 8) * 8;
        NumOut(x, y, rangeData[i], true);
    }
    Wait(waitTime);
    */
    ClearScreen();
    for (int i = 0; i < ArrayLen(rangeData); i++)
    {
        int theta = 90 + rangeData[i];
        i = i + 1;
        int d = rangeData[i]/5;
        int x = (d * Cos(theta)) / 100;
        int y = (d * Sin(theta)) / 100;
        if (d > 0)
            CircleOut(x + 50, y, 2, false);
    }
    Wait(waitTime);
}

bool SonarGetObjectDistanceAtAngle(Sonar &sonar, Vector robotLocation,
                                   int viewAngle, long viewDistance,
                                   int &retBestAngle, long &retBestDistance)
{
    Vector returnObject;
    int angleFrom, angleTo, i;
    long minima;
    long rangeData[];
    bool found;

    if ((viewAngle < sonar.sensorMinAngle) || (viewAngle > sonar.sensorMaxAngle))
        return false;

    angleFrom = viewAngle - (sonar.toleranceAngle/2);
    angleTo = viewAngle + (sonar.toleranceAngle/2);

    SonarSweep(sonar, angleFrom, angleTo, rangeData);
    /* SonarSweepPrint(sonar, rangeData, 2000); */

    /* Find the closest reading to the expected distance */
    minima = 1000000;
    for (i = 0; i < ArrayLen(rangeData); i = i + 2)
    {
        int dist = rangeData[i+1];
        if (dist > 0 && Abs(dist - viewDistance) < minima)
        {
            retBestDistance = dist;
            retBestAngle = rangeData[i];
            minima = Abs(retBestDistance - viewDistance);
            found = true;
        }
    }
    return found;
}

bool SonarGetObjectDistanceAtLocation(Sonar &sonar, Vector robotLocation,
                                      Vector &viewLocation, int &retBestAngle,
                                      long &retBestDistance)
{
    Vector sonarLocationWorld, returnObject, objectError;
    int viewAngle, bestAngle;
    long viewDistance, bestDistance;
    long distance;
    bool found;
    
    /* Sonar location in world */
    SonarGetPosition(sonar, robotLocation, sonarLocationWorld);

    /* Absolute view angle in the world - 0 to 360 */
    viewAngle = VectorGetAngleVec(sonarLocationWorld, viewLocation);
    /* View angle relative to sensor orientation */
    viewAngle = AngleAbsSub(viewAngle, sonarLocationWorld.theta);
    /* Relative view angle - -180 to +180 */
    viewAngle = AngleAbsToRel(viewAngle);

    viewDistance = VectorGetDistanceVec(sonarLocationWorld, viewLocation);

    /* Find object at the relative view angle */
    found = SonarGetObjectDistanceAtAngle(sonar, robotLocation,
                                          viewAngle, viewDistance,
                                          bestAngle, bestDistance);
    if (found)
    {
        retBestAngle = bestAngle;
        retBestDistance = bestDistance;
    }
    return found;
}

bool SonarTriangulateLandmarks(Sonar &sonar, Vector &robotLocation,
                               Vector &landmark1, Vector &landmark2,
                               Vector &retLocation)
{
    bool found, intersect;
    long d1, d2;
    int angle1, angle2;
    Vector sonarLocation, intersect1, intersect2;

    found = SonarGetObjectDistanceAtLocation(sonar, robotLocation, landmark1,
                                             angle1, d1);
    if (found == false)
        return false;

    found = SonarGetObjectDistanceAtLocation(sonar, robotLocation, landmark2,
                                             angle2, d2);
    if (found == false)
        return false;

    intersect = CircleIntersectCircle(landmark1, d1, landmark2, d2,
                                      intersect1, intersect2);
    if (intersect == false)
        return false;

    /* Sonar location in world coordinate */
    SonarGetPosition(sonar, robotLocation, sonarLocation);

    d1 = VectorGetDistanceVec(sonarLocation, intersect1);
    d2 = VectorGetDistanceVec(sonarLocation, intersect2);
    
    /* retLoation, currently triangulated position of the sonar, is one
     * closest to the current sonar position.
     */
    if (d1 > d2)
        retLocation = intersect2;
    else
        retLocation = intersect1;

    sonarLocation = sonar.sensorLocation;
    VectorRotate(sonarLocation, retLocation.theta);
    VectorSubtract(retLocation, sonarLocation);

    /* Fixing robot orientation */
    /* Theta set to sonar angle (in world coordinate) */
    retLocation.theta = VectorGetAngleVec(retLocation, landmark2);
    
    /* Theta set to robot orientation by removing sonar angle at the time
     * of landmark2 observation.
     */
    retLocation.theta = AngleAbsSub(retLocation.theta, angle2);

    return true;
}

bool SonarFindObject(Sonar &sonar, Vector robotLocation, Vector &retObject)
{
    long senseDistance = SensorUS(sonar.sensorPort); /* cm */
    senseDistance = senseDistance * 10;              /* mm */
    senseDistance += sonar.sensorOffset; /* Reading from sensor mount point */
    if ((senseDistance >= sonar.sensorCutOffMin) &&
        (senseDistance <= sonar.sensorCutOffMax))
    {
        /* Translate position to robot location */
        Vector retObject = robotLocation;
        //VectorPrint(retObject, "robotLocation", 2000);
        
        /* Translate position to sonar location */
        VectorAdd(retObject, sonar.sensorLocation);
        //VectorPrint(retObject, "sensorLocation", 2000);

        /* Object location relative to the sensor */
        int objectAngle = retObject.theta + sonar.currentViewAngle;
        
        /* Translate position to object location */
        VectorTranslate(retObject, senseDistance, objectAngle);
        //VectorPrint(retObject, "objectLocation", 2000);

        /* Orientation is meaningless for an object location */
        retObject.theta = 0;
        
        return true;
    }
    return false;
}

bool SonarFindObjectAtAngle(Sonar &sonar, Vector robotLocation,
                            int viewAngle, long viewDistance,
                            Vector &retObject)
{
    Vector returnObject;
    int angleFrom, angleTo, bestAngle, i;
    long bestDistance, minima;
    long rangeData[];
    bool found;
    
    if ((viewAngle < sonar.sensorMinAngle) || (viewAngle > sonar.sensorMaxAngle))
        return false;

    angleFrom = viewAngle - (sonar.toleranceAngle/2);
    angleTo = viewAngle + (sonar.toleranceAngle/2);

    SonarSweep(sonar, angleFrom, angleTo, rangeData);
    //SonarSweepPrint(sonar, rangeData, 2000);
    
    /* Find the closest reading to the expected distance */
    minima = 1000000;
    for (i = 0; i < ArrayLen(rangeData); i = i + 2)
    {
        int dist = rangeData[i+1];
        if (dist > 0 && Abs(dist - viewDistance) < minima)
        {
            bestDistance = dist;
            bestAngle = rangeData[i];
            minima = Abs(bestDistance - viewDistance);
            found = true;
            NumOut(0, LCD_LINE1, dist, true);
            NumOut(0, LCD_LINE2, viewDistance, false);
            NumOut(0, LCD_LINE3, minima, false);
            Wait(1000);
        }
    }
    
    if (found)
    {
        /* Translate position to robot location */
        Vector retObject = robotLocation;

        /* Translate position to sonar location */
        VectorAdd(retObject, sonar.sensorLocation);

        /* Object location relative to the sensor */
        int objectAngle = retObject.theta + bestAngle;

        /* Translate position to object location */
        VectorTranslate(retObject, bestDistance, objectAngle);

        /* Orientation is meaningless for an object location */
        retObject.theta = 0;
    }
    return found;
}

bool SonarFindObjectAtLocation(Sonar &sonar, Vector robotLocation,
                               Vector viewLocation, Vector &retObject)
{
    Vector viewLocationRelative, sonarLocationWorld, returnObject, objectError;
    int viewAngle;
    long viewDistance;
    bool found;
    
    // VectorPrint(viewLocationRelative, "view location", 2000);

    found = false;
    
    /* Sonar location in world */
    sonarLocationWorld = robotLocation;
    // VectorPrint(sonarLocationWorld, "Robot location", 2000);
    
    VectorAdd(sonarLocationWorld, sonar.sensorLocation);
    // VectorPrint(sonarLocationWorld, "Sonar location", 2000);

    /* Reference view location relative to sensor */
    viewLocationRelative = viewLocation;
    VectorSubtract(viewLocationRelative, sonarLocationWorld);
    // VectorPrint(viewLocationRelative, "view rel to sonar", 2000);

    /* Reference view angle relative to sensor - 0 to 360 */
    viewAngle = VectorGetAngle(viewLocationRelative);
    viewDistance = VectorGetDistance(viewLocationRelative);
    
    /* view angle - -180 to +180 */
    if (viewAngle > 180) viewAngle = viewAngle - 360;
    
    /* Find object at the relative view angle */
    found = SonarFindObjectAtAngle(sonar, robotLocation, viewAngle,
                                   viewDistance, returnObject);
    objectError = returnObject;
    VectorSubtract(objectError, viewLocation);
    
    if (found && (VectorGetDistance(objectError) < sonar.toleranceDistance))
        retObject = returnObject;
    else
        found = false;
    return found;
}

bool SonarFindObjectRelative(Sonar &sonar, Vector &retObject)
{
    long senseDistance, objectAngle;

    senseDistance = SensorUS(sonar.sensorPort); /* cm */
    senseDistance = senseDistance * 10; /* mm */
    senseDistance += sonar.sensorOffset; /* Reading from sensor mount point */
    if (senseDistance < sonar.sensorCutOffMin && senseDistance > sonar.sensorCutOffMax)
    {
        /* Object location relative to the sensor */
        objectAngle = sonar.sensorLocation.theta + sonar.currentViewAngle;
        retObject.x = sonar.sensorLocation.x + senseDistance * Cos(objectAngle);
        retObject.y = sonar.sensorLocation.y + senseDistance * Sin(objectAngle);
        retObject.theta = 0;
        return true;
    }
    return false;
}

bool SonarFindObjectAtAngleRelative(Sonar &sonar, int viewAngle,
                                    Vector &retObject)
{
    int deltaAngle;
    
    if ((viewAngle < sonar.sensorMinAngle) || (viewAngle > sonar.sensorMaxAngle))
        return false;
    deltaAngle = viewAngle - sonar.currentViewAngle;
    sonar.currentViewAngle = viewAngle;
    deltaAngle = (deltaAngle * sonar.gearRatio) / 100;
    if (deltaAngle > 0)
        RotateMotor(sonar.sensorMotor, sonar.motorMovePower, deltaAngle);
    else if (deltaAngle < 0)
        RotateMotor(sonar.sensorMotor, -sonar.motorMovePower, -deltaAngle);
    return SonarFindObjectRelative(sonar, retObject);
}

#ifdef ENABLE_TEST
#include "RobotPrima.nxc"

void TestSonar ()
{
    Vector robotLocation;
    Vector viewLocation1;
    Vector viewLocation2;
    Sonar sonar;
    Vector sensorLocation;
    Vector retLocation;
    int retBestAngle;
    long retBestDistance;
    bool success;
    long rangeData[];

    VectorInit(sensorLocation, 60, 0, 0);
    VectorInit(robotLocation, 0, 0, 0);
    VectorInit(viewLocation1, 250, 250, 0);
    VectorInit(viewLocation2, 300, 0, 0);

    SetSensorLowspeed(PRIMA_RANGE_SENSOR);
    SonarInit(sonar, PRIMA_RANGE_SENSOR, sensorLocation,
              10,    /* Sensor reading offset from mount point */
              60,    /* Sensor range cutoff (min): 6 cm */
              2000,  /* Sensor range cutoff (max): 200 cm */
              20,    /* Sensor find area angle: +/- 10 degrees */
              100,   /* Sensor find area distance: +/- 10 cm */
              OUT_A, /* sonar motor */
              500,   /* sonar motor gear ratio */
              100,   /* sonar move motor speed */
              10,    /* sonar scan motor speed */
              -70,   /* sonar motor min angle */
              +70);  /* sonar motor max angle */
    /*
    SonarSweep(sonar, -10, 10, rangeData);
    SonarSweepPrint(sonar, rangeData, 2000);
    
    success = SonarGetObjectDistanceAtAngle(sonar, robotLocation, 45, 150,
                                            retBestAngle, retBestDistance);
    if (success == true)
    {
        TextOut(0, LCD_LINE1, "Best Angle 1", true);
        NumOut(0, LCD_LINE2, retBestAngle, false);
        TextOut(0, LCD_LINE3, "Best Distance 1", false);
        NumOut(0, LCD_LINE4, retBestDistance, false);
        Wait(2000);
    }
    SonarGetObjectDistanceAtLocation (sonar, robotLocation, viewLocation1, retBestAngle,
                                                retBestDistance);
    success = true;
    if (success == true)
    {
        TextOut(0, LCD_LINE1, "Best Angle 1", true);
        NumOut(0, LCD_LINE2, retBestAngle, false);
        TextOut(0, LCD_LINE3, "Best Distance 1", false);
        NumOut(0, LCD_LINE4, retBestDistance, false);
        Wait(2000);
    }
    */
    success = SonarTriangulateLandmarks(sonar, robotLocation, viewLocation1,
                                        viewLocation2, retLocation);
    if (success == true)
    {
        VectorPrint(retLocation, "Robot Location", 2000);
    }
    Wait(20000);
}

/*
task main()
{
    TestSonar();
}
*/
#endif /* ENABLE_TEST */
#endif /* __SONAR_H_ */