Skip to content

- Circuits | Embedded programming

In this part I put into practice Embedded programming - Inputs, Outputs & Communication.

I divided it into 3 parts in order to make easier a replication of each aspect of the project.

Wheeled robot | Output devices

Softwares and hardwares

I programmed an Arduino UNO board in C++ with the Arduino IDE.

Components:

Quantity Description
1 x Arduino UNO board
1 x Adafruit Motor Shield V2.0
3 x 5V DC Motor
2 x UR18650 Li-Ion Battery 2600mAh 3.7V
2 x Battery holder with mini-USB charging module
1 x ON/OFF switch
a few Rigid cables

Circuit

I connected the motors to the Motor Shield which is already tagged with the motor connections:

Physical circuit:

RobotCircuit

Coding

  • First of all, I installed the Adafruit Motor Shield V2 Library into the Arduino IDE.

  • Add libraries for Motor Shield and Math functions:

    1
    2
    3
    #include <Wire.h>
    #include <Adafruit_MotorShield.h>
    #include <math.h> 
    

  • Create Motor shield object:

    1
    Adafruit_MotorShield AFMS = Adafruit_MotorShield();
    

  • Declare motors M1, M2 and M3 on their respective ports:

    1
    2
    3
    Adafruit_DCMotor *Motor1 = AFMS.getMotor(1);
    Adafruit_DCMotor *Motor2 = AFMS.getMotor(2);
    Adafruit_DCMotor *Motor3 = AFMS.getMotor(3);
    

  • Declare angle in rads of each wheel with respect to Y-axis of the robot (correspond to 0º, 120º and 240º), this part is used a bit later:

    1
    const double AngWheels[3] = {0, 2.094, 4.189}; 
    

  • In the setup we initialize the Motor Shield ports previously declared:

    1
    AFMS.begin(); //Initialize Motor shield ports
    

  • The loop:

    • First, there are two main commands for the Motor shield:
      • _MotorName_->run(_direction of rotation_); where MotorName is the name we used to declare them, and direction of rotation can be FORWARD or BACKWARD, it depends on the desired rotation or calculated by the signal of the speed.
      • _MotorName_->setSpeed(_speed_); where speed needs is the desired/calculated one from 0 to 255, by default it takes it as absolute values. To stop the motor, we can use _MotorName_->run(RELEASE) or _MotorName_->setSpeed(_speed_).
    • Logic explanation:
      • If there’s a radio signal, the code starts.
      • Read values from the Transmitter.
      • There are two possibilities: read Joystick_1 or Joystick_2:
        • If Joystick_2 is pushed, check what position it is:
          • Right: make a Clockwise rotation.
          • Left: make a Counter-Clockwise rotation.
        • If Joystick_1 is pushed, check what position it is:
          • Up: Move forward.
          • Down: Move backward.
          • Right: Move to the right.
          • Left: Move to the left.
          • For all of the possible positions, we got to calculate the speed for each wheel by using the holonomic drive function.
        • If we release them or do not push them, then stop the motors.
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
int Values[3];
if (radio.available()) {
  //[here I receive the values from the transmitter]

    if (Values[2]<=-5 || Values[2]>=5) //Detect if the Joystick_2 is activated
    { 
      int speed = map(Values[2],-100,100,-60,60); //Read the percentage values and convert them to speed values for the motors

      //Give direction of rotation to each wheel, depending on direction we push the Joystick
      if (Values[2]<-4) 
      { Motor1->run(FORWARD);
        Motor2->run(FORWARD);
        Motor3->run(FORWARD);}
      if (Values[2]>4)
      { Motor1->run(BACKWARD);
        Motor2->run(BACKWARD);
        Motor3->run(BACKWARD);}

      //Write speed in each motor
      Motor1->setSpeed(speed);
      Motor2->setSpeed(speed);
      Motor3->setSpeed(speed);

      } else if ((Values[0]<=-4 || Values[0]>=4) || (Values[1]<=-4 || Values[1]>=4))
      {
          //Calculate angle of movement of robot (and Read the percentage values and convert them to speed values for the motors)
          double Vx=map(Values[1],-100,100,-230,230);
          double Vy=map(Values[0],-100,100,-230,230);
          double theta = atan2(Vx, Vy);

          //Calculate speed for every wheel, using the formula holonomic drive for 3W omnidirectional robots:
          double SpeedM1 = ((cos(theta)*cos(AngWheels[0]) - sin(theta)*sin(theta + AngWheels[0]))*Vx) - ((sin(theta)*cos(theta + AngWheels[0]) + cos(theta)*sin(theta + AngWheels[0]))*Vy);
          double SpeedM2 = ((cos(theta)*cos(AngWheels[1]) - sin(theta)*sin(theta + AngWheels[1]))*Vx) - ((sin(theta)*cos(theta + AngWheels[1]) + cos(theta)*sin(theta + AngWheels[1]))*Vy);
          double SpeedM3 = ((cos(theta)*cos(AngWheels[2]) - sin(theta)*sin(theta + AngWheels[2]))*Vx) - ((sin(theta)*cos(theta + AngWheels[2]) + cos(theta)*sin(theta + AngWheels[2]))*Vy);

          //Give direction of rotation to each wheel, depending on the signal of their speed
          if (SpeedM1<0){ Motor1->run(BACKWARD);}
          else if (SpeedM1>0){ Motor1->run(FORWARD);}
          if (SpeedM2<0){ Motor2->run(BACKWARD);}
          else if (SpeedM2>0){ Motor2->run(FORWARD);}
          if (SpeedM3<0){ Motor3->run(BACKWARD);}
          else if (SpeedM3>0){ Motor3->run(FORWARD);}

          //Write speed in each motor
          Motor1->setSpeed(SpeedM1);
          Motor2->setSpeed(SpeedM2);
          Motor3->setSpeed(SpeedM3);
      } else { //Deactivate motors
        Motor1->setSpeed(0);
        Motor2->setSpeed(0);
        Motor3->setSpeed(0);}     
  }

