Buy Aruino   Buy Arduino Uno on AliExpress

Motion control of the robot with one joystick

This article will discuss the control algorithm of a 4-wheeled robot using one joystick. Its integral part will be the speed archiving algorithm.

Operating principle

To begin, consider the principle of movement of our robot (a little known theory to you). Its chassis consists of 4 gear motors (one for each wheel). Therefore, the movement is as follows:

Forward. All 4 engines rotate forward with equal speed.

Backward. All 4 engines rotate backward with equal speed.


Slight right turn. The right engines rotate slower than the left. But all 4 rotate in one direction.

Slight left turn. Left engines rotate more slowly than the right engines. But all 4 rotate in one direction.

Sharp right turn. Left engines rotate forward, right backward. If all engines rotate at the same speed, the robot will turn in place.

Sharp left turn. The right engines rotate forward, the left engines rotate backward. If all 4 rotate at the same speed, the robot will turn in place.

Thus, in order to control the movement of the robot, that is, the supply of a control signal to the engine driver (you can consider it as a direct connection, imagining that Arduino is capable of sending a large current from its pins) we need to know the direction of rotation and the speed module for right and left motors. We will store them in the corresponding speed_right and speed_left variables.

Connection diagram

To transfer data from the console to the robot, I use the NRF24L01 radio modules. They pass an array of values ​​each time. The less weight of this array, the faster the signal will be transmitted and the lower the data transfer rate we can assign to the module, which will reduce its power consumption. Therefore, the data from the joystick control will be recorded in 2 cells of the byte array.

Now consider how to read the data from the joystick on the remote. We connect the VCC module with 5V Arduino, GND with GND, the potentiometer of the horizontal axis with A0, vertical with A1, the button is not connected, since we will not use it. The diagram is shown below. The type of module may differ, but the pins will be the same (except for cases with a separate supply of VCC and GND to each potentiometer and button).

Considering the data from the joystick with the functions analogRead(A0) (you can replace A0 with horizontalSticPin) and analogRead(A1) (you can replace A1 with verticalSticPin), we get the following ranges of values. Forward 0 on the vertical axis, back 1023 on the vertical axis, a sharp turn to the right 0 on the horizontal axis, a sharp turn to the left 1023 on the horizontal axis. The values between them will give gradations of turns: a position of 0,0 will give a smooth right turn forward, and a value of 1023,1023 will give a smooth left turn back.

Algorithm

Based on the above, we compose an algorithm that converts the readings of the horizontal and vertical axes of the joystick into two “vertical” axes of the speed of the engines.

Here is its principle:

  1. Enter the following constants:
    - verticalSticCenter - values ​​corresponding to the center position of the joystick in the vertical axis (the average speed of the robot is 0);
    - horizontalSticPin - values ​​corresponding to the center position of the joystick in the horizontal axis (the rate of turn is 0)
    - k - coefficient of deviation, from the central positions, allows to exclude the influence of "noise" on the readings in the full stop position of the robot)
  2. Read the readings of the vertical axis of the potentiometer in the variable speed, which stores the value of the average speed. The read values ​​need to be transferred to the byte type range, dividing by 4 (you can use the map function), in this case, the average speed will vary from 255 to 0. Then subtract the value from the verticalSticCenter, now the average speed changes from 128 to -128. But for convenience of calculations, I would like to see changes in speed in the range from -128 to 128 (then the central position will give 0, which is very important), for this we divide the readings by one.
  3. We look if the values ​​of relative speed differ from 0 by less than k (the joystick is in the center position along the vertical axis), then we equate it to 0.
  4. We read the values ​​of the horizontal axis into the variable horizontal_value, also leading to the byte range, we will call them the speed of rotation.
  5. If the values ​​of the rotation speed deviated by more than k from the center to the right, then the speed of the left engine is equal to the sum of the average speed and the magnitude of the deviation in angular speed from the conditional 0 (subtract the values ​​of the speed of rotation from the values ​​of its center), and the speed of the right to their difference .
  6. If the values ​​of the rotation speed deviated by more than k from the center to the left, then the speed of the left engine is equal to the difference between the average speed and the deviation from the conditional center (from the speed of rotation we subtract the values ​​of conditional 0), and the right to their sum.
  7. Failure to comply with the conditions of paragraphs 5 and 6 indicates that the values ​​of the speed of rotation lie within the center (conditional 0), in this case we equate the speeds of both engines to the average speed.
  8. Now it is necessary to limit the obtained values ​​of the speeds so that they would not go beyond the maximum value of the byte type to prevent overflow of variables in paragraph 9. Just compare the values ​​with the constant declared boundary, and if they are larger, we equate them to it.
  9. Now the speeds of the engines are in the range from -128 to 128, but we want to store them in an array of the byte type, which cannot store negative numbers, so we will “encrypt them”:
    - 0 data array element will store the speed of the left engine, and 1 speed of the second one.
    - If the speed is negative, we put its module in data, and if positive, we add half the range of values ​​of type byte to it. Thus, values ​​from 0 to 128 store the rotational speed backward, and values ​​from 128 to 255 forward.

