Week 13: Networking and communications

Last week in Output Devices I designed a two-channel motor controller based on the L298N driver with an integrated ATtiny1614 (I call it Fab Motor Controller), which has exposed I2C communication pins (TWI) which I plan to use to communicate the ATtiny1614 integrated in the controller with another external ATtiny1614.

The idea is to control the two channels of the controller through I2C sending commands containing the output that you want to enable/disable and the speed at which the motors will run.

At first I had thought to make a simple MASTER/SECONDARY program, upload them to both microcontrollers, make the physical connection, test them and that's it, but I wanted to do something more interesting since I was very fond of the Fab Motor Controller so I decided to make a library for the MASTER to use and a simple firmware for the Fab Motor Controller to work using I2C.

Library design

First start by defining what basic functions the library should perform for now at least for this first Hardware and Software version the library will be in charge of establishing the I2C connection with the Fab Motor Controller, plus two functions to activate and deactivate a given output.

  • Establish I2C communication
  • Activate a given output at a given speed
  • Deactivate a given output

I have some functions in mind that I would like to implement in the future as a blocking function and another one to read the status of the outputs.

Now that I am clear about the functions I want to implement it is time to write the header file that contains the definitions and methods I am going to use.

The file looks like this.

                           
                              /*
                               * FabMotorController.h v0.1
                               * A library to control through I2C the FabMotorController, 
                               * a hardware based on the ATTINY1614 and the L298N driver.
                               * 
                               * Author: Harley Lara
                               * Create: 01 May 2021
                               * License: (CC BY-SA 4.0) Attribution-ShareAlike 4.0 International
                               * 
                               * This work may be reproduced, modified, distributed,
                               * performed, and displayed for any purpose, but must
                               * acknowledge this project. Copyright is retained and
                               * must be preserved. The work is provided as is; no
                               * warranty is provided, and users accept all liability.
                               * 
                               */
                              
                              #ifndef _FABMOTORCONTROLLER_
                              #define _FABMOTORCONTROLLER_
                              
                              #include 
                              
                              // Output identifiers
                              #define OUTPUT1 1
                              #define OUTPUT2 2
                              
                              class FabMotorController{
                                private:
                                  /******** Dirección de las salidas ********/
                                  // 1: Forward
                                  // 0: Backward
                                  bool _OUT1_direction = 1;
                                  bool _OUT2_direction = 1;
                                  
                                  /******** Initial speed of the motors ********/
                                  // _speed1 from 0 to 100;
                                  unsigned char _speed1 = 0;
                                  // _speed2 from 0 to 100;
                                  unsigned char _speed2 = 0;
                                
                                  // I2C Controller address
                                  unsigned char _addr = 0x04;
                                
                                public:
                                  // Initializes the I2C with a default I2C address (0x04)
                                  int begin(unsigned char addr);
                              
                                  /********  Activates the output at a given speed ********/
                                  // output: OUTPUT1, OUTPUT2
                                  // _speed: from -100 to 100, 
                                  //         where _speed greater than zero moves the motor clockwise 
                                  //         (the direction of the motor depends on the physical connection 
                                  //         of the motor, if it turns counterclockwise it is necessary to 
                                  //         reverse the connection).
                                  void run(unsigned char output, int _speed);
                              
                                  /******** Stops the given output by locking for 100ms the motor rotation and then releases the motor lock ********/
                                  // output: OUTPUT1, OUTPUT2
                                  void stop(unsigned char output);
                              
                                  /******** Stops motor rotation allowing it to run freely (no motor blocking) ********/
                                  void stopLock(unsigned char output);
                              
                                  bool isMoving(unsigned char output);
                              };
                              #endif
                           
                        

Then it was necessary to write the main file containing the instructions to be executed by the library.

