Arduino tečaj: praćenje zida

6. lesson

Ovo je dio cjelovitog tečaja Arduino-a:

1. Štampajte dijelove koristeći ovaj Sketchup model RescueLine.zip.
2. sastavite robota koristeći uputa ze sastavljanje ,
3. Spajite kablove koristeći plan spajanja .
4. Programirajte robota .
   1. Osnove.
   2. Praćenje crte.
   3. Naredbe.
   4. LIDARi.
   5. Prekidač, IMU, 8x8 LED.
   6. Praćenje zida (ova stranica).
   7. Srebro.
   8. 8x8 LED programming.
   9. Robotska ruka.
   10. Raspberry Pi.
   11. Prepoznavanje kamerom, OpenCV.
   12. Arduino i RPI surađuju.

Line following

Wall following is similar to line following. When learning about line following we didn't stress that there were in fact 2 errors, not 1: robot's center position against the line and robot's direction. The proper position would be: zero mm distance to the line and robot's longitudinal axis parallel to tangent to the line. It was hard to estimate the later error so we skipped the problem. However, when following a wall, this error must be taken into account, and it is not hard to do so. Having to cope with 2 errors, we will introduce 2 PID objects (as each stores only one error and the parameters can be different).

Again, enter the orange changes:

#include <Commands.h> //ML-R library You use to issue commands to Your robot.
#include <Displays.h> //Libraray for 8x8 3-color display
#include <IMUBoschBNO055.h> //IMU
#include <Motors.h> //Library Motors will be used. It contain motor-control functions.
#include <PID.h> //Library that implements a very simple proportional–integral–derivative controller. Check Wikipedia.
#include <ReflectanceSensors.h>
#include <Switches.h>
#include <VL53L0Xs.h> //STM VL53L0X is the name of LIDAR chip.

Commands commands(&Serial2); // Object for commands is created.
Displays displays;
IMUBoschBNO055 imu(&Serial2);
Motors motors(true, &Serial2); //motors is an C++ object of class Motors. If You do not understand this, never mind. Just alwasys use motors with this line.
PID pidLine(20, 15, 0.01, &Serial2); //PID object: pidLine. After studying Wikipedia, alter the 3 parameters.
PID pidWallAngle(10, 1, 0, &Serial2); //Angle error. 10 and 1 are arbitrary parameters: find the correct values for Your robot.
PID pidWallDistance(10, 1, 0, &Serial2); // Distance to wall error. 10 and 1 are arbitrary.
ReflectanceSensors reflectanceSensors(0, 0.25, &Serial2);
Switches switches;
VL53L0Xs lidars(&Serial2); //LIDARs

void setup() {
  Serial.begin(115200);    // Serial (USB) communication to our computer is established. You call function begin of the object Serial with 1 argument: 115200.
  Serial2.begin(9600);     // Start serial communication with Bluetooth adapter.
  Wire.begin();            //Start I2C bus.
  delay(1000);             // Do nothing for 1000 ms (1 sec.) - wait for the USB connection.
  Serial.println("Start"); // Display "Start" on PC's screen. Function println() prints the argument ("Start") and jumps to a new line.
  
  //The following 2 lines will create 2 commands. First parameter is shortcut that invokes the command. The second 
  //is a function pointer. Again, if that term doesn't ring a bell, it will be no problem. Just write here the name of the function You 
  //want to invoke, for example, after typing "lin" in your mobile phone. The third parameter is an explanation.
  commands.add("dis", displayTest, "Display test");
  commands.add("imu", imuTest, "IMU test");
  commands.add("cal", calibrate, "Calibrate");
  commands.add("lid", lidarsTest, "Test LIDARs");
  commands.add("lin", line, "Follow line", 33); //Pressing the switch (connected to pin 33) will execute this command.
  commands.add("swi", switchesTest, "Switches test");
  commands.add("wal", wall, "Follow left wall");
  
  //Display 8x8
  displays.add(0x70);

  //IMU, false as parameter sets its I2C address to 0x28 instead of 0x29.
  imu.add(false);
  
  //LIDARs. Each add command uses 2 parameters: digital pin number, and a chosen I2C (hexadecimal) address.
  //Prefix "0x" precedes hexadecimal number. As every LIDAR uses the same I2C address, 0x29, there must be a way to resolve the conficts, and there is. During boot phase the library enables all the LIDARs, one by one, using 0x29, and changes their addresses into the ones indicated as the second parameter.
  lidars.add(13, 0x22);    
  lidars.add(17, 0x24);
  lidars.add(29, 0x23); 
  lidars.add(31, 0x26);
  lidars.add(30, 0x25); 
  lidars.add(12, 0x27);
  lidars.begin();

  //In the following 4 lines we will add 4 motors. First 2 parameters, numbers (like 3, 5), are pins that are connected
  //to motor controller. Third parameter (true or false) is true when it is a left motor, otherwise false. There is an optional fourth parameter in the
  //first 2 lines and there can be even more of them. Optional (default) parameters do not have to be mentioned.
  motors.add(3, 5, true, true);
  motors.add(23, 4, false);
  
  //10 reflectance sensors will be added. ML-R libraries always include new elements (like sensors and motors) using the function add().
  //First parameter is analog pin's name, second millimeters from the central longitudinal axis (negative numbers for left sensors).
  //The last parameter is always true and enables that sensor for the libary's line following algorithm.
  reflectanceSensors.add(A11, -49.5, true); 
  reflectanceSensors.add(A10, -38.5, true);
  reflectanceSensors.add(A13, -27.5, true);
  reflectanceSensors.add(A12, -16.5, true);
  reflectanceSensors.add(A1, -5.5, true);
  reflectanceSensors.add(A14, 5.5, true);
  reflectanceSensors.add(A0, 16.5, true);
  reflectanceSensors.add(A2, 27.5, true);
  reflectanceSensors.add(A17, 38.5, true);
  reflectanceSensors.add(A15, 49.5, true);
  reflectanceSensors.eepromRead();//Reading EEPROM calibration data into RAM.
  
  //Switches
  switches.add(33);

  displays.fillScreen(0, LED_GREEN);
  displays.writeDisplay(0);
}

