Mechatronics Project: Lego Sorter

As part of the fulfillment of requirements for the Mechatronics class and as MA1 students in electromechanical engineering of the Bruface program, we were tasked with creating a working mechatronic system that incorporates what we learned in theory during lectures, mainly mechanical and electronic design as well as programming. The project was a group assignment and we were a total of four students working on it. We were given access to resources from the Fablab (e.g Laser cutter, 3D printers, and various other tools) and a global budget of 200 euros.

The goal of our project is to sort lego blocks based on their color and put them in designated boxes using a gripper system. The lego blocks are fed to the sorting mechanism using a mini conveyor belt equipped with a color sensor that gives the signal to the sorting mechanism to put the block in the adequate box.
The system can be divided into three main parts: lego feeding system (conveyor belt), system that drives the vertical and horizontal motion for sorting (cart moving on frame), and the gripper.

The belt is made out of two sets of side supports that would hold both rollers at the extremities. Additionally, the rollers would hold the tensioned belt and drive its movements. The material chosen for the supports was 6mm thick plywood and the material for the rollers was PLA (Polylactic acid) that is used for 3D printing.

The side supports on the motor's side are as follows: side support for the motor is cut on 6mm plywood and linked with the support that has screws for the motor which is cut on 3mm plywood.

For the rest, the supports have normal 8mm holes to incorporate the bearings and we cut 5 of them on 6mm plywood plus one additional one on 3mm thickness.

Additionaly, we added a frame made with a proximity sensor support, a color sensor support, and a hollow frame so we won't get detection errors from the proximity sensor. The frame on the belt serves to detect the block and stopping the belt so that the color sensor that is positioned on the roof of the frame senses the color of the lego. Then, the lego moves to the initial position so it gets sorted as is shown on the video.

To link the supports together, a piece of 6mm plywood with four contact points was added and would also act as a tension tuner tuner for the belt. Finding a mechanism to correctly tension the belt without redoing the entire design was a very challenging task and the result isn't the most efficient. For future reference, we would advise the reader to first think about a good tensioning system for the belt and then design the entire conveyor belt system around it. That being said, even though the tension tuner isn't optimal, it does the job since the loads are not that big.

For the electrical circuit an example is given above. It shows how the motor driver has to be connected with the DC motor and the Arduino. Note that for the belt only one DC motor is needed. The blue component is the color sensor that has to be fed with 5 V and two pins have to go to the analog ports of the Arduino. The two cables of the color sensor that are connected to the digital pins of the Arduino are not mandatory for a good functioning of the sensor.

Components needed:

- 6mm plywood

- 3mm plywood

- 3 608-2RSH bearings

- 4 x M3 16mm screws

- 8 x M3 25mm screws

- 2 x M3 10mm screws

- 14 x M3 nuts

- 8 x M3 washers

- Laser cutter & 3D printer

- JGA25-370 DC motor 120 RPM (or similar)

- 50mm x 694.25mm x 1.5mm rubber belt

- Thread (to join extremeties of belt)

- 8mm diameter metal shaft

- 1 x E18-D80NK proximity sensor

- 1 x TCS34725 color sensor

- 1 Motor driver L298N

The gripper consists of 3D printed parts whose stl files are shown below. Note that if you can't have acces to 3D printer, it's also possible to make the pieces in wood using a laser cutter. But you have to be carreful concerning the thickness of the pieces. Indeed, all the pieces have a thickness of 3 mm except the "supportGripper" which has a thickness of 6 mm and the "Arm" which has a thickness of 9 mm.The dxf files of the different pieces can be found below. Note that the use of wood will increase the friction.

All parts have been assembled using M3 size nuts and bolts, except for the SG-90 motor, which is only suitable for M2. To assemble the gripper, 7 bolts /nuts M3 and 3 bolts/nuts M2 are used.

The actuator of the gripper is a servomotor SG-90 controlled by the Arduino UNO. The circuit is as follows: orange wire connected to the PWM 9 port, red wire at 5V and brown wire connected to the ground. The arduino code can be found below.

Note that's possible to improve this gripper by using a force sensitive resistor, which makes it possible to determine when the servomotor provides enough force to grip the object.If you use this sensor, you only have to add to the circuit the sensor with a resistor pull-up, the circuit stay quite simple. Indeed, you don't have to use an external source.

Demonstration of the gripper:

Components needed:

- 7 bolts and nuts M3

- 3 bolts and nuts M2

- Arduino uno

- Servomotor SG-90

- 3D printer or Laser cutter

