Homesick emotional robo

This is my first (ready to publish) RobotC program and Lego Mindstorm vehicle.

ultrasonic_robo

ico_robotC RobotC source code and sound files
ico_ldd Lego Digital Designer vehicle project

This robo is programmed to walk across the room avoiding obstacles. It starts to complain (for example telling dirty words) when stuck between obstacles for too long time. After arriving to black ring on Lego Mindstorms test pad robo tries to localize center of the pad and orient themselves to 0 degrees.

Let’s start…

Sensors configuring:

#pragma config(Sensor, S1, touchSensor_L,  sensorTouch)
#pragma config(Sensor, S2, touchSensor_R,  sensorTouch)
#pragma config(Sensor, S3, colourSensor,   sensorCOLORRED)
#pragma config(Sensor, S4, sonar,          sensorSONAR)
//*!!Code automatically generated by 'ROBOTC' configuration wizard!!*//

We need some global constans and variables:

bool bIsBlackLine = false;
int nAccident = 0;
const float fMoveRatio = 20.525;
const float fTurnRatio = 2.483;
typedef struct
{
  int nGapAngle;
  int nGapWidth;
  int nGapDistance;
} GT;
  • bIsBlackLine tells if robo is inside black ring or outside. We suppose that it’s outside on the beggining. This variable can be modified by lookForBlackLine task. I’ll tell about it later.
  • nAccident shows if some touch sensor is pressed. Also can be modified by additional task.
  • Ratio variables depends on tyres and gears mounted. fMoveRatio = 20.525 means: you must turn both servos the same direction 20.525 degrees to move robo one centimeter.
  • fTurnRatio = 2.483 – you must turn both servos different directions 2.483 degrees to turn robo 1 degree.
  • GT struct will be used in the table of surrounding obstacles positions.

Egzamining surface

task lookForBlackLine()
{
  while (true)
  {
    if (SensorValue(colourSensor) > 60)
    {
      while (SensorValue(colourSensor) > 30)
      {
      }
      ClearTimer(T1);
      while (time1[T1] < 20)
      {
      }
      if (SensorValue(colourSensor) < 30)
      {
        while (SensorValue(colourSensor) < 30)
        {
        }
        ClearTimer(T1);
        while (time1[T1] < 200)
        {
        }
        if (SensorValue(colourSensor) > 50)
          bIsBlackLine = true;
      }
    }
  }
}

Task above modifies bIsBlackLine to tell the main task that robo has arrived the test pad and is inside black ring.
When robo notices it is on white surface starts to look for black line. 20 ms delay is set to prevent robo from concerning another accidental narrow black lines as main line. If there is white surface during next 200 ms robo is supposed to be inside the black ring. Sometimes it fails… but rarely.

Using touch sensors

If angle between wall and running robo is too low it’s impossible for ultrasonic sensor to measure distance correctly. That’s why I have used touch sensors which are triggered by the bumper.

task wallAvoid()
{
  while (true)
  {
    if (SensorValue(touchSensor_L) == 1)
      nAccident = 1;
    if (SensorValue(touchSensor_R) == 1)
      nAccident = 2;
    wait1Msec(50);
  }
}

wallAvoid() task observes touch sensors and changes value of nAccident global variable depending on pressed sensor. I think it is reasonable to delay task for 50 ms not to hog the processor… regardless of fact that this is very litle resources consuming task.

Move solutions