Project codes

Below you can download or copy the necessary sketches for this lesson. Next, I will explain all the main points of project programming. Also, before starting, you need to download and install the necessary libraries:

Now go directly to the project code.

#include <SPI.h> // we connect library for work with the serial peripheral interface
#include <RF24.h> // connect the library to work with the module NRF24L01

#define horizontalSticPin A0  // pin on which the vertical axis of the joystick is connected
#define verticalSticPin A1    // pin of the horizontal axis of the joystick
#define horizontalSticCenter 130 // the value "0" on the horizontal axis in the above values
#define verticalSticCenter 125 // the value "0" on the vertical axis in the above values
#define k 3                 // coefficient of deviation from the center
#define max_speed 127         // Maximum value of speed. It is used for coding of speed.
#define min_speed -127     // minimum value of speed. It is used for coding of speed

byte data[2];  // an array that is passed to the robot. Stores "encrypted" engine speeds
int speed_left, speed_right;   // variables stored in the speed calculations of the left and right engines

int speed, horizontal_value;  // variables that store average speed and turn speed

const uint32_t pipe1 = 123456789; // channel address of this communication module

RF24 radio(9, 10); // pins connection with the module. You can use any (ports 15-19 CSN CE MOSI MISO SCK)

/*-------------------- encodings function of values of speed ---------------*/

void getValue() {
  speed = -1 * (analogRead(verticalSticPin) / 4 - verticalSticCenter); // 2.
  if (speed < k && speed > -1 * k) speed = 0;                          // 3.
  horizontal_value = analogRead(horizontalSticPin) / 4;                // 4.
  if (horizontal_value < horizontalSticCenter - k) {                   // 5.
    speed_left = speed + (horizontalSticCenter - horizontal_value);
    speed_right = speed - (horizontalSticCenter - horizontal_value);
  }
  else if (horizontal_value > horizontalSticCenter + k) {              // 6.
    speed_left = speed - (horizontal_value - horizontalSticCenter);
    speed_right = speed + (horizontal_value - horizontalSticCenter);
  } else {                                                             // 7.
    speed_left = speed;
    speed_right = speed;
  }
  // 8.
  if (speed_left > max_speed) speed_left = max_speed;
  if (speed_right > max_speed) speed_right = max_speed;
  if (speed_left < min_speed) speed_left = min_speed;
  if (speed_right < min_speed) speed_right = min_speed;
  // 9.
  if (speed_left <= 0) data[0] = byte(-1 * speed_left);
  else if (speed_left > 0) data[0] = byte(128 + speed_left);
  if (speed_right <= 0)data[1] = byte(-1 * speed_right);
  else if (speed_right > 0) data[1] = byte(128 + speed_right);
}

/*--------- function of sending values to the communication module -----------*/

void sendData() {
  radio.write(&data, sizeof(data)); 
}

/*-------- фfunction of data output in the terminal ------------------*/

void printData() {
  Serial.print(speed); // average speed output
  Serial.print("  "); // output dividing value of a space
  Serial.print(horizontal_value); // turning speed output
  Serial.print("  ");
  Serial.print(data[0]); // left engine speed output
  Serial.print("  ");
  Serial.print(data[1]); // right motor speed output
  Serial.println(); // line feed
}