Full code:

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
//Libraries for Communication part (nRF24)
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(7, 8); //Create radio object with and CE & CSN pins
const byte address[6] = "00111"; //Receiver Communication address

//Libraries for Motor shield
#include <Wire.h>
#include <Adafruit_MotorShield.h>

//Create Motor shield object with the default I2C address 
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

//Declare motors (M1, M2 and M3) on their respective ports.
Adafruit_DCMotor *Motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *Motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *Motor3 = AFMS.getMotor(3);

const double AngWheels[3] = {0, 2.094, 4.189}; //Angle in rads of each wheel with respect to Y-axis of the robot (correspond to 0º, 120º and 240º)

//Library for math functions
#include <math.h> 

void setup() {
  Serial.begin(9600); //Begin Serial communication at 9600 bauds

  radio.begin(); //Initialize radio object
  radio.setPALevel(RF24_PA_HIGH); //Set radio range
  radio.openReadingPipe(0, address); //Set address to transmitter
  radio.startListening(); //Set module as receiver

  AFMS.begin(); //Initialize Motor shield ports
}

void loop() {
  int Values[3];

  if (radio.available()) {  
    radio.read(&Values, sizeof(Values)); //Read array of Values from Transmitter

      if (Values[2]<=-5 || Values[2]>=5) //Detect if the Joystick_2 is activated
      { int speed = map(Values[2],-100,100,-60,60); //Read the percentage values and convert them to speed values for the motors

        //Give direction of rotation to each wheel, depending on direction we push the Joystick
        if (Values[2]<-4) 
        { Motor1->run(FORWARD);
          Motor2->run(FORWARD);
          Motor3->run(FORWARD);
          Serial.println("CCW rotation");}
        if (Values[2]>4)
        { Motor1->run(BACKWARD);
          Motor2->run(BACKWARD);
          Motor3->run(BACKWARD);
          Serial.println("CC rotation");}

        //Write speed in each motor
        Motor1->setSpeed(speed);
        Motor2->setSpeed(speed);
        Motor3->setSpeed(speed);
        } else if ((Values[0]<=-4 || Values[0]>=4) || (Values[1]<=-4 || Values[1]>=4))
        {
            //Calculate angle of movement of robot (and Read the percentaje values and convert them to speed values for the motors)
            double Vx=map(Values[1],-100,100,-230,230);
            double Vy=map(Values[0],-100,100,-230,230);
            double theta = atan2(Vx, Vy);

            //Calculate speed for every wheel, using the formula holonomic drive for 3W omnidirectional robots:
            double SpeedM1 = ((cos(theta)*cos(AngWheels[0]) - sin(theta)*sin(theta + AngWheels[0]))*Vx) - ((sin(theta)*cos(theta + AngWheels[0]) + cos(theta)*sin(theta + AngWheels[0]))*Vy);
            double SpeedM2 = ((cos(theta)*cos(AngWheels[1]) - sin(theta)*sin(theta + AngWheels[1]))*Vx) - ((sin(theta)*cos(theta + AngWheels[1]) + cos(theta)*sin(theta + AngWheels[1]))*Vy);
            double SpeedM3 = ((cos(theta)*cos(AngWheels[2]) - sin(theta)*sin(theta + AngWheels[2]))*Vx) - ((sin(theta)*cos(theta + AngWheels[2]) + cos(theta)*sin(theta + AngWheels[2]))*Vy);

            //Give direction of rotation to each wheel, depending on the signal of their speed
            if (SpeedM1<0){ Motor1->run(BACKWARD);}
            else if (SpeedM1>0){ Motor1->run(FORWARD);}
            if (SpeedM2<0){ Motor2->run(BACKWARD);}
            else if (SpeedM2>0){ Motor2->run(FORWARD);}
            if (SpeedM3<0){ Motor3->run(BACKWARD);}
            else if (SpeedM3>0){ Motor3->run(FORWARD);}

            //Write speed in each motor
            Motor1->setSpeed(SpeedM1);
            Motor2->setSpeed(SpeedM2);
            Motor3->setSpeed(SpeedM3);

            if (Values[1]<-4) {Serial.println("Moving to the Left");}
            if (Values[1]>4) {Serial.println("Moving to the Right");}
            if (Values[0]<-4) {Serial.println("Moving Backward");}
            if (Values[0]>4) {Serial.println("Moving Forward");}
            Serial.print("M1: ");
            Serial.println(SpeedM1);
            Serial.print("M2: ");
            Serial.println(SpeedM2);
            Serial.print("M3: ");
            Serial.println(SpeedM3);
            Serial.println("");
        } else { //Deactivate motors
          Motor1->setSpeed(0);
          Motor2->setSpeed(0);
          Motor3->setSpeed(0);}     
   }
}

