Line Follower Robot V4

24 08 2020

I present to you the advance of the robot following lines that I have worked lately.

Components:

  • 1 Arduino Nano
  • 1 Arduino Nano Shield
  • 5 CNY70 sensors
  • 2 IR Sensors GP1A01 (Similar FC-03)
  • 1 Driver L298N
  • 2 Motors with gearbox and encoder
  • 2 Batteries 3.7V 4200mAH 18650
  • 1 18650 battery holder
  • 1 5V USB charger
  • 1 1P1T switch 1 Pushbutton
  • 1 2-tier circular acrylic chassis
  • 2 Wheels covered with eva foam
  • 2 Sphere wheels
  • 2 double-sided perforated breadboard
  • Some LEDs and SMD resistors and various connecting cables

This is one of the first functional tests, in which I included the use of the programming of the PID feedback algorithm in which at the moment I use the constants P and D.

Adjustments regarding speed control through RPM and activate the constant I are pending.

The sensors used five CNY70 and are read analogically. Initially it is necessary to calibrate the sensors for best performance (10 seconds).

CNY70 Datasheet: Download

The motors are powered by 2×3.7V 4200mAH 18650 batteries in series (7.4V) and for the control part (Arduino Nano) I use a 5V USB portable charger.

I include the physical design made in TinkerCAD

I have made some templates with which depending on the way they are placed, any size of track can be made. I have done 2, but the limit is imatination.

Code: DrakerDG/Line-Follower-Robot-V4