The carrier will provide both horizontal and vertical movement, essentially for the gripper itself. To achieve this, and compliment its inspiration (i.e. Lego), gearing with a rack and pinion system was utilized. The carrier frame was designed so it could be laser cut (6mm plywood or MDF), for easy, fast and cheap production (see the DXF files). The sides were 2-layered to be able to place bearings for both the outer gears and inner gears. However, the gearing, requiring precision to function correctly, were 3D printed (see the STL files). Straight gears and racks were created with integrated software from Fusion 360. Backlash and interference were detrimental to avoid, and the decision of a module of 1.5 and contact angle of 20 degrees imposed a minimal amount of teeth of 18. Reinforced motor mounts and plates were employed, to minimize flexing.

Horizontal motion was achieved by driving 2 big gear wheels on the same axle, with a 1:1 ratio since gravity was no obstacle. The other 2 big gear wheels were undriven. Additional guide wheels kept the carrier in place: high acceleration or vibrations could potentially lift the wheel gears of its rack.

Vertical motion was achieved by driving 1 two-stage gear (i.e. 2 different gears on the same axis) with a 24:18 reduction to battle gravity even more (although the vertical rack and gripper were light, reduction also gives more precision in movement). This gear then actuated the 3 others (through gearing and axles), and thus the rack could move up and down reliably. 4 additional guide gears were implemented to ensure the rack would remain straight. Locking up the frame when the motor is not powered, is achieved by a catching pin that follows a curved path and jumps from dent to dent.

To also minimize friction, bearings were used for every rotating axis, and pieces were cut out of acrylic rather than plywood to avoid the high friction of wood on wood. Every axle was locked in place on both sides to prevent teeth from lifting.

The power source were DC motors with internal gearing (63:1 to achieve the desired linear speeds), to prevent that static friction would cause the motors to deliver stall torque (and burning up the windings). DC motors have a high power density, are cheap, reliable and easy to control. However, they do not possess built-in feedback, and thus, tachometers attempted to fulfill this role. The previous sensor was an ultrasonic one, but they are too slow, take too many inputs (multiplexing required) and could produce erratic results. However, the tachometers, even after endless tries, did not perform satisfactory. They only measure relative speed, not the absolute position. Additional switches, that the cart will activate by rolling over them, provided the necessary reference position for each box and the beginning position. The tachometers can still be used to anticipate the position of the switches to prevent excessive overshoot.

Components used:

• 2x 63:1 DC motors with internal gearing, 6V nominal (Pololu 3452)
• 2x L298N motor drivers
• 14x d10D19b5 grooved ball bearings
• 4x d12D26b6 grooved ball bearings
• 2x Photo interruptor Venel B073VGZN6K
• 3mm and 6mm thick plywood
• 6mm thick acrylic
• Prusa PLA 3D printing material
• 17x M3x16 bolts and nuts
• 8x M3x20 bolts and nuts
• 4x M5x40 bolts and nuts
• 2x 3x12 screws

The frame, visible in the previous step, will carry the carrier and therefore it has to be stable and strong. The teeth on which the carrier moves are immediately "inserted" in the long horizontal beams. So it is advised to cut these components with a laser cutter. Ideally all the pieces should be cut by the laser cutter. Note that every "beam" is made of two pieces that will be fixed together with screws and nuts. Then to connect all these "beams" together, connections have to be made with a 3D printer. These pieces were made to stiffen the whole structure. When the beams were screwed together, the frame was too weak and the beams rotated a bit when the carrier moved. For the material plywood is recommended except for the long racks. For the racks plexiglass is advised, because the teeth of the racks can be damaged quite fast when the carrier moves on racks made of plywood. All the drawings can be found below to make this frame. Note that the pieces are 6 mm thick.

The support plate offers stability for the frame, boxes, control panel, the arduino and drivers. The boxes are the obvious target for the sorted bricks and the control panel will provide an easy interface, as well as safety measures for the user. The LED will remain steady when the sorter is ready to be engaged (or a new brick hasnt been detected), and a blinking LED indicates an ongoing sorting process. The initialise the sorting, the START button needs to be pushed. To reset and abort the sorting, the RESET button will need to be pushed. An emergency button will interrupt all power electronics would something go wrong. A 2A slow fuse (considering all the inductive loads) should provide the robot against short circuits. The robot needs to be fed with 8.5VDC from a regulated power source, to overcome the voltage drop of the motor drivers.