Download files

- ODRobot Code: ReceiverOfficial.ino


Remote control | Input devices

I used two joysticks to control horizontal movements (forward, backward, left and right) and rotations (Clockwise and Counter-Clockwise).

Softwares and hardwares

I programmed a Joy-IT Nano-V3 board (CH340G USB-TTL converter) in C++ with the Arduino IDE.

Components:

Quantity Description
1 x Joy-IT Nano-V3 board
2 x Grove - Thumb Joystick v1.1
1 x UR18650 Li-Ion Battery 2600mAh 3.7V
1 x Battery holder with mini-USB charging module
1 x Prototyping PCB
1 x ON/OFF switch
a few Rigid cables

Circuit

Pin configuration:

Joystick 1 (for the horizontal movements):

Pin Connection
Vcc 5V
GND GND
x Joy-IT Pin A0
y Joy-IT Pin A1

Joystick 2 (for the rotations):

Pin Connection
Vcc 5V
GND GND
x Joy-IT Pin A2
y Joy-IT Pin A3

Joystick 2 x-pin was connected but not used, then the remote control could be re-used in another project such as a basic drone.

Physical circuit:

RemoteControlCircuit

Coding

  • First of all, we got to download and install the CH340G driver due to it’s not in the IDE Arduino by default; without it the computer couldn’t recognize the board. I founded it here.

  • Get the Joysticks readings:

    1
    2
    3
    4
    5
    6
    7
    8
    int Values[3]; //
    
    //Read values from Joysticks and convert them to percentage
    Values[0] = map(analogRead(A0),0,1023,100,-100); //Forward and Backward movement from Joystick_1
    Values[1] = map(analogRead(A1),0,1023,-100,100); //Left and Right movement from Joystick_1
    Values[2] = map(analogRead(A3),0,1023,-100,100); //Rotation movement from Joystick_2
    
    //[here I sent the Values to the receiver]
    