/* Line Follower 5 Analog Sensor
+ RPM Robot V2
By DrakerDG (c)
https://www.youtube.com/user/DrakerDG
*/
#include <SimplyAtomic.h>
#include <TimerOne.h>
// Speeds Motors Base 
const byte SpBSE = 175; 
const byte SpFWD = 135;
const byte SpREV = 180;
// Line PID constants
float Kp = 0.02; // 0.02;
float Ki = 0.00; // 0.00;
float Kd = 0.69; // 0.065;
long P=0, I=0, D=0, PIDv=0, pErr=0;
// Analog Sensor Pins
const byte pSen[5] = {14, 15, 16, 17, 18};
// Sensor Position
unsigned long PosX = 0;
// LEDs Pins
const byte pLED[3] = {9, 10, 11};
// Switch Pin
const byte pSW = 4;
// Sensor Values
int SenX[5];
// Max Sensor Samples
int MinX[5];
// Min Sensor Samples
int MaxX[5];
// On Line Status
bool detLe = false;
// Running Status
bool OnRun = false;
// Timer Counters
unsigned long Tm0 = 0;
unsigned long Tm1 = 0;
unsigned long prT = 0;
// RPM Pins
const byte PinSA = 2;
const byte PinSB = 3;
const long uSeg = 5000;
// Pulse timer counters
volatile unsigned long pwc[2];
// PWM periods
volatile unsigned long pwm[2];
// RPM values
unsigned long rpm[2];
// Left Motor Pins
const byte SML = 5;
const byte ML1 = 8;
const byte ML2 = 7;
// Right Motor Pins
const byte SMR = 6;
const byte MR1 = 13;
const byte MR2 = 12;
// Functions
void CalSnX(void);
void BlinkX(void);
void EstSnX(void);
void PosLED(void);
void CalRPM(void);
void CalPID(void);
void MoCTRL(void);
void SAcoun(void);
void SBcoun(void);
void RPMctr(void);
void setup(){
  Serial.begin(9600);
  
  // Left Motor Pins Setup
  pinMode(SML, OUTPUT);
  pinMode(ML1, OUTPUT);
  pinMode(ML2, OUTPUT);
  digitalWrite(SML, LOW);
  digitalWrite(ML1, LOW);
  digitalWrite(ML2, LOW);
    
  // Right Motor Pins Setup
  pinMode(SMR, OUTPUT);
  pinMode(MR1, OUTPUT);
  pinMode(MR2, OUTPUT);
  digitalWrite(SMR, LOW);
  digitalWrite(MR1, LOW);
  digitalWrite(MR2, LOW);
  for(byte i=0;i<3;i++){
    pinMode(pLED[i], OUTPUT);
    digitalWrite(pLED[i], LOW);
  }
  
  // Sensor Pins to RPM Meter
  pinMode(PinSA, INPUT);
  pinMode(PinSB, INPUT);
  
  // Start SW
  pinMode(pSW, INPUT);
  
  for (byte i=0; i<2; i++){
    pwc[i] = uSeg;
    pwm[i] = uSeg;
    rpm[i] = 0;
  }
  // Count Period Time Interrupt
  Timer1.initialize(100);
  Timer1.attachInterrupt(RPMctr);
  
  //  RPM Motor A Sensor Interrupt
  attachInterrupt(digitalPinToInterrupt(PinSA), SAcoun, FALLING);
  
  // RPM Motor B Sensor Interrupt
  attachInterrupt(digitalPinToInterrupt(PinSB), SBcoun, FALLING);
  delay(1500); 
  // Calibration Init
  digitalWrite(pLED[1], HIGH);
  CalSnX();
  digitalWrite(pLED[1], LOW);
  // Calibration End
  delay(500);  
}
void loop(){
  if(digitalRead(pSW)) OnRun=true;
  EstSnX();
  PosLED();
  if(OnRun){
    CalRPM();
    CalPID();
    MoCTRL();
  }
}
void CalSnX(){
  Tm0 = millis();
  Tm1 = Tm0;
  unsigned long TmL; 
  for(byte i=0; i<5; i++){
    SenX[i]=analogRead(pSen[i]);
    MinX[i]=SenX[i];
    MaxX[i]=SenX[i];
  }
  while((millis()-Tm0)<=10000){
    for(byte i=0; i<5; i++){
      SenX[i]=analogRead(pSen[i]);
      if(SenX[i]<MinX[i]) MinX[i]=SenX[i];
      if(SenX[i]>MaxX[i]) MaxX[i]=SenX[i];
    }
    TmL = millis();
    if ((TmL-Tm1)>=100){
      BlinkX();
      Tm1 = TmL;
    }
  }
/*
  for(byte i=0; i<5; i++){
    Serial.print(MinX[i]);
    Serial.print("  ");
  }
  Serial.println();
  for(byte i=0; i<5; i++){
    Serial.print(MaxX[i]);
    Serial.print("  ");
  }
*/
}
void BlinkX(){
  for(byte i=0;i<3;i++){
    digitalWrite(pLED[i], !digitalRead(pLED[i]));
  }
}
void EstSnX(){
  unsigned long TmE = millis();
  if ((TmE-Tm0)>10){
    detLe = false;
    unsigned long avgS = 0;
    unsigned int sumS = 0;
    
    for(byte i=0; i<5; i++){
      SenX[i] = analogRead(pSen[i]);
      SenX[i] = map(SenX[i], MinX[i], MaxX[i], 1000, 0);
      SenX[i] = constrain(SenX[i], 0, 1000);
      if(SenX[i]>200)detLe = true;
      if(SenX[i]>50){
        avgS += (long)SenX[i]*(i*1000);
        sumS += SenX[i];
      }
    }
    if(detLe)PosX = avgS/sumS;
    else if(PosX < 2000)PosX = 0;
    else PosX = 4000;
/*    
    char DataX[60];
    sprintf(DataX,"%4d  %4d  %4d  %4d  %4d  %4d  ", SenX[0], SenX[1], SenX[2], SenX[3], SenX[4], PosX);
    Serial.print(DataX);
*/
    Tm0 = TmE;
  }
}
void PosLED(){
  unsigned long TmL = millis();
  if((PosX>1500)&&(PosX<2500)) digitalWrite(pLED[1], HIGH);
  else digitalWrite(pLED[1], LOW);
  
  if(detLe){
    if(PosX<1800) digitalWrite(pLED[0], HIGH);
    else digitalWrite(pLED[0], LOW);
    if(PosX>2200) digitalWrite(pLED[2], HIGH);
    else digitalWrite(pLED[2], LOW);
  }
  else{
    if((PosX<1800)&&((TmL-Tm1)>100)){
      digitalWrite(pLED[0], !digitalRead(pLED[0]));
      Tm1 = TmL;
    }
    if((PosX>2200)&&((TmL-Tm1)>100)){
      digitalWrite(pLED[2], !digitalRead(pLED[2]));
      Tm1 = TmL;
    } 
  }
  
}
void SAcoun(){
  pwm[0] = pwc[0]; // Save the period
  pwc[0] = 0; // Reset the timer
}
void SBcoun(){
  pwm[1] = pwc[1]; // Save the period
  pwc[1] = 0; // Reset the timer
}
void RPMctr(){
  for (byte i=0; i<2; i++){
    // Increase the time counter
    pwc[i]++;
    if (pwc[i] > (uSeg)){
      // Limit the timer & period
      pwc[i] = uSeg;
      pwm[i] = uSeg;
    }
  }  
}
void CalRPM(){
  unsigned long nwT = millis();
  // Calculations and prints every 10ms
  if ((nwT - prT) > 10){
//    char sRPM[10];
    prT = nwT;
    for (byte i=0; i<2; i++){
      //RPM
      // Protects math calculation
      ATOMIC()
      {
        // Detect Rotation Decrease
        if (pwc[i]>(pwm[i]*2)){
          pwm[i] *= 2;
          pwm[i] = constrain(pwm[i], 0, uSeg);
          pwc[i] = pwc[i]*2;
        }
        /* detects or not the
        rotation of the motors */
        if (pwm[i] < uSeg) rpm[i] = 6*uSeg/pwm[i]; // Detects rotation
        else if ((rpm[i] > 0)&&(pwm[i] == uSeg)) rpm[i] = int(rpm[i]/2); // No rotatiom
        
        // Limits the value of RPMs
        rpm[i] = constrain(rpm[i], 0, 9999);
      }
/*
      dtostrf(rpm[i], 4, 0, sRPM);
      Serial.print("  M");
      Serial.print(i);
      Serial.print(": ");
      Serial.print(sRPM);
*/
      // Print the RPMs
    }
/*    
    long RPMx = rpm[0] - rpm[1];
    dtostrf(RPMx, 4, 0, sRPM);
    Serial.print(" Delta: ");
    Serial.println(sRPM);
*/
  }
}
void CalPID(){
  P = PosX - 2000;
  I = P + pErr;
  D = P - pErr;
  
  PIDv = (Kp*P) + (Ki*I) + (Kd*D);
  pErr = P;
}
void MoCTRL(){
  int MoSpL = 0;
  int MoSpR = 0;
  if(detLe){
    MoSpL = SpBSE + PIDv;
    MoSpR = SpBSE - PIDv;
    
    MoSpL = constrain(MoSpL, 0, 255);
    MoSpR = constrain(MoSpR, 0, 255);
    
    digitalWrite(ML1, LOW);
    digitalWrite(ML2, HIGH);
    digitalWrite(MR1, LOW);
    digitalWrite(MR2, HIGH);
  }
  else{
    if(P==-2000){
      MoSpL = SpREV;
      MoSpR = SpFWD;
      digitalWrite(ML1, HIGH);
      digitalWrite(ML2, LOW);
      digitalWrite(MR1, LOW);
      digitalWrite(MR2, HIGH);
    }
    else if(P==2000){
      MoSpL = SpFWD;
      MoSpR = SpREV;
      digitalWrite(ML1, LOW);
      digitalWrite(ML2, HIGH);
      digitalWrite(MR1, HIGH);
      digitalWrite(MR2, LOW);
    }
  }
  char vals[28]; 
  sprintf(vals,"  L %i  R %i", MoSpL, MoSpR);
  Serial.print(P);
  Serial.println(vals);
  analogWrite(SML, MoSpL);
  analogWrite(SMR, MoSpR);
}
Schematic Circuit