void softDistance(float fMaxSpeed, float fDistance, float fEasePart, int nDirectionL, int nDirectionR)
{
  const float fDistanceA = round(fDistance * fEasePart / 2);
  const float fDistanceB = round(fDistance - fDistance * fEasePart);
  float fPointZero = nMotorEncoder[motorB];
  nMotorPIDSpeedCtrl[motorB] = 1;
  nMotorPIDSpeedCtrl[motorC] = 1;
  nPidUpdateInterval = 1;
  float fPartDone = 0;
  int nCurrentSpeed = 0;
  while (fPartDone < fDistanceA)
  {
    nCurrentSpeed = round((1-(fPartDone/fDistanceA))*3)+round((fPartDone/fDistanceA)*fMaxSpeed);
    motor[motorB] = nCurrentSpeed * nDirectionL;
    motor[motorC] = nCurrentSpeed * nDirectionR;
    fPartDone = abs(nMotorEncoder[motorB] - fPointZero);
  }

  while (abs(nMotorEncoder[motorB] - fPointZero) - fDistanceA < fDistanceB)
  {
  }

  float fStartSpeed = abs(motor[motorB]);
  fPartDone = 0;
  while (fPartDone < fDistanceA)
  {
    nCurrentSpeed = fStartSpeed - round((fPartDone / fDistanceA) * fStartSpeed) + round((fPartDone / fDistanceA) * 3);
    motor[motorB] = nCurrentSpeed * nDirectionL;
    motor[motorC] = nCurrentSpeed * nDirectionR;
    fPartDone = abs(nMotorEncoder[motorB] - fPointZero) - (fDistanceA + fDistanceB);
  }
  motor[motorB] = 0;
  motor[motorC] = 0;

  nPidUpdateInterval = 20;
}
  • softDistance() function makes robo to start smoothly and stop smoothly after distance set.
  • fMaxSpeed parameter sets max speed for both servos.
  • fDistance parameter sets both servos’ rotation angle.
  • fEasePart parameter sets part of fDistance when robo accelerates and decelerates. It means that if we set this value to 1 robo will accelerate half its way and decelerate half its way.
  • nDirectionL and nDirectionR sets direction of servos rotation. 1 is left and -1 is right… or inversely. Direction depends on gears too. Yes – robo can turn smoothly too.

Instead of using integers I used floats because some computations needs float operators.
The first robo computes distance of acceleration and deceleration and next it computes distance of constant speed.
This function is very simple and uses only one encoder (motor[B]) to control distance but this is precise enough solution.
‘nPidUpdateInterval’ is set to 1 in order move to be more precise.
In the first ‘while’ loop robo increases speed from 0 to fMaxSpeed value. In fact it increases speed from 3 becouse speed depends on distance and it would be impossible to start moving the robo with speed value close to zero.
Next ‘while’ loop waits until all the constant-speed-distance is finished. Third loop acts likewise the first one but it decelerates robo.
We should set ‘nPidUpdateInterval’ to the default value finally.

void softStart(int nSpeedL, int nSpeedR, int nDuration)
{
  short sDirectionL = sgn(nSpeedL);
  short sDirectionR = sgn(nSpeedR);
  int nFrequency = nDuration / abs(nSpeedL);
  for (int i=1; i<=abs(nSpeedL); i++)
  {
    ClearTimer(T1);
    motor[motorB] = i*sDirectionL;
    motor[motorC] = i*sDirectionR;
    while (time1[T1] < nFrequency)
    {
    }
  }
}

softStart() function lets robo to accelerate smoothly. We can set time of acceleration and speed value of both servos but absolute value of servos must be equal each other. nSpeedL can differ from nSpeedR when you need to start rotating robo. When function ends robo still moves! Next function is very similar:

void softStop(int nDuration)
{
  short sDirectionL = sgn(motor[motorB]);
  short sDirectionR = sgn(motor[motorC]);
  int nFrequency = nDuration / abs(motor[motorB]);
  for (int i=abs(motor[motorB]); i>=0; i--)
  {
    ClearTimer(T1);
    motor[motorB] = i * sDirectionL;
    motor[motorC] = i * sDirectionR;
    while (time1[T1] < nFrequency)
    {
    }
  }
}

You can set time of breaking.

Looking for gaps between obstacles

Noticed obstacle triggers findGap() function.