void setup() {
  Serial.begin(9600);   // Serial port (terminal) launch at 9600 speed
  radio.begin();                // launch of communication channel with radio module
  radio.setDataRate(RF24_2MBPS); // set the baud rate of RF24_1MBPS or RF24_2MBPS
  radio.setCRCLength(RF24_CRC_8); // setting the length of the checksum 8-bit or 16-bit
  radio.setPALevel(RF24_PA_MAX); // setting the power level of the RF24_PA_MIN amplifier (minimum), RF24_PA_LOW (low), RF24_PA_HIGH (high) and RF24_PA_MAX (maximum)
  // Corresponds to levels:    -18dBm,      -12dBm,      -6dBM,           0dBm
  radio.setChannel(120);         // channel setup
  radio.setAutoAck(false);       // auto answer off
  radio.powerUp();               // enable or reduce powerDown consumption - powerUp
  radio.openWritingPipe(pipe1);   // open channel to send data
}

void loop() {
  getValue();  // read and process values
  sendData();  // send values
  printData(); // output values to the terminal
}
#include <SPI.h>    // connection library to work with a serial peripheral interface
#include <RF24.h>   // connection library to work with a serial peripheral interface
#include <Servo.h>

RF24 radio(7, 8);   // pin assignment of communication with the module
const uint32_t pipe = 123456789; // setting the address of the radio module in the channel
byte data[2];  // array of values received from the radio module

// pins to which the engine driver is connected

#define M1 6
#define M2 9
#define M3 3
#define M4 5

#define speed_border 128 // speed range separation boundary

/*-------------- engine control function ----------------------------------------*/

void Motor(bool directoin_left, byte speed_left, bool direction_right, byte speed_right) {
  //(left direction, left speed, right direction, right speed) 1 is forward, 0 is back
  if (directoin_left == 1) {
    digitalWrite(M2, 0); // set the direction of movement
    analogWrite(M1, speed_left); // set the rotation speed
  }
  if (directoin_left == 0) {
    digitalWrite(M1, 0);// set the direction of movement
    analogWrite(M2, speed_left); // set the rotation speed
  }
  if (direction_right == 1) {
    digitalWrite(M4, 0); // set the direction of movement
    analogWrite(M3, speed_right); // set the rotation speed
  }
  if (direction_right == 0) {
    digitalWrite(M3, 0);// set the direction of movement
    analogWrite(M4, speed_right); // set the rotation speed
  }
}

/*-------------------------- function "decrypt" the accepted values ---------------------------------*/

void drive() {
  if (data[0] <= speed_border) {            
    if (data[1] <= speed_border) Motor(0, data[0], 0, data[1]);
    else if (data[1] > speed_border) Motor(0, data[0], 1, data[1] - speed_border);
  } else if (data[0] > speed_border) {
    if (data[1] <= speed_border) Motor(1, data[0] - speed_border, 0, data[1]);
    else if (data[1] > speed_border) Motor(1, data[0] - speed_border, 1, data[1] - speed_border);
  }
}

/*-------------------------- function "decrypt" the accepted values with multiplication ---------------------*/

void driveHigh() {
  if (data[0] <= speed_border) { 
    if (data[1] <= speed_border) Motor(0, data[0]*2, 0, data[1]*2);
    else if (data[1] > speed_border) Motor(0, data[0]*2, 1, (data[1] - speed_border)*2);
  } else if (data[0] > speed_border) {
    if (data[1] <= speed_border) Motor(1, (data[0] - speed_border)*2, 0, data[1]*2);
    else if (data[1] > speed_border) Motor(1, (data[0] - speed_border)*2, 1, (data[1] - speed_border)*2);
  }
}

/*----- function of receiving data from the radio module ---------*/

void getData() {
  if (radio.available()){  // if new data is available
    radio.read(&data, sizeof(data)); // read them into the data array
  }
}

/*---- output function to terminal ----------*/

void printData() {
  Serial.print(data[0]); 
  Serial.print("  ");
  Serial.print(data[1]);
  Serial.print("  ");
  Serial.println();
}

void setup(){
  Serial.begin(9600);       // launch of the terminal (serial port) with a speed of 9600
  radio.begin();            // launch of the radio module
  radio.setDataRate(RF24_2MBPS); // set the baud rate of RF24_1MBPS or RF24_2MBPS
  radio.setCRCLength(RF24_CRC_8); // setting the length of the checksum 8-bit or 16-bit
  radio.setPALevel(RF24_PA_MAX); // setting the power level of the RF24_PA_MIN amplifier (minimum), RF24_PA_LOW (low), RF24_PA_HIGH (high) and RF24_PA_MAX (maximum)
  // Corresponds to levels:    -18dBm,      -12dBm,      -6dBM,           0dBm
  radio.setChannel(120);         // channel setup
  radio.setAutoAck(false);       // auto answer off
  radio.powerUp();               // enable or reduce powerDown consumption - powerUp
  radio.openReadingPipe(1, pipe); // channel opening on reception
  radio.startListening(); // start receiving data
  // assigning driver pins to the output
  pinMode(M1, OUTPUT); 
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
}