Dual Twister Ramp (Handwork)

3 12 2012

This is a big versión (59 cm tall).

DSC02608

DSC02532

DSC02567

DSC02601

DSC02588

DSC02658

DSC02620

In this scale, the handwork is some different, like I show it in this video:

In the next video, I show the DTR + in action:





Dual Twister Ramp + (Design)

2 12 2012

This is the last design of a DTR.  I add 3 levels between the floor and ceiling. This levels are interconnected with this two ramps.

Rampa003

This is the video of the last design of the Twister Dual Ramp +:

This is a render:

Rampa013w

This is template in PDO format : Rampa06x.pdo (Zip file)

Rampa014

This is PDF version file:

Rampa013





Abba Cab from Carmageddon Paper Cars

26 11 2011

This is another model that you chose my little boy of 14 that are available on a page of Carmageddon papercrafts, with respective links to download.

This is the direct link to download the Abba Cab:

Both the game’s website are for anyone 18 years, the role models are beautiful. While most are simple, so much that my son picked as the Big Dump (A3), need instructions for assembling them.

For this reason I am not 100% that should unite parts where they were assembled.

These are some pictures where I show the assembly process:


These are some pictures where you can see the finished model including a couple of buddies as passengers:


Greetings 🙂





Thumper Cutie from disneyfamily.com