The arduino code that you can find here below will execute the whole cycle of the robot. In the first part all the variables that are needed for the code are initialized. Some of the variables have to be modified, because they have to be fine tuned with experiments. These variables depend on the robot used. Pay attention also to the pins used for each component and on how the direction of the rotation of the motors are defined.

The second part of the code sets the pins in de setup part. Afterwards the main loop starts. During the first cycle the position of the robot has to be initialized because the robot uses only sensors with relative position. Also the first Lego block has to be placed at the end of the belt to be ready to be grabbed.

Then the main loop can start. The code will "jump" from one if-block to another while it follows the sequence "move vertically up" - "move horizontally" - "move vertically down". Depending if the gripper has a Lego Block or not, the robot will move towards the boxes or towards the belt. When the robot leaves the belt with a Lego block, the carrier won't move during a few seconds when the gripper reached the top, to let the belt move and to detect the color of the next block. During this process, the Arduino must also pay attention to the buttons. Because when the reset button, the whole code will be reset. And when the emergency button is pressed, the motors are not fed anymore and the arduino code must then stay in standby. Obviously the cycle will repeat itself over and over again. A flow chart can be found above that will make the structure of the code more clear.

To avoid repeating code and to make the main code more understandable, a lot of functions were defined after the main loop. So there is a function for each motor, sensor, button and LED. Note that the code will be put in standby if the belt takes too much time to detect the next Lego block.

//Arduino Code: Lego Sorter

#inlude <Servo.h>;
Servo gripper;
#include <Wire.h>
#include "Adafruit_TCS34725.h"

//set all the variables needed

//constants that will be used when we call functions
int upDirection = 1;
int downDirection = -1;
int boxDirection = 1;
int blockDirection = -1;
int gripperOpening = 1;
int gripperClosing = -1;

//at the beginning I set all the variables to false to have no errors like “no value attributed”

bool moveRobot = false; //if true, the robot has to move vertically or horizontally; if false the robot doesn’t

bool gripperWorks = false;   //true when the gripper opens or grabs
bool grab; //true when the gripper has to close; false when it has to open
int pinGripper = 9;  //number of the pin to control the servomotor
int angleLow = 10; //angles have to be finetuned
int angleHigh = 120; //angles have to be finetuned
int delayGripper = 5;

bool moveGlobalToBox = false; //if true, the goal of the robot is to at the end to the box (contains the following sequence of movement up -> then horizontal towards boxes->finally down to the box); if false the goal of the robot is to go finally  to the block (contains the following sequence of movement up -> then horizontal towards blocks>finally down to the block)

bool moveVertical = false; //true when the robot has to move vertically
bool moveUp = false; //true when it moves upwards; false when it moves downwards

bool moveHorizontal = false; //true when the robot has to move horizontally
bool moveToBox = false; //true when it moves towards the boxes; false when it moves towards the //block

bool reached; //this variable will be used several times in the code; it will be the output of the function moveVerticalFunction and moveHorizontalFunction

bool moveBelt = false; //if true, the belt has to work till the next block is detected by the proximity sensor

bool firstCycle = true; //special things to do for the first cycle

/* Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3.3V DC
//color sensor

Connect GROUND to common ground */
/* Initialise with default values (int time = 2.4ms, gain = 1x) */
// Adafruit_TCS34725 tcs = Adafruit_TCS34725();
 
/* Initialise with specific int time and gain values */
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_700MS, TCS34725_GAIN_1X);
int limitColor = 1500; //value has to be fine tuned depending on the sensor and the blocks used

//set the array of 'limits'/boundaries
int distances[4] = {200, 320, 420}; //first value depends on where the belt is placed

//store the color of the current and following color with a number
int currentColor = -1; //1 = grey; 2 = red; 3 = green
//color of the following color
int followingColor; //1 = grey; 2 = red; 3 = green

//setup

//set all the pins

int pinTachoHor = 0;
int counterTachHor = 0;
int prevValTachHor = -1;
int valTachHor;
int treshholdTachHor = 0.5;
int numberOfTeethHor = 18;  //of the tachometer
int numberOfDetectionsPerCycleHor = 18*2;
int diameterHor = 4.2; //cm   pitchdiameter of the wheels for the horizontal movement
int maxCountTachHor;

 //tachometer vertical