The file looks like this.

                           
                              /*
                              * FabMotorController.cpp v0.1
                              * A library to control through I2C the FabMotorController, 
                              * a hardware based on the ATTINY1614 and the L298N driver.
                              * 
                              * Author: Harley Lara
                              * Create: 01 May 2021
                              * License: (CC BY-SA 4.0) Attribution-ShareAlike 4.0 International
                              * 
                              * This work may be reproduced, modified, distributed,
                              * performed, and displayed for any purpose, but must
                              * acknowledge this project. Copyright is retained and
                              * must be preserved. The work is provided as is; no
                              * warranty is provided, and users accept all liability.
                              * 
                              */
                              
                              #include "FabMotorController.h"
                              #include 
                              
                              /******** Initializes and sets the I2C address ********/
                              // addr: motor controller address (default 0x04)
                              int FabMotorController::begin(unsigned char addr){
                                  if (addr != 0x04){
                                  Serial.println("[ERROR]: Incorrect I2C address, default address 0x04.");
                                  return -1; // Incorrect I2C address
                                  }
                                  Wire.begin();
                                  delayMicroseconds(10000);
                                  this -> _addr = addr;
                                  Serial.println("[INFO]: I2C Initialization");
                                  return 0; // Initialization completed without errors
                              }
                              
                              /********  Activates the output at a given speed ********/
                              // output: OUTPUT1, OUTPUT2
                              // _speed: from -100 to 100, 
                              //         where _speed greater than zero moves the motor clockwise 
                              //         (the direction of the motor depends on the physical connection 
                              //         of the motor, if it turns counterclockwise it is necessary to 
                              //         reverse the connection).
                              void FabMotorController::run(unsigned char output, int _speed){
                                  if (output < OUTPUT1 || output > OUTPUT2){
                                  Serial.println("[ERROR]: Out of range, must be OUPUT1 or OUPUT2, or alternatively 1 or 2");
                                  return;
                                  }
                                  if (output == OUTPUT1){
                                  if (_speed >= 0){
                                      this->_OUT1_direction = 1;
                                      _speed = _speed > 100 ? 100 : _speed;
                                      this->_speed1 = map(_speed, 0, 100, 0, 255);
                                  } else if(_speed < 0){
                                      this->_OUT1_direction = 0;
                                      
                                      _speed = _speed < -100 ? 100 : -(_speed);
                                      this->_speed1 = map(_speed, 0, 100, 0, 255);
                                  }
                              
                                  // Send action OUTPUT 1
                                  Serial.println("[INFO]: Sending Command to OUPUT1");
                                  Wire.beginTransmission(this -> _addr);  // Starts I2C transmission
                                  Serial.println(_OUT1_direction);
                                  Wire.write(OUTPUT1);                          // Set the output to be activated
                                  Wire.write(this->_speed1);              // Set the speed
                                  Wire.write(this->_OUT1_direction);    // Set the direction
                                  Wire.endTransmission();                 // Transmission stops
                                  delay(4);
                                  Serial.println("[INFO]: Command sent");
                                  
                                  }else if(output == OUTPUT2){
                                      if (_speed >= 0){
                                      this -> _OUT2_direction = 1;
                                      _speed = _speed > 100 ? 100 : _speed;
                                      this -> _speed2 = map(_speed, 0, 100, 0, 255);
                                  } else if(_speed < 0){
                                      this -> _OUT2_direction = 0;
                                      _speed = _speed < -100 ? 100 : -(_speed);
                                      this->_speed2 = map(_speed, 0, 100, 0, 255);
                                  }
                              
                                  // Send action OUTPUT 2
                                  Serial.println("[INFO]: Sending Command to OUPUT2");
                                  Wire.beginTransmission(this -> _addr);  // Starts I2C transmission
                                  Wire.write(OUTPUT2);                          // Set the output to be activated
                                  Wire.write(this->_speed2);              // Set the speed
                                  Wire.write(this -> _OUT2_direction);    // Set the direction
                                  Wire.endTransmission();                 // Transmission stops
                                  delay(4);
                                  Serial.println("[INFO]: Command sent");
                                  
                                  }
                              }
                              
                              void FabMotorController::stop(unsigned char output){
                                  if (output < OUTPUT1 || output > OUTPUT2){
                                  Serial.println("[ERROR]: Out of range, must be OUPUT1 or OUPUT2, or alternatively 1 or 2");
                                  return;
                                  }
                              
                                  run(output, 0);
                              }
                           
                        

