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).
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);
}
PaperBotz 