int pinTachoVer = 1;
int counterTachVer = 0;
int prevValTachVer = -1;
int valTachVer;
int treshholdTachVer = 0.5;
//int numberOfTeethVer = 18;  //of the tachometer
int numberOfDetectionsPerCycleVer = 18*2;
//int diameterVer = 2.7; //cm    pitchdiameter of the wheels for the vertical movement
int maxCountInitVer = 36; //has to be finetuned (position of the rack at the beginning at the position LOW, number of rising/faling edges from when the rack is completely down)
int maxCountTachVer = 36; //to be fine tuned

 //pin proximity sensor
 int pinProximitySensor = 2;//ATTENTION PULL UP RESISTOR, for example pin 2

 int detectionProximity; //equal to ZERO if it detects an object

 //DC motors
int pinEnableVert = 3; //PWM
int pinInMotorVert1 = 4; //not PWM
int pinInMotorVert2 = 7; //not PWM
int pwmOutput = 255; //full power
int pinEnableHor = 5;
int pinInMotorHor1 = 8; //not PWM
int pinInMotorHor2 = 12; //not PWM
int pwmZero = 0; //to disable the H-bridge
int delayInitHor = 500; //milliseconds
int pwmSlow = 100; //has to be finetuned

//motor belt
int pinEnableBelt = 6;
int pinInMotorBelt1 = 10; //not PWM
int pinInMotorBelt2 = 11; //not PWM
int delayStopBelt = 900; //has to be finetuned
int delaySecondWorkBelt = 500; //has to be finetuned

//switches placed on the rack that will be enabled when the motor passes on
int pinSwitches = A3; //analog
//array values of the switches
//lower limit to be sure to detect the switch
int voltageSwitches[3] = {200, 500, 800}; //1024*ratio with ratio = 10kOhm/(resisitor+10kOhm); resisitors are equal to 39kOhm, 10 kOhm and 2.4kOhm
int valueSwitch;

//LED's and buttons
int pinLED = 13;
int pinButtonOn = A0; //analog  //to start the cycle
int pinButtonReset = A1; //analog   //to reset the whole cycle
int treshholdReset = 500;
int valueReset;
bool resetEnabled = false;
int tressholdStart = 500;
bool startButtonPressed = false;
int valueButtonStart;

//emergency button
int pinEmergency = A2; //analog input
bool emergencyEnabled = false;
int treshholdEmergency = 500;
int valueEmergency;

//time
unsigned long prevTime = 0;
unsigned long currentTime = millis();
int maxDurationBelt = 10000; //10 seconds max
unsigned long startTimeBelt;
int timeLimitVerInit = 100;

void setup(){

Serial.begin(9600);

//set all the pins
gripper.attach(pinGripper);   //pin to control the servomotor of the gripper

//DC motors (OUTPUTS)  //9 digital ports needed
pinMode(pinEnableVert, OUTPUT); //PWM
pinMode(pinInMotorVert1, OUTPUT);
pinMode(pinInMotorVert2, OUTPUT);
pinMode(pinEnableHor; OUTPUT); //PWM
pinMode(pinInMotorHor1, OUTPUT);
pinMode(pinInMotorHor2, OUTPUT);

//motor belt
pinMode(pinEnableBelt; OUTPUT); //PWM
pinMode(pinInMotorBelt1, OUTPUT);
pinMode(pinInMotorBelt2, OUTPUT);

//tachometer
pinMode(pinTachoHor, INPUT);
pinMode(pinTachoVer, INPUT);

//proximity sensor
pinMode(pinProximitySensor, INPUT_PULLUP);

//LED's and buttons
pinMode(pinLED, OUTPUT);

//emergency button
pinMode(pinEmergency,INPUT) //MAYBE NOT NEEDED

}

void(* resetFunc) (void) = 0;//declare reset function at address 0

