Primitive Self-Driving Robot on Ks0428 Mini Tank Robot V2

Hi! I am software engineer and I love robotics. I am going to share with you my experience as my own ideas or experiments and something I am facing at my job.

Imagine: you have a passion to robotics. And your first question is — what I should do first to learn about robotics more.

Well we have lots of simulators and visualization software where you can try something — Gazebo, Omniverse from Nvidia etc, and we will review some of it later, but it is much more interesting to deal with real robot.

For your very beginning it is better to work with a simple wheel robot. It is quite simple in controlling, so you have only linear velocity and angular velocity, and you consider the moving only in two axis.

I bought this cute baby to develop some stuff by myself and for better understanding how it works from scratch.

Image 1: Mini Tank Robot V2

MiniTank Robot consists of:

• Tank Robot classic — the platform with tank wheels and wheels’ motors;
• Keystudio V4 development board — its’ processor is compatible with Arduino, has 14 digital input/output pins (of which 6 can be used as PWM outputs), 6 analog inputs, a 16 MHz crystal oscillator;
• L298P Shield — Motor Driver Module;
• VP Sensor Shield — the board where sensors are connected;

Image 2: VP Sensor Shield Pins

These boards are set one by one as you can see on Image 1 above.

Let us consider the task of our primitive autonomy: this vehicle should move across the room and move from / around obstacles. For that we should use the information from sensors and by considering this information we should take decisions of how we move. To be more precise: what action our vehicle should do.

We have sensors:

• HC-SR04 Ultrasonic Sensor;
• Photocell Sensor;

From above list we have the one senor we can use— Ultrasonic Sensor. We cannot use photocell because it cannot recognize the obstacles. As we have only one sensor to use, we are going to move mostly straight and turn left and right. Moving back we will use only for moving from obstacles.

For better observation we should turn somehow our Ultrasonic Sensor. For that we can use:

• 9G Servo Motor;

So after assembling the robot parts, swearing during discovering that you have switched the different part wrong way — here we are (My personal hell was the super little screws). We have a robot, which we can program with Arduino.

There are ready modules for working with different sensors and move our hell machine. So, I’ve got the following modules.

Module for working with Ultrasonic sensor.

Let me explain how Ultrasonic sensor works. The sensor uses sonar to determine distance to an object. It sends the waves and waits for reflection. The time of receiving reflection has linear dependence with distance. Here is the Image 3 for visualization.

Image 3: Ultrasonic sensor’s work

Ultrasonic sensor has two pins for sending and receiving data from it — trigger pin and echo pin. We should connect them to pins on VP Sensor Shield and then define the pins in Arduino code.

#define trigPin = 5    // Trigger
#define echoPin = 4    // Echo

Also we should setup pinmode:

//Define inputs and outputs
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

We are sending three impulses to trigger pin (HIGH means logical one, LOW means logical zero) and then read from echo pin time and translate it to distance. Here is the code after some modifications for my task:

long get_distance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);.
  long duration = pulseIn(echoPin, HIGH, 100000);
  long cm = (duration/2) / 29.1;
  return cm;
}

Module for working with Servo Motor.

For better observing the space around we are using Servo Motor for rotating Ultrasonic Sensor. Let us check what is said in documentation to Robo Tank about Servo Motor: “Servo Motor consists of housing, circuit board, core-less motor, gear and position sensor. It receives the signal sent by micro controller unit or receiver and produces a reference signal with a period of 20 ms and width of 1.5 ms, then compares the acquired DC bias voltage to the voltage of the potentiometer and obtain the voltage difference output. When the motor speed is constant, the potentiometer is driven to rotate through the cascade reduction gear, which leads that the voltage difference is 0, and the motor stops rotating. Generally, the angle range of servo rotation is 0° — 180 °“ ©

Image 4: Dependence the angle on time delay

So, consider written below, we should:

• Write the logic one (HIGH) to pin;
• Count pulse width which is depended on angle we would like to rotate. For it we have in arguments angle value and we know minimum range value of pulse width which is 500 ms. You can also notice “the magic number” 11 in the code fragment below. We have a table above in Image 4 with dependence an angle value to delay time, so from it we can receive linear equitation like: Time = k * angle + minimum_range where k is 11 here;
• Set the delay on this value in microseconds;
• Then send the logic zero (LOW) to pin;
• Set the delay again which is meant to be zero voltage difference leading to stop the servo motor. Here we have a circle in 20 ms and divide pulse width on 1000;