void findGap(int nMinDistance, int nMinGapWidth)
{
  StopTask(lookForBlackLine);
  StopTask(wallAvoid);
  nMotorEncoder[motorA] = 0;
  int uDistance[35];
  int i = 0;
  int nStart10Degrees = 0;
  while (nMotorRunState[motorA] != runStateIdle)
  {
  }
  motor[motorA] = 20;
  while (nMotorEncoder[motorA] <= 350)
  {
    uDistance[i] = SensorValue(sonar);
    while (nStart10Degrees + 10 > nMotorEncoder[motorA])
    {
    }
    nStart10Degrees = nMotorEncoder[motorA];
    i++;
  }
  motor[motorA] = 0;
  nMotorEncoderTarget[motorA] = nMotorEncoder[motorA];
  motor[motorA] = -50;

  GT gapTable[10];
  for (i = 0; i < 10; i++)
  {
    gapTable[i].nGapAngle = 0;
    gapTable[i].nGapWidth = 0;
    gapTable[i].nGapDistance = 0;
  }

  i = 0;
  int l = 0;
  int k = 0;
  while (i < 36)
  {
    if (uDistance[i] > nMinDistance)
    {
      k = 0;
      while (uDistance[i+k+1] > nMinDistance && abs(uDistance[i+k+1]-uDistance[i+k]) < 50)
        k++;
      if (k >= nMinGapWidth)
      {
        gapTable[l].nGapAngle = i*10+((k*10)/2);
        gapTable[l].nGapWidth = k;
        gapTable[l].nGapDistance = uDistance[i+k/2];
        l++;
      }
      i = i + k + 1;
    }
    else
      i++;
  }

  int nGapTableSize = 0;
  for (i = 0; i < 10; i++)
  {
    if (gapTable[i].nGapWidth != 0)
      nGapTableSize++;
  }

  int nShortGapCounter = 0;
  for (i = 0; i < nGapTableSize; i++)
  {
    if (gapTable[i].nGapDistance < 50)
      nShortGapCounter++;
  }

  int sStrategy;
  if (nGapTableSize-nShortGapCounter < 2)
    sStrategy = 3;
  else
  {
    srand(nSysTime);
    sStrategy = (rand() % 2) + 1;
  }

  int nResultAngle;
  switch (sStrategy)
  {
    case 1:
      if (gapTable[0].nGapAngle < 360 - gapTable[nGapTableSize-1].nGapAngle)
        nResultAngle = gapTable[0].nGapAngle;
      else
        nResultAngle = gapTable[nGapTableSize-1].nGapAngle;
      break;
    case 2:
      int nMaxWidth = 0;
      for (i = 0; i < nGapTableSize; i++)
        if (gapTable[i].nGapWidth > nMaxWidth)
        {
          nMaxWidth = gapTable[i].nGapWidth;
          nResultAngle = gapTable[i].nGapAngle;
        }
      break;
    case 3:
      int nMaxGapDistance = 0;
      for (i = 0; i < nGapTableSize; i++)
        if (gapTable[i].nGapDistance > nMaxGapDistance)
        {
          nMaxGapDistance = gapTable[i].nGapDistance;
          nResultAngle = gapTable[i].nGapAngle;
        }
      break;
  }

  if (nResultAngle <= 180)
    softDistance(50, nResultAngle * fTurnRatio, 0.5, -1, 1);
  else
    softDistance(50, (360 - nResultAngle) * fTurnRatio, 0.5, 1, -1);

  StartTask(lookForBlackLine);
  StartTask(wallAvoid);
}

findGap() is essential function to recognize where to go when obstacle is on the road.

  • nMinDistance tells the robo the minimum distance to be concerned as passable way to go.
  • nMinGapWidth (multiplied by 10) is minimum angle for gap to be passable for robo

I think it is reasonable to stop all necessary tasks. We have started them in main() task (look into source code).
uDistance[35] is an array we will store distances from obstacles around the robo in.
nStart10Degrees is a variable to controll ultrasonic sensor rotation to measure distance every 10 degrees of sensor rotation.
‘while’ loop makes uDistance[] array full.
Next we declare and zero gapTable[] array to store parameters of gaps between obstacles. ‘GT’ struct allows to store angle of miggle of the gap, it’s width in degrees and distance to the nearest obstacle. After that we can analyze uDistance[] array to find proper gaps for continue robo move. Analyzing uDistance[] array we concern too short distance and 50 cm difference between neighbour distances as a gap splitter.
findGap
Robo counts gaps which aren’t very deep (less than 50 cm). We need to know it in order robo to avoid stucking. If there is only one gap deeper than 50 cm ‘longest-distance-gap’ strategy is chosen to find the way. In other way robo will randomly choose strategy of pointing direction:

  • closest-gap-strategy
  • widest-gap-strategy

Finally robo uses softDistance() function tu turn robo proper angle.

Preparing to looking for center of test pad

void TurnToBlackLine()
{
  nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
  motor[motorB] = -10;
  motor[motorC] = -20;
  while (SensorValue(colourSensor) > 35)
  {
  }
  bFloatDuringInactiveMotorPWM = true;
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(200);
  bFloatDuringInactiveMotorPWM = false;
  softDistance(20, 10 * fTurnRatio, 0.3, 1, -1);
}
void lineTrack()
{
  bool bIsColour = false;
  SensorType[colourSensor] = sensorCOLORFULL;
  while(!bIsColour)
  {
    if (SensorValue(colourSensor) == 6)
    {
      motor[motorB] = 40;
      motor[motorC] = 0;
    }
    if (SensorValue(colourSensor) == 1)
    {
      motor[motorB] = 0;
      motor[motorC] = 40;
    }
    if (SensorValue(colourSensor) == 4)
    {
      motor[motorB] = 0;
      motor[motorC] = 0;
      bIsColour = true;
    }
    else
      bIsColour = false;
  }
}

There are two functions for looking yellow spot on the test pad. The first tries to place robo very near black line and the second makes roboto follow the line and stop on the yellow spot.
We can start looking for center of the test pad from this point.

Where is center?

void lookForCenter()
{
  const float fAlfaAngle = 45;
  const float fTryAngle = 7 * fTurnRatio;
  float fTryCounter = 0;

  nMotorPIDSpeedCtrl[motorB] = 1;
  nMotorPIDSpeedCtrl[motorC] = 1;
  for (int i=0; i<5; i++)
  {
    bool bIsTooFar = false;
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    softDistance(15, fTryAngle, 1, -1, 1);
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    softStart(20, 20, 500);
    while (SensorValue(colourSensor) != 5)
    {
      if (nMotorEncoder[motorB] > (45 * fMoveRatio))
      {
        softStop(500);
        int nReturnDistance = nMotorEncoder[motorB];
        softDistance(30, nReturnDistance, 1, -1, -1);
        bIsTooFar = true;
        break;
      }
    }
    if (bIsTooFar == false)
      break;
  }
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(200);

  float fDistanceA = nMotorEncoder[motorB];

  softDistance(30, fDistanceA, 1, -1, -1);

  softDistance(20, (fAlfaAngle * fTurnRatio), 1, -1, 1);

  for (int i=0; i<5; i++)
  {
    fTryCounter = i+1;
    bool bIsTooFar = false;
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    softDistance(15, fTryAngle, 1, 1, -1);
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    softStart(20, 20, 500);
    while (SensorValue(colourSensor) != 5)
    {
      if (nMotorEncoder[motorB] > (50 * fMoveRatio))
      {
        softStop(500);
        int nReturnDistance = nMotorEncoder[motorB];
        softDistance(30, nReturnDistance, 1, -1, -1);
        bIsTooFar = true;
        break;
      }
    }
    if (bIsTooFar == false)
      break;
  }
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(200);

  float fDistanceB = nMotorEncoder[motorB];

  float fFinalAlfaAngle = fAlfaAngle - (fTryCounter * fTryAngle / fTurnRatio);

  float fDistanceC = sqrt((fDistanceA*fDistanceA) + (fDistanceB*fDistanceB) - 2 * fDistanceA * fDistanceB * cosDegrees(fFinalAlfaAngle));
  float fBetaAngle = asin((fDistanceA*sinDegrees(fFinalAlfaAngle)/fDistanceC))*(180/PI);

  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  float fGoTurnAngle = (180 - fBetaAngle) * fTurnRatio;
  softDistance(30, fGoTurnAngle, 1, 1, -1);

  softStart(-20, -20, 500);
  while (SensorValue(colourSensor) != 1)
  {
  }
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(200);

  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  softStart(20, 20, 500);
  wait1Msec(2000);
  while (SensorValue(colourSensor) != 1)
  {
  }
  motor[motorB] = 0;
  motor[motorC] = 0;
  int nWidth = abs(nMotorEncoder[motorB]);
  wait1Msec(200);

  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  softDistance(30, ((nWidth/2)-(3*fMoveRatio)), 1, -1, -1);

  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  softDistance(30, (93 * fTurnRatio), 1, -1, 1);
  softDistance(30, 6 * fMoveRatio, 1, 1, 1);
}

It is time to measure some distances and compute center position of the test pad.
lookForCenter
Function moves the robo from yellow spot to red line to measure fDistanceA, goes backward to yellow spot, turns fAlfaAngle angle and measures fDistanceB. Thats why robo is able to turn properly to be parallel to red line. Robo looks for opposite sides of black-lined ring and goes to the center of the pad.
The first ‘for’ loop ensures 5 fDistanceA measure tries. While running robo observes surface and checks if there is red line. It checks if it hadn’t run unbelievable distance (45 cm). If robo did, it goes back to yellow spot, turns left fTryAngle degrees and tries again.
When robo meets red line, it writes encoder value as fDistanceA, goes back, turns fAlfaAngle and tries to measure fDistanceA in a similar way.
Beta angle is computed using law of cosines and robo can turn right accordingly to fBetaAngle. It is necessary to go back in order to meet black line. Then robo can measure width of ring on test pad and goes straight to center of it.

Being stuck and complaining

void ohShit()
{
  int nStartVolume = nVolume;
  srand(nSysTime);
  int nShitSoundNumber = (rand() % 2) + 1;
  ClearSounds();
  nVolume = 4;
  while(bSoundActive)
  {
  }
  switch (nShitSoundNumber)
  {
    case 1:
      PlaySoundFile("kurwa1.rso");
      break;
    case 2:
      PlaySoundFile("kurwa2.rso");
      break;
  }
  nVolume = nStartVolume;
}

ohShit() plays sounds when robo want to say that it gets anxious.

main() task

task main()
{
  nMotorPIDSpeedCtrl[motorA] = 1;
  nMotorPIDSpeedCtrl[motorB] = 1;
  nMotorPIDSpeedCtrl[motorC] = 1;
  bMotorReflected[motorA] = true;
  bMotorReflected[motorB] = true;
  bMotorReflected[motorC] = true;
  nMaxRegulatedSpeedNxt = 1000;
  StartTask(lookForBlackLine);
  StartTask(wallAvoid);
  ClearTimer(T2);
  int nStuckCounter = 0;
  while (!bIsBlackLine)
  {
    ClearTimer(T3);
    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    softStart(100, 100, 300);
    while (SensorValue(sonar) > 40)
    {
      if (bIsBlackLine || nAccident != 0)
        break;
    }
    softStop(200);
    if (bIsBlackLine)
      break;
    if (nAccident == 1)
    {
      softDistance(30, 7 * fMoveRatio, 0.5, -1, -1);
      softDistance(30, 20 * fTurnRatio, 0.5, 1, -1);
      nAccident = 0;
    }
    if (nAccident == 2)
    {
      softDistance(50, 7 * fMoveRatio, 0.5, -1, -1);
      softDistance(50, 20 * fTurnRatio, 0.5, -1, 1);
      nAccident = 0;
    }
    if (time1[T3] > 1500)
    {
      nStuckCounter = 0;
      ClearTimer(T2);
    }
    nStuckCounter++;
    if (nStuckCounter > 3 && time100[T2] < 400)
    {
      ohShit();
      ClearTimer(T2);
    }

    findGap(60, 3);
  }
  StopTask(lookForBlackLine);
  StopTask(wallAvoid);
  TurnToBlackLine();
  lineTrack();
  softDistance(30, 20 * fTurnRatio, 1 , -1, 1);
  lookForCenter();
  nMotorPIDSpeedCtrl[motorA] = 0;
  nMotorPIDSpeedCtrl[motorB] = 0;
  nMotorPIDSpeedCtrl[motorC] = 0;
}

At first we should enable PID for each servo. I wrote most functions for my previous temporary vehicle. That’s why I need to set ‘bMotorReflected’ value true for each servo. Now, we can take advantage of multitasking and run two additional task for observing surface and for hitting the walls. It’s reasonable for additional task not to be very resources demanding.
[T2] timer is reset to control 40 s time period when robo isn’t able to go straight for at least 1.5 s and let nStuckCounter to be increased. If robo hadn’t four opportunities to go straight for 1.5 s during 40 s, it starts complaining.
Until bIsBlackLine global variable isn’t changed by lookForBlackLine() task to true, there is running ‘while’ loop. That loop controls robo moving from obstacle to next obstacle. We need to reset next timer [T3] to check the time period when robo goes straight not meeting any obstacle. If it goes longer than 1.5 s, robo isn’t stuck.
In the ‘while’ loop we observe distance when running. If ultrasonic sensor discovered an obstacle closer than 40 cm loop brakes. It brakes also in case of meeting black line or hitting the wall. Next, there are ‘if’ statements making robo move back when bumper is hit. We also need to check complaining conditions. There are next two ‘if’ statements checking [T3] timer, quantity of obstacles met and [T2] timer. It is enough to make decision to run ohShit() function or not.
If one obstacle appeared robo triggers findGap() function to find a free way.
Finally, black line is found so robo can finish looking for obstacles and stop all additional task. Tracking black line robo is supposed to get to yellow spot and run lookForCenter() function which centers robo on the pad.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s