void loop(){

//only during the first loop
if(firstCycle){
  //put a Lego block on the position where it can be grabbed and detect the color
  moveBeltFunction();
}
  //initialize the vertical and horizontal position, because the sensors only measure relative positions
  //initialize the vertical position
  //rack moves downwards
  analogWrite(pinEnableVer, pwmOutput); // Send PWM signal to L298N Enable pin
  digitalWrite(pinInMotorVer1, LOW); //control high and low
  digitalWrite(pinInMotorVer2, HIGH);
  LEDblink();
  currentTime = millis();
  prevTime = currentTime;
  //vertical rack moves till it goes to the end
  //loop stops when the rack doesn't move anymore, so when the tachometer does not move anymore
  counterTachVer = 0;
  while(currentTime<prevTime + timeLimitVerInit){
      valTachVer = digitalRead(pinTachoVer);
      if(prevValTachVer<valTachVer && valTachVer>treshholdTachVer){
        counterTachVer++;
        prevTime = millis();
      }
      else if(prevValTachVer>valTachVer && valTachVer<treshholdTachVer){
        counterTachVer++;
        prevTime = millis();
      }
      prevValTachVer = valTachVer;
      currentTime = millis();
    }  
    analogWrite(pinEnableVer, pwmZero); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
    prevValTachVer = -1;
    counterTachVer = 0;

  //rack moves upwards
    analogWrite(pinEnableVer, pwmOutput); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, HIGH); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDblink();

    counterTachVer = 0; //initialize
    while(counterTachVer<maxCountInitVer){
      valTachVer = digitalRead(pinTachoVer);
      //detect a faling or rising edge of the teeth of the tachometer
      if(prevValTachVer<valTachVer && valTachVer>treshholdTachVer){
        counterTachVer++;
      }
      else if(prevValTachVer>valTachVer && valTachVer<treshholdTachVer){
        counterTachVer++;
      }
      prevValTachVer = valTachVer;
    }
    analogWrite(pinEnableVer, pwmZero); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
    counterTachVer = 0;
    prevValTachVer = -1;

  
  //initialize the horizontal position
  //go to block
  analogWrite(pinEnableHor, pwmOutput); // Send PWM signal to L298N Enable pin
  digitalWrite(pinInMotorHor1, LOW); //control high and low
  digitalWrite(pinInMotorHor2, HIGH);
  LEDblink();
  delay(delayInitHor)
  //motor stop
  analogWrite(pinEnableHor, pwmZero); // Send PWM signal to L298N Enable pin
  digitalWrite(pinInMotorHor1, LOW); //control high and low
  digitalWrite(pinInMotorHor2, LOW);
  LEDshutDown();

  //motor goes to the boxes 
  analogWrite(pinEnableHor, pwmOutput); // Send PWM signal to L298N Enable pin
  digitalWrite(pinInMotorHor1, HIGH); //control high and low
  digitalWrite(pinInMotorHor2, LOW);
  LEDblink();
  valueSwitch = analogRead(pinSwitches);
  while(valueSwitch<voltageSwitches[3]){
    valueSwitch = analogRead(pinSwitches);
  }
  //last switch is reached, now the absolute value of the robot is known
    analogWrite(pinEnableHor, pwmZero); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorHor1, LOW); //control high and low
    digitalWrite(pinInMotorHor2, LOW);
    LEDshutDown();
    
    
   valueButtonStart= analogRead(pinButtonOn);
   if(valueButtonStart>tressholdStart){
    startButtonPressed = true;
   }
   //wait till the start button is pressed
  while(!startButtonPressed){
    valueButtonStart= analogRead(pinButtonOn);
    if(valueButtonStart>tressholdStart){
     startButtonPressed = true;
   }
  }
  moveRobot = true;
  moveGlobalToBox = false;
  moveVertical = true;
  moveUp = true;
  
  firstCycle = false;
}

//Go to block

//function used when the gripper has to find the block  and go to it

if(moveRobot && !moveGlobalToBox){
//Move vertically upwards
if(moveVertical && moveUp){
  //use DC motor to go upwards
  //+check when you reach the “top”
  reached = moveVerticalFunction(upDirection);
  if(reached){
    moveVertical = false;
    moveHorizontal = true;
    moveToBox = false;
  }
}

//Move horizontally towards the blocks
if(moveHorizontal && !moveToBox){
  //use DC motor to go to the block
  //+check when you are above the block
  reached = moveHorizontalFunction(blockDirection);
  if(reached){
    moveHorizontal = false;
    moveVertical = true;
    moveUp = false;
  }
}

//Move vertically downwards
if(moveVertical && !moveUp){
  //use DC motor to go downwards
  //+check when you are at the block
  reached = moveVerticalFunction(downDirection);
  if(reached){
    moveVertical = false;
    moveRobot = false;
    gripperWorks = true;
    grab = true;
}
}
}
//Go to boxes

//The robot has to find the right box and go to it

if(moveRobot && moveGlobalToBox){

//Move vertically upwards
//+controlBelt when the gripper is fully up
//+detect next color (maybe we need an array of the colors in the queue)
if(moveVertical && moveUp){
  //use DC motor to go upwards
  //+check when you reach the “top”
  reached = moveVerticalFunction(upDirection);
  if(reached){
    moveVertical = false;
    moveRobot = false;
    moveBelt = true;
  }
}

//Move horizontally towards the boxes
if(moveHorizontal && moveToBox){
  //use DC motor to go to the block
//+check when you are above the block
  reached = moveHorizontalFunction(boxDirection);
  if(reached){
    moveHorizontal = false;
    moveVertical = true;
    moveUp = false;
  }
}

//Move vertically downwards
if(moveVertical && !moveUp){
  //use DC motor to go downwards
  //+check when you are at the block (How do we check? Ultrasonic sensor?)
  reached = moveVerticalFunction(downDirection);
  if(reached){
    moveVertical = false;
    moveRobot = false;
    gripperWorks = true;
    grab = false;
  }
}
}
//Grab the the block or open the gripper