At this point the library is ready.

Firmware design for the Fab Motor Controller

The firmware for the controller basically consists of the definitions of the ATtiny1614 pins connected to the L298N driver and the control signals to send to control the outputs based on the information received by I2C.

The firmware that is loaded to the Fab Motor Controller looks like this.

                           
                              /*
                              * FabMotorController Firmware v0.1
                              * A Firmware to control through I2C the FabMotorController, 
                              * a hardware based on the ATTINY1614 and the L298N driver.
                              * 
                              * Author: Harley Lara
                              * Create: 01 May 2021
                              * License: (CC BY-SA 4.0) Attribution-ShareAlike 4.0 International
                              * 
                              * This work may be reproduced, modified, distributed,
                              * performed, and displayed for any purpose, but must
                              * acknowledge this project. Copyright is retained and
                              * must be preserved. The work is provided as is; no
                              * warranty is provided, and users accept all liability.
                              * 
                              */
                              
                              #include 
                              
                              #define CONTROLLER_ADDR 0x04
                              
                              /******* CONTROL PINs *******/
                              #define IN1 9   // OUTPUT 1 INPUT 1
                              #define IN2 8   // OUTPUT 1 INPUT 2
                              #define IN3 10  // OUTPUT 2 INPUT 1
                              #define IN4 2   // OUTPUT 2 INPUT 2
                              #define EN1 0   // 0 ENABLE 1 (PWM - SPEED CONTROL)
                              #define EN2 1   // 1 ENABLE 2 (PWM - SPEED CONTROL)
                              
                              #define RESPONSE_SIZE 12
                              String response = "executing...";
                              
                              void setup() {
                                  Wire.begin(CONTROLLER_ADDR);
                                  //Wire.onRequest(requestEvent); // TODO
                                  Wire.onReceive(receiveEvent);
                                  Serial.begin(115200);
                                  Serial.println("[INFO]: FAB MOTOR CONTROLLER - Starting ...");
                                  delay(20);
                              
                                  // PINs Mode
                                  pinMode(IN1, OUTPUT);
                                  pinMode(IN2, OUTPUT);
                                  pinMode(IN3, OUTPUT);
                                  pinMode(IN4, OUTPUT);
                                  pinMode(EN1, OUTPUT);
                                  pinMode(EN2, OUTPUT);
                              }
                              
                              unsigned char output;
                              int speed;
                              int direction;
                              
                              void receiveEvent(){
                              
                                  /******* Read data from Master *******/
                                  while (Wire.available()){
                                  output = Wire.read();
                                  speed =  Wire.read();
                                  direction = Wire.read();
                                  }
                              
                                  /******* OUTPUT 1 *******/
                                  if (output == 1){
                                  Serial.println("[DEGUB]: OUTPUT 1");
                                  // Forward direction
                                  if (direction == 1){
                                      // stop output
                                      if (speed == 0){
                                      //Serial.println("[DEGUB]: OUTPUT 1 STOP FREE");
                                      digitalWrite(IN1, LOW);
                                      digitalWrite(IN2, LOW);
                                      }
                                      // moving output
                                      else{
                                      //Serial.println("[DEGUB]: OUTPUT 1 FORWARD");
                                      analogWrite(EN1, speed);
                                      digitalWrite(IN1, HIGH);
                                      digitalWrite(IN2, LOW);
                                      }
                                  }
                                  // backward direction
                                  else if (direction == 0){
                                      //Serial.println("[DEGUB]: OUTPUT 1 STOP FREE");
                                      if (speed == 0){
                                      digitalWrite(IN1, LOW);
                                      digitalWrite(IN2, LOW);
                                      }
                                      else{
                                      //Serial.println("[DEGUB]: OUTPUT 1 BACKWARD");
                                      analogWrite(EN1, speed);
                                      digitalWrite(IN1, LOW);
                                      digitalWrite(IN2, HIGH); 
                                      }
                                  }
                                  }
                                  /******* OUTPUT 2 *******/
                                  else if (output == 2){
                                  Serial.println("[DEGUB]: OUTPUT 2");
                                  // Forward direction
                                  if (direction == 1){
                                      // stop output
                                      //Serial.println("[DEGUB]: OUTPUT 2 STOP FREE");
                                      if (speed == 0){
                                      digitalWrite(IN3, LOW);
                                      digitalWrite(IN4, LOW);
                                      }
                                      // moving output
                                      //Serial.println("[DEGUB]: OUTPUT 2 FORWARD");
                                      else{
                                      analogWrite(EN2, speed);
                                      digitalWrite(IN3, HIGH);
                                      digitalWrite(IN4, LOW);
                                      }
                                  }
                                  // backward direction
                                  else if (direction == 0){
                                      if (speed == 0){
                                      //Serial.println("[DEGUB]: OUTPUT 2 STOP FREE");
                                      digitalWrite(IN3, LOW);
                                      digitalWrite(IN4, LOW);
                                      }
                                      else{
                                      //Serial.println("[DEGUB]: OUTPUT 2 BACKWARD");
                                      analogWrite(EN2, speed);
                                      digitalWrite(IN3, LOW);
                                      digitalWrite(IN4, HIGH); 
                                      }
                                  }
                                  }
                                  
                                  /*
                                  // For degubbing
                                  Serial.println("[DEGUB]: Data received ...");
                                  Serial.print("[DEGUB]: Ouput=");
                                  Serial.println(output);
                                  Serial.print("[DEGUB]: Speed= ");
                                  Serial.println(speed);
                                  Serial.print("[DEGUB]: Direction=");
                                  Serial.println(direction);
                                  */
                              }
                              
                              void loop() {
                                  delay(50);
                              }
                           
                        

