Peguino Forum
The Peguino Bot 1 - Printable Version

+- Peguino Forum (https://www.peguino.com/chat)
+-- Forum: Community (https://www.peguino.com/chat/forum-18.html)
+--- Forum: Exhibition / Gallery (https://www.peguino.com/chat/forum-23.html)
+--- Thread: The Peguino Bot 1 (/thread-65.html)



The Peguino Bot 1 - Peguinohero - 07-02-2019

The Peguino Bot 1

The Peguino Bot 1 has the capability to follow a black line and can be controlled via WIFI!
See the robot in action at https://youtu.be/OJZcmLMYcnw

How to use:
  1. Connect to the WIFI PeguinoBot1 and use the password peguinobot1
  2. Open a webbrowser on your phone or pc's browser and type in the given IP address from the Peguino OLED Display. Usually 192.168.4.1 or type http://192.168.4.1


The Script
Code:
/*
 WIFI Remote Bot 1.8 Version 0.991
 (C) 2019 by Peguino LTD

 The Peguino Bot opens an access point to connect via WIFI.

 Connect the following Bricks
   Left Microprox Brick to port C3
   Right Microprox Brick to Port D1
   Left 360 deg Microservo Brick to C1
   Right 360 deg Microservo Brick to D3
   Button Brick to Port D2
   Buzzer Brick to Port C2
   Proximity Brick to Port A
   OLED96 Display Brick to Port B

 How to use:
 1. Connect to the WIFI PeguinoBot1 and use the password peguinobot1
 2. Open a webbrowser on your phone or pc's browser and type in the given IP address from the Peguino OLED Display. Usually 192.168.4.1 or type http://192.168.4.1
 Use the Serial Plotter and Monitor to check debug values


 Peguino Uno ESP 32 Development Board Pinout
           ┌─╥─┐
 3.3     1 │     │ 30  5V
 GND     2 │     │ 29  GND
 GPIO15  3 │     │ 28  GPIO13
 GPIO2   4 │     │ 27  GPIO12
 GPIO4   5 │     │ 26  GPIO14
 GPIO16  6 │     │ 25  GPIO27
 GPIO17  7 │     │ 24  GPIO26
 GPIO5   8 │     │ 23  GPIO25
 GPIO18  9 │     │ 22  GPIO33
 GPIO19 10 │     │ 21  GPIO32
 GPIO21 11 │     │ 20  GPIO35
 GPIO3  12 │     │ 19  GPIO34
 GPIO1  13 │     │ 18  GPIO39
 GPIO22 14 │     │ 17  GPIO36
 GPIO23 15 │     │ 16  Chip enable
           └───┘
 Visit https://www.peguino.com
*/



#include "WiFi.h"
#include "SSD1306Wire.h"
SSD1306Wire  display(0x3c, 21, 22);     // Pin 21 und 22 = I2C and on Connector Port B where the Peguino OLED96 brick is connected
//#include "OLEDPEGULOGO_01.h"          // contains the Peguino Logo as XBM file.
#define Microprox_Left 36               // Analog Input from GPIO 36 which is physically located on Pin 17 and available on Port C3 where the left Microprox Brick is connected
#define Microprox_Right 32              // Analog Input from GPIO 32 which is physically located on Pin 21 and available on Port D1 where the right Microprox Brick is connected
int valueLeft = 0;
int valueRight = 0;
int gochart = 0;                        // indicator for switch button in Serial Plotter
const int LEFT_SERVO = 27;              // GPIO Pin for the left wheel / Peguino 360Deg. Microservo
const int RIGHT_SERVO = 33;             // GPIO Pin for the right wheel / Peguino 360Deg. Microservo
char * ssid = "PeguinoBot1";            // The Peguino Bot SSID
char * password = "peguinobot1";        // The WIFI Password
int buttonPin = 25;                     // Port D2 (Pin 23  GPIO25) connects the Button Brick
int buttonState = LOW;                  // variable for reading the Button Brick status
int drivestatus = 0;                    // do the robot's wheels spinning?

int ledPinR = 13;                       // red    RGB LED Brick
int ledPinG = 12;                       // green  RGB LED Brick
int ledPinB = 14;                       // blue   RGB LED Brick
int ledChannelR = 13;                  
int ledChannelG = 12;
int ledChannelB = 14;
int LEDfreq = 500;                        

 int val = -1;
int servoSpeed = 25;
int servoSpeedLeft = 49;
String showiptext = "Pegubot IP: ";
String showip = "              ";
int servoSpeedRight = 51;
int trigPin = 2;                        // Trigger Signal for the Peguino Proximity Brick
int echoPin = 15;                       // Echo Signal for the Peguino Proximity Brick
long duration, cm;
int freq = 440;                         // the initial audio frequency in Hz for the Peguino Buzzer Brick
int whizchannel = 2;                    // ESP32 speciality
int resolution = 8;                     // ESP32 speciality - it means 8 Bits resolution to count from 0 - 255
int whizzer = 0;                        // the counter of whizz sound cycles
int whizz = 0;                          // the sound frequency we use
int reverseonly = 0;                    // is used to allow only to dive backwards in case a potential collision could be detected by the Proximity Brick
int nowdriving = 0;                     // is used to memorize that we drive right now....

WiFiServer server(80);

void setup() {
 Serial.begin(115200);
 delay(500);
 Serial.println("Start");
 Serial.setDebugOutput(true);
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
 display.init();
 display.flipScreenVertically();
 display.setFont(ArialMT_Plain_10);
 
 initServos();                          //  Initialise the Peguino 360Deg. Microservos

 // Wireless
 Serial.print("Creating Access Point");
 WiFi.softAPdisconnect();
 WiFi.mode(WIFI_MODE_AP);
 while (!WiFi.softAP(ssid, password)) {
   Serial.print(".");
 }
 Serial.println("");
 Serial.println("Network Created!");
 Serial.print("PeguinoBot IP address = ");
 Serial.println(WiFi.softAPIP());

 // Webserver
 Serial.print("Creating HTTP Server");
 server.begin();
 WiFi.printDiag(Serial);

 // Buzzer Brick
 ledcSetup(whizchannel, freq, resolution);   //ESP32 speciality - PWM function to control the Buzzer Brick
 ledcAttachPin(23, whizchannel);             //ESP32 speciality - attach it at Pin23

 display.clear();  // clean screen
 display.setFont(ArialMT_Plain_10);
 display.setTextAlignment(TEXT_ALIGN_LEFT);
 showip =  String(showiptext + WiFi.softAPIP().toString());
 display.drawStringMaxWidth(0, 0, 128, showip);
 display.display();
 Serial.println(showip);

 // RGB LED Brick
 ledcSetup(ledChannelR, LEDfreq, resolution);
 ledcAttachPin(ledPinR, ledChannelR);  
 ledcSetup(ledChannelG, LEDfreq, resolution);
 ledcAttachPin(ledPinG, ledChannelG);
 ledcSetup(ledChannelB, LEDfreq, resolution);
 ledcAttachPin(ledPinB, ledChannelB);
 
 ledcWrite(ledChannelR, 255);            // 255 = LED off, 0 = LED maximal brightness!
 ledcWrite(ledChannelG, 255);
 ledcWrite(ledChannelB, 255);

 // Button Brick
 pinMode(buttonPin, INPUT);              // initialize the Button Brick pin as an input
     ledcWrite(ledChannelR, 0);
   ledcWrite(ledChannelG, 255);
   ledcWrite(ledChannelB, 255);
}

// -------------------------------

void loop() {
 WiFiClient client = server.available();
 if (client) {
   // we have a new client sending some request
   String req = client.readStringUntil('\r');
   Serial.println(req);
   String response = processRequest(req);
   while (client.connected()) {
     if (client.available()) {
       String line = client.readStringUntil('\r');
       Serial.print(line);
       if (line.length() == 1 && line[0] == '\n') {
         client.println(prepareHtmlPage(response));
         break;
       }
     }
   }
   delay(1); // give the web browser time to receive the data

   // close the connection:
   client.stop();
   Serial.println("[Client disonnected]");
 }
 
 buttonState = digitalRead(buttonPin); // read the state of the Button Brick

 // now check if the Bbutton brick is pressed. If it is, the buttonState is HIGH:
 if (buttonState != HIGH) {

 } else {
   ledcWrite(ledChannelR, 255);
   ledcWrite(ledChannelG, 255);
   ledcWrite(ledChannelB, 0);
   delay (750);        // if you remove the 0.75 seconds waiting time you can see the bouncing spikes interfering the results                    
  if (drivestatus == 7)
     {
       drivestatus = 4;  // Stop
       ledcWrite(ledChannelR, 0);
       ledcWrite(ledChannelG, 255);
       ledcWrite(ledChannelB, 255);
     }
  else
     {
       drivestatus = 7;  // Start Linetracking
       // val = -7;
       reverseonly = 0;
       ledcWrite(ledChannelR, 255);
       ledcWrite(ledChannelG, 0);
       ledcWrite(ledChannelB, 255);
       whizzer = 0;                    // Startsound
       }                          
 }


 // Microprox reading
 valueLeft = analogRead(Microprox_Left);
 valueRight = analogRead(Microprox_Right);

 // Proximity sensor
 // The sensor triggers by a HIGH pulse of 10 or more microseconds - a short LOW pulse first ensures a clean HIGH pulse:
 digitalWrite(trigPin, LOW);
 delayMicroseconds(5);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);

 // Signal readout: The HIGH pulse duration is the time (in microseconds) from sending ping to receiving the echo bumping from an object.
 pinMode(echoPin, INPUT);
 duration = pulseIn(echoPin, HIGH);

 // Convert time of flight to distance
 cm = (duration / 2) / 29.1;   // Divide by 29.1 for cm and 74 for inches
 //Serial.print(cm * 2.2);             // Multiply by 2.2 to get a nice chart ....
 //Serial.print(",");

 char valueLeft_str[16];
 itoa(valueLeft, valueLeft_str, 10);   // converts integer number to string
 char valueRight_str[16];
 itoa(valueRight, valueRight_str, 10); // converts integer number to string
 if (cm > 50) { cm = 50;}              // everything above 50cm creates useless values for us here
 char cm_str[16];
 itoa(cm, cm_str, 10);                 // converts integer number to string

 display.clear();                      // clear the Peguino OLED96 display
 display.drawStringMaxWidth(0, 0, 128, showip);
 display.drawStringMaxWidth(0, 10, 128, cm_str );  
 display.drawStringMaxWidth(10, 10, 128, " cm");  
 display.drawStringMaxWidth(0, 40, 128, valueLeft_str );  
 display.drawStringMaxWidth(102, 40, 128, valueRight_str );
 
 if (cm <= 5 && reverseonly == 0) {
  // String response = processRequest("/servo/stop"); // BRAKE!!! Stop driving to avoid collision
   display.drawStringMaxWidth(44, 10, 128, "OBSTACLE!");
   ledcWrite(ledChannelR, 0);
   ledcWrite(ledChannelG, 255);
   ledcWrite(ledChannelB, 255);
 //  if (drivestatus != 7) {reverseonly = 1;}    // assumed we started somehow remote controlled
 //  if  (drivestatus == 7) {drivestatus = 4;}   // assumed we started as a linefinder and something is in front of us and lets as stop driving
 }
 // show the Microprox Sensors
 if (valueLeft > 3900) // black line is above 3900 - max is 4095
 {display.drawStringMaxWidth(0, 50, 128, "BLACK" );}
 if (valueRight > 3900) // black line is above 3900 - max is 4095
 {display.drawStringMaxWidth(94, 50, 128, "BLACK" );}
 
 // START DRIVE INFO (and some basic behaviour logic...)
 if ((val == -2) || drivestatus == 2 && reverseonly == 0)    // allows driving forward only if there is nothing in front of the Proximity Brick
 {  display.drawStringMaxWidth(36 ,20, 128, "FORWARD"); drivestatus = 2; nowdriving = 1;}  
 if ((val == -5) || drivestatus == 2 && reverseonly == 1 && nowdriving == 1)
 {  display.drawStringMaxWidth(36, 20, 128, "REVERSE"); drivestatus = 3; nowdriving = 0; reverseonly = 0;  }  // allows only to drive backwards in case something is in front of the Proximity Brick
 if ((val == -3) || drivestatus == 3)
 {  display.drawStringMaxWidth(36, 20, 128, "REVERSE"); drivestatus = 3; }
/* if ((val == -4) || drivestatus == 4)
 {  display.drawStringMaxWidth(44, 20, 128, "STOPPED"); drivestatus = 4; }  */
 if ((val == -5) || drivestatus == 5)
 {  display.drawStringMaxWidth(30, 20, 128, "TURN LEFT"); drivestatus = 5; }
 if ((val == -6) || drivestatus == 6)
 {  display.drawStringMaxWidth(30, 20, 128, "TURN RIGHT"); drivestatus = 6; }
  if ((val == -7) || drivestatus == 7 && reverseonly == 0)
 {  
   display.drawStringMaxWidth(22, 20, 128, "FOLLOW LINE"); drivestatus = 7; nowdriving = 1;
 }
 if (drivestatus == 4){
   //display.clear();
   display.drawStringMaxWidth(22, 20, 128, "Button STOP");
   servoWrite(0, 54); // left stop
   servoWrite(1, 50); // right stop
   ledcWrite(ledChannelR, 0);
   ledcWrite(ledChannelG, 255);
   ledcWrite(ledChannelB, 255);
   }
 
 if ((valueLeft > 3900) && (valueRight > 3900) && (drivestatus == 7))
 {
   // both Microprox sensors triggering, better to stop now...
   // String response = processRequest("/servo/stop"); // BRAKE!!! Stop driving to avoid collision
   display.drawStringMaxWidth(44, 10, 128, "2 BLACKS!");
   servoWrite(0, 54); // left stop
   servoWrite(1, 50); // right stop
   ledcWrite(ledChannelR, 0);
   ledcWrite(ledChannelG, 255);
   ledcWrite(ledChannelB, 255);
   // if (drivestatus != 7) {reverseonly = 1;}    // assumed we started somehow remote controlled
   drivestatus = 4;                              // assumed we started as a linefinder and something is in front of us and lets as stop driving
   }
  else {
   if ((valueLeft > 3900) && (drivestatus == 7)) // black line is above 3900 - max is 4095
   {
     display.drawStringMaxWidth(0, 50, 128, "BLACK" );
     // stop the left wheel, assume the right is still spinning it turns the driving bot to the left
       servoWrite(0, 54);      // 54 slow down the right wheel
       servoWrite(1, 54);      // 54
      //String response = processRequest("/servo/stop"); // BRAKE!!! Stop driving to avoid collision
       ledcWrite(ledChannelR, 255);
       ledcWrite(ledChannelG, 0);
       ledcWrite(ledChannelB, 128);
    }
   else if ((valueRight > 3900) && (drivestatus == 7)) // black line is above 3900 - max is 4095
   {
      display.drawStringMaxWidth(94, 50, 128, "BLACK" );
     // stop the right wheel, assume the right is still spinning it turns the driving bot to the left
       servoWrite(0, 58);      // 59
       servoWrite(1, 51);      // 50 slow down the left wheel
       ledcWrite(ledChannelR, 128);
       ledcWrite(ledChannelG, 0);
       ledcWrite(ledChannelB, 255);
   }
   else if (drivestatus == 7)
   {
     // everything is fine - keep going
       servoWrite(0, 57); // 59 right wheel     90 full speed, 56 halt range of 34 half 17; 90 82 75  68 62  58 || 50 or 54 = stop
       servoWrite(1, 53); //  54 left wheel (drive direction)
       ledcWrite(ledChannelR, 255);
       ledcWrite(ledChannelG, 0);
       ledcWrite(ledChannelB, 255);
   }
 
 }
 // STOP DRIVE INFO
 display.display();                    // (re)draw the Peguino OLED96 display

 // process Microprox input
 //Serial.print(valueLeft);              // the Left Microprox for plotting
 //Serial.print(",");                    // seperator
 //Serial.print(valueRight);             // the right Microprox for plotting
 //Serial.print(",");
 //Serial.println(gochart);              // the start button (low and high)
 //delay(5);
 // Buzzer Brick stuff
 while (whizz <= 3) {
   whizz += 1;
   for (int freq = 440; freq <= 2000; freq = freq + 30) {
     ledcWriteTone(whizchannel, freq);
     delay(5);
   }
   whizzer += 1;
 }
 if (whizzer == 4) {
   ledcWriteTone(whizchannel, -1);     //ESP32 speciality - turn off sound
   
 }
}

// process wifi input
String processRequest(String req) {
//  int val = -1;
 if (req.indexOf("/led/0") != -1) {
   val = 0;
 } else if (req.indexOf("/led/1") != -1) {
   val = 1;
 } else if (req.indexOf("/servo/forward") != -1) {
   val = -2;
 } else if (req.indexOf("/servo/reverse") != -1) {
   val = -3;
 } else if (req.indexOf("/servo/stop") != -1) {
   val = -4;
 } else if (req.indexOf("/servo/turnleft") != -1) {
   val = -5;
 } else if (req.indexOf("/servo/turnright") != -1) {
   val = -6;
 } else if (req.indexOf("/servo/follow") != -1) {
   val = -7;
 }

 if (val >= 0) {
   return "nope";
 } else {
   switch (val) {
     case -1:
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
     case -2: // forward
       servoWrite(0, 90);
       servoWrite(1, 90);
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
     case -3: // reverse
       servoWrite(0, 10); // 0
       servoWrite(1, 10); // 20
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
     case -4: // stop
       servoWrite(0, 54); // left
       servoWrite(1, 50); // right
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
     case -5: // turn left
       servoWrite(0, 75);
       servoWrite(1, 56);
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
     case -6: // turn right
       servoWrite(0, 56);
       servoWrite(1, 75);
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'> FORWARD AND FOLLOW A BLACK LINE</a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;
 case -7: // forward and follow the line
       servoWrite(0, 90);
       servoWrite(1, 90);
       return "<h2>Peguino Bot Remote Control</h2><br/><table class='buttons' cellpadding='10'><tr><td colspan='3'><a href='/servo/follow'><i> FORWARD AND FOLLOW A BLACK LINE </i></a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/forward'> FORWARD </a></td><td>&nbsp;</td></tr><tr><td><a href='/servo/turnleft'> TURN LEFT </a></td><td align='center'><a href='/servo/stop'> STOP </a></td><td><a href='/servo/turnright'> TURN RIGHT </a></td></tr><tr><td>&nbsp;</td><td><a href='/servo/reverse'> REVERSE </a></td><td>&nbsp;</td></tr></table>";
       break;  
   }
 }
 return "Something went wrong";
}


String prepareHtmlPage(String output) {
 String htmlPage =
   String("HTTP/1.1 200 OK\r\n") +
   "Content-Type: text/html\r\n" +
   "Connection: close\r\n" +           // the connection will be closed after completion of the response
   "\r\n" +
   "<!DOCTYPE HTML>" +
   "<html><style>html {color: black; font-family: sans-serif;} .buttons {border: 2px red solid;} .buttons td {border: 2px red solid; background-color: #9ceeff; padding-top: 25px; padding-bottom: 25px} a, a.visited { color: black; font-weight: bold; text-decoration: none;} </style><body>" +
   output +
   "</body></html>" +
   "\r\n";
 return htmlPage;
}

void initServos() {
 ledcSetup(0, 50, 16); // channel 0, 50 Hz, 16-bit depth because my servos do not stop on the same position thez are changed to 51 and 49!!!
 ledcAttachPin(LEFT_SERVO , 0);
 ledcSetup(1, 50, 16); // channel 1, 50 Hz, 16-bit depth
 ledcAttachPin(RIGHT_SERVO , 1);

}
void servoWrite(int channel, int dutyPercent) {
 int pulse = map(((channel == 0) ? 104 - dutyPercent : dutyPercent), 0, 100, 1500, 8000);
 ledcWrite(channel, pulse);
 // Serial.print("Wrote "); Serial.print(pulse); Serial.print(" to Servo "); Serial.println(channel);
}



Images

[Image: Peguinobot_01.jpg]



[Image: Peguinobot_01_drive_01.jpg]