//the robot has to grab the robot and we assume that the robot is already at the block in position to grab it or it has to put the block in the box by opening the gripper

if(gripperWorks){
  if(grab){
    reached = moveGripperFunction(gripperClosing);
    if(reached){
      gripperWorks = false;
      moveRobot = true;
      moveGlobalToBox = true;
      moveVertical = true;
      moveUp = true;
    }
  }
  if(!grab){
    reached = moveGripperFunction(gripperOpening);
    if(reached){
      gripperWorks = false;
      moveRobot = true;
      moveGlobalToBox = false;
      moveVertical = true;
      moveUp = true;
    }
  }
}
//Belt is working

//belt has to work when a block is grabbed and taken away
if(moveBelt){
  reached = moveBeltFunction();
  if(reached){
    //detect the color of the next block (maybe stack in an array)
    
    moveBelt = false;
    moveRobot = true;
    moveGlobalToBox = true;
    moveHorizontal = true;
    moveToBox = true;
  }
}
}

//Functions

//Define “global” functions that will be used in different “paragraphs”

bool moveVerticalFunction(int directionVer ){
//direction is positive if it moves upwards and negative if it moves downwards
// the output is a boolean that says if his task is finished (robot reached his final position)

if(directionVer>0){
  //action on the dc motor
    analogWrite(pinEnableVer, pwmOutput); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, HIGH); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDblink();
    counterTachVer = 0; //initialize
    while(counterTachVer<maxCountTachVer){
      emergencyControl(); //control if the emergency button was pressed
      resetControl(); // control if the reset button was pressed
      valTachVer = digitalRead(pinTachoVer);
      //detect a faling or rising edge of the teeth of the tachometer
      if(prevValTachVer<valTachVer && valTachVer>treshholdTachVer){
        counterTachVer++;
      }
      else if(prevValTachVer>valTachVer && valTachVer<treshholdTachVer){
        counterTachVer++;
      }
      prevValTachVer = valTachVer;
    }
    emergencyControl(); //control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    analogWrite(pinEnableVer, pwmZero); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
    counterTachVer = 0;
    prevValTachVer = -1;
  
}
else if(directionVer<0){
  analogWrite(pinEnableVer, pwmOutput); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, HIGH);
    LEDblink();
    counterTachVer = 0; //initialize
    while(counterTachVer<maxCountTachVer){
      emergencyControl(); //control if the emergency button was pressed
      resetControl(); // control if the reset button was pressed
      valTachVer = digitalRead(pinTachoVer);
      //detect a faling or rising edge of the teeth of the tachometer
      if(prevValTachVer<valTachVer && valTachVer>treshholdTachVer){
        counterTachVer++;
      }
      else if(prevValTachVer>valTachVer && valTachVer<treshholdTachVer){
        counterTachVer++;
      }
      prevValTachVer = valTachVer;
    }
    emergencyControl(); //control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    analogWrite(pinEnableVer, pwmZero); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
    counterTachVer = 0;
    prevValTachVer = -1;
}
Serial.print("Vertical ");
Serial.print("up ");
Serial.println(directionVer);
return true; //go back to the main loop
}