With the program loaded into the controller it is time to make the physical connections and see how everything works together.

Physical Connection

I am going to start by connecting the motors and power supply to the Fab Motor Controller.

The Fab Motor Controller has some pins that allow us to enable an internal voltage regulator that uses the input voltage of the motors to power the ATtiny1614 and we can use those 5v to power our master board.

The I2C connection is quite simple, there are two SDA AND SCL connection pins which are connected as follows.

The SDA cable should be connected to the SDA pin of the other controller and the SCL to the SCL pin.

For the MASTER board I will use a board I made at Electronic Design week which I call **MidTiny** it is a board that uses the ATtiny1614 and exposes all its pins.

Don't forget that both boards the Fab Motor Controller and the MidTiny must share the connection to GND otherwise it won't work properly or just won't work at all.

Video Test

Everything seems to work pretty well.

Writing a test program using the FabMotorController library

It's time to create a test program to test the functions we defined in our library.

The idea of the library is to make the controller easier to use so with a few lines of code we can create a routine to test both outputs of the controller at different speeds and directions.

The test program looks like this.

                           
                              #include 
                              
                                  // // Creates a object
                                  FabMotorController MotorControl;
                                  
                                  void setup() {
                                      Serial.begin(115200);
                                      // Initializes I2C communication with the secondary at address 0x04
                                      MotorControl.begin(0x04);
                                  }
                                  
                                  void loop() {
                                      // Forward test at 70% speed
                                      MotorControl.run(OUTPUT1, 70);
                                      delay(1000);
                                      MotorControl.stop(OUTPUT1);
                                      delay(1000);
                                      MotorControl.run(OUTPUT2, 70);
                                      delay(1000);
                                      MotorControl.stop(OUTPUT2);
                                      delay(1000);
                                  
                                      // Backward test at 30% speed
                                      MotorControl.run(OUTPUT1, -30);
                                      delay(1000);
                                      MotorControl.stop(OUTPUT1);
                                      delay(1000);
                                      MotorControl.run(OUTPUT2, -30);
                                      delay(1000);
                                      MotorControl.stop(OUTPUT2);
                                      delay(1000);
                                  }
                           
                        

Pretty simple isn't it?

After loading the test program into our MidTiny it's time for the moment of truth and try controlling the motors from the MidTiny using I2C.

3-node Communication

The following 3 node communication system is NOT INTENDED TO HAVE AN APPLICATION OR BE FUNCTIONAL, it is only for the purpose of simply testing the communication and integration.

The system is composed of a MASTER board (MidTiny) and two SECONDARY boards (Fab IMU, Fab Motor Controller), the communication is done completely by I2C with the following configuration.

The code essentially reads the value of the angular velocity on the X-axis and maps that value to the motor speed.

                        
                           #include 
                              #include 
                              #include 
                              #include 
                              #include 
                              
                              FabMotorController MotorControl;
                              
                              Adafruit_ICM20948 icm;
                              uint16_t measurement_delay_us = 65535; // Delay between measurements for testing
                              // For SPI mode, we need a CS pin
                              #define ICM_CS 10
                              // For software-SPI mode we need SCK/MOSI/MISO pins
                              #define ICM_SCK 13
                              #define ICM_MISO 12
                              #define ICM_MOSI 11
                              
                              void setup(void) {
                                Serial.begin(115200);
                              
                                MotorControl.begin(0x04);
                                 
                                while (!Serial)
                                  delay(10); // will pause Zero, Leonardo, etc until serial console opens
                              
                                // Try to initialize!
                                if (!icm.begin_I2C()) {
                              
                                }
                                
                                Serial.println("ICM20948 Found!");
                                // icm.setAccelRange(ICM20948_ACCEL_RANGE_16_G);
                                Serial.print("Accelerometer range set to: ");
                                switch (icm.getAccelRange()) {
                                case ICM20948_ACCEL_RANGE_2_G:
                                  Serial.println("+-2G");
                                  break;
                                case ICM20948_ACCEL_RANGE_4_G:
                                  Serial.println("+-4G");
                                  break;
                                case ICM20948_ACCEL_RANGE_8_G:
                                  Serial.println("+-8G");
                                  break;
                                case ICM20948_ACCEL_RANGE_16_G:
                                  Serial.println("+-16G");
                                  break;
                                }
                                Serial.println("OK");
                              
                                //  icm.setAccelRateDivisor(4095);
                                uint16_t accel_divisor = icm.getAccelRateDivisor();
                                float accel_rate = 1125 / (1.0 + accel_divisor);
                              
                                Serial.print("Accelerometer data rate divisor set to: ");
                                Serial.println(accel_divisor);
                                Serial.print("Accelerometer data rate (Hz) is approximately: ");
                                Serial.println(accel_rate);
                              
                                //  icm.setGyroRateDivisor(255);
                                uint8_t gyro_divisor = icm.getGyroRateDivisor();
                                float gyro_rate = 1100 / (1.0 + gyro_divisor);
                              
                                Serial.print("Gyro data rate divisor set to: ");
                                Serial.println(gyro_divisor);
                                Serial.print("Gyro data rate (Hz) is approximately: ");
                                Serial.println(gyro_rate);
                              
                                // icm.setMagDataRate(AK09916_MAG_DATARATE_10_HZ);
                                Serial.print("Magnetometer data rate set to: ");
                                switch (icm.getMagDataRate()) {
                                case AK09916_MAG_DATARATE_SHUTDOWN:
                                  Serial.println("Shutdown");
                                  break;
                                case AK09916_MAG_DATARATE_SINGLE:
                                  Serial.println("Single/One shot");
                                  break;
                                case AK09916_MAG_DATARATE_10_HZ:
                                  Serial.println("10 Hz");
                                  break;
                                case AK09916_MAG_DATARATE_20_HZ:
                                  Serial.println("20 Hz");
                                  break;
                                case AK09916_MAG_DATARATE_50_HZ:
                                  Serial.println("50 Hz");
                                  break;
                                case AK09916_MAG_DATARATE_100_HZ:
                                  Serial.println("100 Hz");
                                  break;
                                }
                                Serial.println();
                              
                              }
                              
                              void loop() {
                              
                                //  /* Get a new normalized sensor event */
                                sensors_event_t accel;
                                sensors_event_t gyro;
                                sensors_event_t mag;
                                sensors_event_t temp;
                                icm.getEvent(&accel, &gyro, &temp, &mag);
                              
                                float value = map(gyro.gyro.x, 0, 20, 50, 100);
                                MotorControl.run(OUTPUT1, value);
                              
                                delay(20);
                              }
                        
                     

A video demonstration of the operation.

Group Assignment

For the group assignment we are communicating 3 boards, Gerhard's board has a LDR photoresistor that senses the amount of light in the environment and turns on/off an LED connected to his board, the sensed value is sent via Serial Communication to Jefferson's board which also turns on an LED but also communicates via I2C with my board to activate/deactivate the motor based on the value sent by Gerhard's board.

Gerhard's code is:

                        
                           // ==========================
                           // SENDER
                           // ==========================
                           
                           #include 
                           
                           SoftwareSerial softwareSerial(0, 1); // RX, TX
                           
                           const int sendPin = 10; // LED pin
                           const int ldrPin = 9;  // LDR pin
                           
                           void setup() {
                             
                             Serial.begin(9600);
                             softwareSerial.begin(9600);
                           
                             pinMode(sendPin, OUTPUT);
                             pinMode(ldrPin, INPUT);
                           
                             Serial.println("Sender ready");
                           }
                           
                           void loop() {
                           
                             // Read ldr value
                             int ldrStatus = analogRead(ldrPin);
                           
                             /******* Serial Monitor for debugging *******/
                             Serial.print("[SENDING] Data: ");
                             Serial.println(ldrStatus); // Displays the value of "ldrStatus" on the serial monitor
                           
                             /******* Send data to Reciver *******/
                             softwareSerial.write(ldrStatus);
                           
                             if (ldrStatus <= 500) {
                               digitalWrite(sendPin, HIGH);
                               
                               // for debugging
                               //Serial.println("[STATUS] Its DARK Turn ON the LED");
                             } else {
                               digitalWrite(sendPin, LOW);
                           
                               // for debugging
                               //Serial.println("[STATUS] Its BRIGHT Turn off the LED");
                             }
                           
                             delay(200);
                           
                           }
                        
                     

Jefferson's code is:

                        
                           // ==========================
                           // RECIVER
                           // ==========================
                           
                           #include 
                           #include 
                           
                           SoftwareSerial softwareSerial(0, 1); // RX, TX
                           
                           #define LED 10 // LED pin
                           
                           int data; // data from the sender
                           
                           // Creates a object
                           FabMotorController MotorControl;
                           
                           void setup() {
                           
                             Serial.begin(9600);
                             softwareSerial.begin(9600);
                           
                             pinMode(LED, OUTPUT);
                           
                             Serial.println("Reciver ready");
                           
                             // Initializes I2C communication with the secondary at address 0x04
                             MotorControl.begin(0x04);
                           }
                           
                           void loop() {
                           
                             /* Read data from Sender */
                             while (softwareSerial.available()){
                               data = softwareSerial.read();
                             }
                           
                             Serial.print("[RECIVER] data: ");
                             Serial.println(data);
                           
                             if (data <= 125){
                               digitalWrite(LED, HIGH);
                               MotorControl.run(OUTPUT1, 100);
                           
                             }
                             else{
                               digitalWrite(LED, LOW);
                               MotorControl.run(OUTPUT1, 0);
                             }
                           
                             delay(200);
                           }
                        
                     

My code is here

This is a demonstration video

Group assignment page

Software
  • Arduino 1.8.19
Files
Fab Motor Controller - Tutorial step by step
Fab Motor Controller - Firmware
Fab Motor Controller - Arduino Library
Fab Motor Controller - Library demo program
3 nodes - Arduino program