void loop() {
  //List all the available commands and wait for user's input.
  do {
    commands.list();
  } while (commands.prompt());
}

/**Function for reflectance sensors calibration
*/
bool calibrate() {
  reflectanceSensors.calibrate();
  return true;
}

/** Any key pressed?
@return - true if so.
*/
bool commandAvailable() {
  return commands.available();
}

bool displayTest() {
  displays.test(0, commandAvailable);
  return true;
}

//The next 6 functions convert mm to cm and introduce some more meaningful names. First of the last 2 letters (L, F or R) is robot's side and the one following determines the sensor on that side. For example LB - left side of the robot (L), back (rear sensor, B). F is front, L is left, R is right, B is back (rear). 
float distanceLB() { return lidars.distance(0) / 10.0; }
float distanceLF() { return lidars.distance(1) / 10.0; }
float distanceFL() { return lidars.distance(2) / 10.0; }
float distanceFR() { return lidars.distance(3) / 10.0; }
float distanceRF() { return lidars.distance(4) / 10.0; }
float distanceRB() { return lidars.distance(5) / 10.0; }

/**All the ML-R libraries invoke this function when something goes wrong. Here we stop the motors, display the error
message which enters the function as the only argument ("message") and then enter an endless loop. while(1) never ends as the condition
(1) is always true. The only command that is in the loop is ";" (empty command - does nothing).
@param message
*/
void error(String message){
  motors.go(0, 0);         //stop the motors to avoid harm
  Serial.println(message); //display error message
  while(1)                 //never stop
    ;                      //do nothing
}

bool imuTest() {
	imu.test(commandAvailable);
	return true;
}

/** Test LIDARs
@return - always true
*/
bool lidarsTest() {
  lidars.test(commandAvailable);
  return true;
}

/** A line-following function, can invoked by a command.
*/
bool line(){
  while (!commandAvailable()) { // There is a way to stop the line following - press a key.
    bool found, nonLineFound;
    //The following function returns position of the black line against the longitudinal centre of the robot. found and notFound are output
    //parameters. found is true if a line is found (at least one sensor black). nonLineFound not found (that at least one sensor doesn't detect black).
    float centerMm = reflectanceSensors.findLine(found, nonLineFound);
    //centerMm is in fact error in line following. It should be 0. PID algorithm uses it as an input and calculates error correction:
    float correction = pidLine.calculate(centerMm);

    const int speedAhead = 60; //Average speed of the left and right motor. You can change this number.
    if (found)
      motors.go(speedAhead + correction, speedAhead - correction);//Here we control the motors.
    else
      motors.go(50, 50); //If no line, go straight ahead.
  }
  motors.go(0, 0);
  return true;
}

bool switchesTest() {
  switches.test(commandAvailable);
  return true;
}

/** Follow left wall
@return - always true
*/
bool wall() {
  while (!commandAvailable())
    wall(true, 10); // true - left wall, 10 - follow it at 10 cm.
  motors.go(0, 0);
  return true;
}

/** Follow wall
@param isLeftWall - left wall (true) or right (false)
@param wallDistanceNeeded - wall following distance in cm.
*/
void wall(bool isLeftWall, float wallDistanceNeeded) {

  //Determine front and back distances to the wall
  float wallDistanceFront = isLeftWall ? distanceLF() : distanceRF();
  float wallDistanceRear = isLeftWall ? distanceLB() : distanceRB();
  const float betweenLIDARs = 13.5; //This number is needed to calculate distance to the wall.

  //Check Your knowledge of primary school math to prove that this calculation yields distance to the wall:
  float distanceToWallMeasured = betweenLIDARs *(wallDistanceFront + wallDistanceRear) / 2.0 /
	hypot(betweenLIDARs, wallDistanceFront - wallDistanceRear);
  float tooFar = distanceToWallMeasured - wallDistanceNeeded; //This is the error in distance - we will try to make it 0.
  float correction = pidWallDistance.calculate(tooFar) * (isLeftWall ? -1 : 1); //PID calculates the correction for position

  //This is the second error, direction (should be parallel to the wall)
  float clockwiseTooMuch = (wallDistanceFront - wallDistanceRear) * (isLeftWall ? -1 : 1); // 2nd error.
  correction += pidWallAngle.calculate(clockwiseTooMuch); //PID calculates correction for the 2nd error and we add it to the first correction.

  const int speedAhead = 60; // Average speed ahead.
  motors.go(speedAhead + correction, speedAhead - correction);//Here we control the motors, by applying the cumulative correction for 2 errors.
}

A non-obvious use of wall following: avoiding obstacle on line. First implement wall following fully. The part that is missing is avoiding front wall by turning away from the wall You are following. Use both front sensors to do this, by calculating front distance continually during the turn. Use the formula that calculated variable distanceToWallMeasured in the program above. After You complete the turning, notice that You can avoid an obstacle on the line by switching from line following to wall following! No other action is needed. Just make sure You are watching for the line appearing again. When it happens, switch from wall following to line following. There is a warning, though. This works when both sensors on robot's side detect the obstacle. If not so, You will have to correct the algorithm. Still, this is a superior way to handle obstacles, compared to preprogrammed times (of rotation,...).

Previous lesson

Next lesson