bool moveHorizontalFunction(int directionHor ){
//direction is positive if it moves to the boxes and negative if it moves to the blocks
// the output is a boolean that says if his task is finished (robot reached his final position)
distanceHor = measureUltrasonic("Hor")
if(directionHor>0){
  analogWrite(pinEnableHor, pwmOutput); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorHor1, HIGH); //control high and low
    digitalWrite(pinInMotorHor2, LOW);
    LEDblink();

  counterTachHor = 0; //initialize
  maxCountTachHor = numberOfDetectionsPerCycleHor*distances[currentColor]/diameterHor/3.1415; //number of edges that the tacho has to detect till the target
    while(counterTachHor<maxCountTachHor){
      emergencyControl(); //control if the emergency button was pressed
      resetControl(); // control if the reset button was pressed
      valTachHor = digitalRead(pinTachoHor);
      //detect a faling or rising edge of the teeth of the tachometer
      if(prevValTachHor<valTachHor && valTachHor>treshholdTachHor){
        counterTachHor++;
      }
      else if(prevValTachHor>valTachHor && valTachHor<treshholdTachHor){
        counterTachHor++;
      }
      prevValTachHor = valTachHor;
    }
    emergencyControl(); //control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    //slow down
    analogWrite(pinEnableVer, pwmSlow); // Send PWM signal to L298N Enable pin
    counterTachVer = 0;
    prevValTachVer = -1;
    //detect when we are at the right switch
    valueSwitch = analogRead(pinSwitches);
    while(valueSwitch<voltageSwitches[currentColor]){
      valueSwitch = analogRead(pinSwitches);
    }
    analogWrite(pinEnableVer, pwmZero);
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
    
}
else if(directionHor<0){
  analogWrite(pinEnableHor, pwmOutput); // Send PWM signal to L298N Enable pin
    digitalWrite(pinInMotorHor1, LOW); //control high and low
    digitalWrite(pinInMotorHor2, HIGH);
    LEDblink();

 counterTachHor = 0; //initialize
  maxCountTachHor = numberOfDetectionsPerCycleHor*distances[currentColor]/diameterHor/3.1415; //number of edges that the tacho has to detect till the target
    while(counterTachHor<maxCountTachHor){
      emergencyControl(); //control if the emergency button was pressed
      resetControl(); // control if the reset button was pressed
      valTachHor = digitalRead(pinTachoHor);
      //detect a faling or rising edge of the teeth of the tachometer
      if(prevValTachHor<valTachHor && valTachHor>treshholdTachHor){
        counterTachHor++;
      }
      else if(prevValTachHor>valTachHor && valTachHor<treshholdTachHor){
        counterTachHor++;
      }
      prevValTachHor = valTachHor;
    }
    emergencyControl(); //control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    counterTachVer = 0;
    prevValTachVer = -1;
    analogWrite(pinEnableVer, pwmZero);
    digitalWrite(pinInMotorVer1, LOW); //control high and low
    digitalWrite(pinInMotorVer2, LOW);
    LEDshutDown();
}

Serial.print("Horizontal ");
Serial.print("to Box ");
Serial.println(directionHor);
return true; //go back to the main loop
}

bool moveGripperFunction(int directionGripper ){
//the servomotor has to open or close
//it opens when direction is positive and closes when it’s negative
// the output is a boolean that says if his task is finished (gripper reached his final position)
//pay attention on how the servomotor was placed: it will affect how the angle will be defined
if(directionGripper<0){
  //gripper closing
  LEDblink();
  for(int angle = angleLow; angle<angleHigh; angle++){
    emergencyControl(); // control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    gripper.write(angle);
    delay(delayGripper);
    
  }
  LEDshutDown();
}
else if(directionGripper>0){
  //gripper opening
  LEDblink();
  for(int angle = angleHigh; angle>angleHigh; angle--){
    emergencyControl(); // control if the emergency button was pressed
    resetControl(); // control if the reset button was pressed
    gripper.write(angle);
    delay(delayGripper);
  }
  LEDshutDown();
}
Serial.print("Gripper ");
Serial.print("opens ");
Serial.println(directionGripper);
return true; //go back to the main loop
}

bool moveBeltFunction(){
//DC motors work till the proximity sensor detects the next block
// the output is a boolean that says if his task is finished (next block is at the right position)
Serial.println("Belt");
//let the motor work
analogWrite(pinEnableBelt, pwmOutput);
digitalWrite(pinInMotorBelt1, HIGH); // pay attention to the direction of rotation of the motor
digitalWrite(pinInMotorBelt2, LOW);
LEDblink();
startTimeBelt = millis();
boolean detected = detectProximity();
while(detect==false){
  //action on dc motor
  emergencyControl(); // control if the emergency button was pressed
  resetControl(); // control if the reset button was pressed
  timeControl(startTimeBelt);
  detected = detectProximity(); //control if the proximity sensor detected a Lego block
}
//stop belt
emergencyControl(); // control if the emergency button was pressed
resetControl(); // control if the reset button was pressed
delay(delayStopBelt) //don't stop the belt immediately to put the color sensor in the center of the Lego block
//we are blind for the two buttons during this delay

emergencyControl(); // control if the emergency button was pressed
resetControl(); // control if the reset button was pressed
analogWrite(pinEnableBelt, pwmZero);
digitalWrite(pinInMotorBelt1, LOW);
digitalWrite(pinInMotorBelt2, LOW);
LEDshutDown();
//detect the color
detectColor();
emergencyControl(); // control if the emergency button was pressed
resetControl(); // control if the reset button was pressed

//put the block at the end of the belt to be able to grab the block
//enable again the motor
analogWrite(pinEnableBelt, pwmOutput);
digitalWrite(pinInMotorBelt1, HIGH);
digitalWrite(pinInMotorBelt2, LOW);
LEDblink();
emergencyControl(); // control if the emergency button was pressed
resetControl(); // control if the reset button was pressed
delay(delaySecondWorkBelt)
//we are blind for the two buttons during this delay

emergencyControl(); // control if the emergency button was pressed
resetControl(); // control if the reset button was pressed
//stop the motor again
analogWrite(pinEnableBelt, pwmZero);
digitalWrite(pinInMotorBelt1, LOW);
digitalWrite(pinInMotorBelt2, LOW);
LEDshutDown();

return true; //go back to the main loop
}