void loop(){
  getData();    // read values
  drive();      // processing values
  printData();  // output values to the terminal
}

We write this algorithm in the getValue function. In order not to overload the code in the comments, the numbers above the written points, and not their text, stand.

Now you need to transfer the values to the robot through the radio module and it would be nice to bring them to the terminal. Let's use the sendData and printData functions:

It remains only to call the functions in the loop:

Go to the robot.

Here we will need to write an algorithm for “decrypting values”. For his work need the function of engine management.

At the beginning of the program, we will declare pins for which the engine driver is connected (if you use one). Constantly set the speed limit (the middle of the maximum value of the type byte). And write the engine control function.

It takes 4 parameters to the input:

  • the direction of the left engine,
  • speed,
  • the direction of the right engine,
  • speed.

1 - forward rotation, 0 - backward. The speed is in the range from 0 to 255. As is known, the current flows from high potential to low, so if you send a high signal to the first pin and a low signal to the second pin, the current will go from the first to the second and the motor will begin to rotate in the appropriate direction.

Instead of a constant high signal, you can send a PWM signal, then we will be able to control the rotational speed, so we’ll do it. If the received direction is 1 (forward), then we will give a low signal to the first pin, and the adopted speed to the second PWM, if the direction is 0, then vice versa. For another engine is similar. Let's write it in the Motor function.

Now you can write the algorithm of "decryption".

We check if the speed value in the data array is less than or equal to the speed_border boundary we set, then we transfer the 0 direction (backward rotation) and the speed value to the Motor function. If the value is greater than the border, then we transfer direction 1 (forward) to Motor, and subtract the border from the speed value (this will be equivalent to finding the remainder from dividing the speed by the border).

This algorithm is the same for both engines. However, we cannot transfer to the function the values ​​for the engines separately. To call a function, we must immediately know the directions and speeds for both engines. Therefore, it is necessary to check the condition first for the left one, and if it is true, then for the right one and, if it is also true, call the Motor function. Let's write this in the drive function. Now the speed is in the range from 0 to 128, and the PWM signal has a range of 0-255, which coincides with the range of byte (that is why this data type was used, well, it is also the smallest of the standard ones after bool, which did not suit us). Therefore, the speed needs to be multiplied by 2. I suggest multiplying the speed values ​​by 2 in the driveHigh function, and transfer them to the drive unchanged, then it can be useful.

Before processing the values ​​of the speeds, they must also be obtained, for this, we will do the getData function.

Similar to the sketch for the console, we output the speed data to the terminal using the printData function. Data output to the terminal will be useful when debugging the program and therefore is not mandatory.

It remains only to call all the functions in the main loop.

Finally, the prospect of optimization. Each motor has minimum values ​​of current and voltage at which it can work, that is, the robot engine will rotate only when the PWM values ​​exceed a certain threshold of values, let's call it the “dead” interval. This threshold is most dependent on the power of the power source and the engine itself, it usually ranges from 30 to 70.

Since the engine does not work in between these values, you can use them to encode something else. At the smallest threshold of 30, we get 30 positions encoded from each engine, when combining positions from 2 engines already 900! If your robot does not require instant response speed and the number of packets sent is large (> 300-500), then you can spend up to 20% of packets to send additional codes at dead intervals, and you will not notice.

The more packets a robot receives in 1 second, the greater the number of them in a percentage that can be spent on sending codes, but the more they are lost, the greater the percentage it is worth leaving for speed packets if you want a timely response. To catch these packages, you just need to check that the speed values ​​are lower than the limit you set, and how to process them further (through mathematical calculations, if or switch-case constructions) and why you should think about it yourself.

Using the idea of ​​splitting large ranges into intervals in this article, you can encode both speeds for engines and some other values ​​in one int type variable, but you need to remember that in one package we can send as many codes as it contained array elements, that is when using an array of 2 elements, we received both speeds for 1 package and using one variable, we would have to use 2 packages already given.

Ардуино+