We start with defining pin:

#define servoPin 9

Then we have a function for rotating Servo. We are sending two impulses to servo pin and then wait for pulse width which is depended on angle value:

void procedure(int myangle) {
   min_pulsewidthe_range = 500;
   pulsewidth = myangle * 11 + min_pulsewidthe_range;  //calculate the value of pulse    width
  digitalWrite(servoPin,HIGH);
  delayMicroseconds(pulsewidth);   //The duration of high level is pulse width
  digitalWrite(servoPin,LOW);
  delay((20 - pulsewidth / 1000));  // the cycle is 20ms, the low level last for the rest of time
}

Modules for moving.

Here we are working with pins for direction and PWM controls both for left and right wheels.

#define ML_Ctrl 13  //define the direction control pin of left motor
#define ML_PWM 11   //define the PWM control pin of left motor
#define MR_Ctrl 12  //define direction control pin of right motor
#define MR_PWM 3   // define the PWM control pin of right motor

In our case we are using PWM for speed control and direct control for controlling the direction of wheels (they are moving back or forward). I wrote the 5 functions for moving front, back, right, left and stop.

Here is an example of function moving front:

void go(){
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,constant_speed);
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,constant_speed);
}

If we want to turn left and right, we should move the wheels in opposite direction.

void turn_left() {
  digitalWrite(ML_Ctrl,HIGH);
  analogWrite(ML_PWM,constant_speed);
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,constant_speed);
}

So, we are done now with assembling modules part what we need for our primitive autonomy. As we are using Arduino and have very low possibility to save some data in memory, we are using imperative paradigm instead of object-oriented. So my task here is to write instructions for robot properly, so I can insert them in loop.

My first list was like that:

• To get distance to obstacles from left, right and forward;
• Then go forward if the distance is not less then definite number (stop_distance value). I chose 10 cm;
• If it has obstacles somewhere, it stops and it is trying to turn left or right;
• If it could not turn left or right it returned the 1 code;

So it seems logical, but I forgot several things:

• I did not use delays first time. So the servo was rotating too fast and also vehicle could not stop in time;
• If the work of sensors + motor wheels are clear for you enough, you can see that while we are understanding we have an obstacle, we will go forward for a while. So during the first experiments vehicle stopped right before the obstacle and could not turn somewhere;
• To decide the problem above I tried two ways: to increase the distance and to decrease the value for speed control. Unfortunately, the decreasing the speed was not effective, because in turn_right() and turn_left() it stopped working. The small speed value was not enough to move wheels. Increasing the distance helped a little;
• It was not clear how far should I turn left, so first I used the circle from 1 to 1000 just to watch whether it is able to turn left / right;

Here is a video of first tries. My cat was shocked also:)

first experiment with primitive autonomous vehicle

So after some debugging I:

• Added delays just to have an opportunity to watch the action order;
• Added moving back from obstacles. It allowed me not to guess with how much I should turn left or right. You are moving to obstacle and then move back, then trying to turn and then move back again until you would be able to move around obstacle;

So here is result, the vehicle is moving more predictable and can move around obstacles.

Here is more interesting video: it moved under the table where lots of obstacles, but still could handle it.

final experiment 2

Here is how I developed the loop() in Arduino finally:

void loop()
{ 
  procedure(straight); //90 degrees
  long distance = get_distance();
  delay(500);
  procedure(right_angle); // 135 degrees
  long right_distance = get_distance();
  delay(500);
  procedure(left_angle); // 45 degrees
  long left_distance = get_distance();
  delay(500);
  
  if (distance > stop_distance && left_distance > stop_distance && right_distance > stop_distance) {
     go();
     delay(500);
  }
else {
    stop_move();
delay(900);
    go_back_from_obstacle(); //moving back until straight distance is more than stop_distance value
    
    if (try_left()){
      if (try_right()) {
        go_back_from_obstacle();
      }
    }
    delay(500);
  }
}

That’s all for today, I hope you had some fun:)

Next time I will tell you how I set Raspberry Pi instead of Keystone Shield and built more smart system of autonomy robot with CV and kind of SLAM on it.