//control if the Lego block is detected by the proximity sensor
boolean detectProximity(){
  detectionProximity = =digitalRead(pinProximitySensor);
  boolean detected = false;
  if(detectionProximity == 0){
    detected = true;
  }
  return detected;
}

//must detect the color and then depending on the color, the robot knows to which box it has to go
void detectColor(){
  //initialize the variables needed for the color sensor
  uint16_t r, g, b, c, colorTemp, lux;
  tcs.getRawData(&r, &g, &b, &c);  //gives the red, green and blue color
  //grey block
  if(r>limitColor && g>limitColor && b>limitColor){
    //first box
    currentColor = followingcolor;
    followingColor = 1;
  }
  //red block
  else if(r>limitColor){
    //second box
    currentColor = followingcolor;
    followingColor = 2;
  }
  //green block
  else{
    //third block
    currentColor = followingcolor;
    followingColor = 3;
  }
}

//control if the emergency button was pressed
void emergencyControl(){
  valueEmergency = analogRead(pinEmergency);
  if(valueEmergency>treshholdEmergency){
    emergencyEnabled = true;
  }
  
  //stay in this while loop till the emergency button is released
  //when it is released, the code will go back where it was
  while(emergencyEnabled){
    Serial.println("emergency enabled");
    emergencyEnabled = false;
    valueEmergency = analogRead(pinEmergency);
    if(valueEmergency>treshholdEmergency){
      emergencyEnabled = true;
  }
  Serial.println("emergency disabled");
}

//control if the reset button is pressed
void resetControl(){
  valueReset = analogRead(pinButtonReset);
  if(valueReset>treshholdReset){
    resetEnabled = true;
    resetFunc(); //call reset 
  }
}

void timeControl(unsigned long startBelt){
  currentTime = millis();
  if(startBelt + maxDurationBelt>= currentTime){
    Serial.println("Belt took too much time");
    Serial.println("User has to take action and afterwards restart the code");
    while(1){
      //code stays stuch here because there is a problem with the belt and the Lego blocks
    }
  }
}

void LEDblink(){
  digitalWrite(pinLED, HIGH);
}

void LEDshutDown(){
  digitalWrite(pinLED,LOW);
}

An example of the electrical circuit can be found above. Pay attention to the pins used, because they don't match with those used in the code. Note also that the proximity sensor, the emergency sensor, the DC motor with the motor driver and the two photo interrupts are missing in the electrical circuit.

Arno, Grégoire, Hafsa and Réda:

We didn't meet the deadline for a full complete functioning robot, and although every component was working separately, the combination of everything proved to be an issue. But we still believe this was a good design; the main problem was time management. This project will be updated after the exams of January 2021. Some key ideas so you don't fall into the same trap:

• Keep it simple and put reliability first. Designs quickly gain additions, become complex and assembly becomes a struggle: do not try to reinvent the wheel;
• Try to think of every degree of freedom, every possible problematic situation;
• Plan worst-case scenario, you never have enough time;
• Trade perfection in for completion;
• Things go wrong all the time, guaranteed;
• Drawings both in 3D and 2D should be made at home to work as efficient as possible in the fablab;
• 3D prints have great potential but are very slow and only reliable when done right;
• Never neglect friction, especially not wood on wood. This could prove to be very problematic when dimensioning actuators;
• Allow for tolerances in your design, especially with high precision like with e.g. gearing. It will never match perfectly together as in the 3D drawing.
• Use reliable sensors: the ultrasonic sensors from Arduino are not suited for this kind of project.