Full code:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//Libraries for Communication part (nRF24)
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(7, 8); //Create radio object with and CE & CSN pins
const byte address[6] = "00111"; //Transmitter communication address

void setup() {
  radio.begin(); //Initialize radio object
  radio.setPALevel(RF24_PA_HIGH); //Set radio range
  radio.openWritingPipe(address); //Set address to receiver
  radio.stopListening(); //Set module as transmitter
}

void loop() {
  int Values[3]; //

  //Read values from Joysticks and convert them to percentage
  Values[0] = map(analogRead(A0),0,1023,100,-100); //Forward and Backward movement from Joystick_1
  Values[1] = map(analogRead(A1),0,1023,-100,100); //Left and Right movement from Joystick_1
  Values[2] = map(analogRead(A3),0,1023,-100,100); //Rotation movement from Joystick_2

  radio.write(&Values, sizeof(Values)); //Send to Receiver an array that containing the Values

  delay(100); //Send values every 0.1 seconds
}

Download files

- Remote control Code: TransmitterOfficial.ino


Communication

This is the explanation of the communication part, which is already contained on the codes of Wheeled (which I will refer to as “Receiver”) robot and Remote control (which I will refer to as “Transmitter”).

I learned how to do it with the nRF24L01 tutorial of How to Mechatronics.

Components: 2 x nRF24L01 wireless transceiver module, jumper cables

Pin configuration

Pins 11, 12 and 13 are the SPI connections on both of the boards.

- Transmitter:

Pin Connection
Vcc 3.3V
CE Joy-IT Pin 7
CSN Joy-IT Pin 8
SCK Joy-IT Pin 13
MOSI Joy-IT Pin 11
MISO Joy-IT Pin 12
GND GND

- Receiver:

Pin Connection
Vcc 3.3V
CE Arduino Pin 7
CSN Arduino Pin 8
SCK Arduino Pin 13
MOSI Arduino Pin 11
MISO Arduino Pin 12
GND GND

Coding

  • First of all we got to install the RF24 (by TMRh20).

  • Import the SPI and RF24 libraries:

    1
    2
    3
    #include <SPI.h>
    #include <nRF24L01.h>
    #include <RF24.h>
    

  • Create RF24 object and specify the pins for CSN and CE connection:

    1
    RF24 radio(7, 8);
    

  • Create an address for the communication:

    1
    const byte address[6] = "00111";
    

  • In the setup we initialize radio object and set the PowerAmplifier level:

    1
    2
    radio.begin();
    radio.setPALevel(RF24_PA_MIN);
    

  • In the setup of the Transmitter, we set address to receiver, and set the module as transmitter:

    1
    2
    radio.openWritingPipe(address);
    radio.stopListening();
    

  • In the setup of the Receiver, we set address to transmitter, and set the module as receiver:

    1
    2
    radio.openReadingPipe(0, address);
    radio.startListening();
    

  • In the loop of the Transmitter, we just need to send the message:

    1
    2
    //[here I declared "int Values[3];" and got the reads from the joysticks]
    radio.write(&Values, sizeof(Values));
    

  • In the loop of the Receiver, we just need to read the message if there is a radio connection:

    1
    2
    3
    4
    //[here I declared "int Values[3];"]
    if (radio.available()) {
    radio.read(&Values, sizeof(Values));
    }
    

Download files

- nRF24L01 Transceiver Datasheet: nRF24L01_Datasheet.pdf
- nRF24L01 Transmitter example code: RF_Transmitter.ino
- nRF24L01 Receiver example code: RF_Receiver.ino


Last update: February 12, 2021