22 11 2011

This model is quite simple, ideal for children. Again my son decided to make another model, cutting, folding and gluing.

This is the Thumper Cutie


This is the website where you can download the template in PDF format:

Thumper Cutie

This is the direct link to download the PDF template:


In the following pictures you can see the steps to be joining parts of the model.


This is the finished model:

This model can be used as finger puppets. In the website of origin, there are other models as bamby in the same style.
Hope you like it, and my son likes.





Dinos from PaperBox

19 11 2011

These are two models of dinosaurs that are obtained from http://paperboxworld.weebly.com/dinos.html page.

My son cut, fold and paste, with the minimum possible help (with six years old).

Plant must be careful with the scissors.  We recommend using special scissors for children and all under supervision.

The first is as follows:


This is the PDF template:

This is the finished model:

The second model is the following:


This is the PDF template:


This is the finished model:

See you next time 😉





Japan mourns (11-03-2011)

19 03 2011

One minute of silence
With my deepest respect, I make public my sorrow for all the people who perished in the disaster happened on the island of Japan.


My deepest condolences to all families of the victims. I hope your hearts be filled with peace and resignation, and will soon overcome this sad event.
DrakerDG





Super Mario (Model)

2 02 2011

From: Super Mario (Design)

Ok, I managed to finish the model of Super Mario (30 cm tall). I took a little longer than expected, since no size so hard it was to build the hands.

The following photos show the final result.

The following videos show how to put together all the parts to complete the papercraft model.

The model I serve as the basis for a 150cm tall. I expected a lot of work.

Templates

PDF: smario01b.pdf

PDO: 558.pdo

PDO (Mirror 1): 635.pdo

PDO (Mirror 2): Smario01B.zip

Pepakura Viewer (To PDO files): www.tamasoft.co.jp/…/viewer.html





Lego Man (Repair legs)

31 01 2011

From: Lego Man (Model)

The support legs to the hips, the effort did not tolerate for long time. The joints are very weak and broke.


These are two rolls of cardboard that will replace the original carriers, providing the strongest legs.

These are the two legs repaired.


In the following video, I show how new supports are developed and how they were placed in each leg and finish.

Another is .. Like New!





Super Mario (Design)

14 01 2011

Now it’s time is a classic character, this is Super Mario, in the version used in the Nintendo Wii game consoles.I first began to document with pictures I found on Google. Since I have extensive experience in 3D design from Metasequoia, I chose a symmetrical and there is also another view of the drawing, which in this case was a side view.These are the pictures chosen and although the image from the side view is not 100% fit with the image of the front view, is a good reference to achieve the approximate volume. 

The following images are renders made from Metasequoia, which shows the dimensions achieved in 3D based on 2D views and different views on the design of the finished model.

The following video shows some details of the design in 3D from Metasequoia.
With 3D design, made in Metasequoia, load it in Pepakura to unfold the design and to design templates, ordering all parties and grouped by color.
The following file is the template group in both color and black and white:

Original file format is PDO, can be downloaded from the following link: Smario01B.pdo(zip file).
Following video shows how the pieces fit together, from different parts of the model of Super Mario in Pepakura software.
Now is time to bring the colored paper to print the template group and then cut, foldand glue and create the papercraft design model of Super Mario.







%d bloggers like this: