Hi Dmitriy
Tnx for the fast reply!
- what do you mean by that? I have ping in my project but not via blynk it runs local and measures every 50mS. This I verified in my terminal there I see good measurment results.
- no is also slow
- I will paste below or attach to this post
- I have no local server
Tnx! Dennis
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
//For Blynk app to control this via Android phone
#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>
//for PING sensor
#include <Ticker.h> //to do PING measurements every x mS (similar to irq based method)
#include <NewPing.h>
// called this way, it uses the addres you add as an argument, my chinese clone board has 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
//not used right now but they are safe values for my system
#define SERVOMIN 325 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 500 // this is the 'maximum' pulse length count (out of 4096)
//I2C speeds to let the Servo controller work faster if required
#define _100KHZ 100000
#define _200KHZ 200000
#define _300KHZ 300000
#define _400KHZ 400000
//local wifi
#define SSID1 "ID"
#define PASS1 "PASS"
#define WIFI_TIMEOUT 30 //divided by 2 is the time in seconds we give the WiFi to connect before giving up
//variable to control the robots moving speed (it is a delay between the moves the legs do and is controlled using Blynk slider)
int DELAY_BETWEEN_MOVES = 5; //take large value to debug legs
//To this pin(s) a virtual BLynk button on Android app is assigned to
const int buttonPin = D3;
char auth[] = "2d32633c488842f0a7955e1263ee986f"; // You should get Auth Token in the Blynk App, go to the Project Settings (nut icon).
int joy_x = 512; //to control the robot via Blynk's Joystick widget. These 2 start values correspond with moving forward
int joy_y = 1023;
//Ping pins and max distance
#define TRIGGER_PIN D8 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN D7 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
//instantiate 1 ping
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int staticpos = 375; //these are the midpositions for all servos
int delta1 = 120; //controls how deep the robot goes down w.r.t "staticpos"
int delta2 = 50; //controls how high the robot rises w.r.t "staticpos"
int delta3 = 38; //controls deviation of hips w.r.t "staticpos"
//instantiate the ticker and define a function to run using the ticker
Ticker measure_distance; //measure with ping periodically
int distance = 0; //global variable to store measured distance, ticker wants void functions as argument so cannot return values
boolean autonomous = true;
void setup()
{
Serial.begin(9600);
pwm.begin();
/*DVEDEBUG the next piece of code only works for Arduino compatible boards not for ESP8266
//next command is to make the PWM controller work 4 times as fast (default I2C clock is 100kHz)
TWBR = ((F_CPU /(_200KHZ+1)) - 16) / 2; // Change the i2c clock to 200KHz
*/
//this line changes the I2C frequency for esp8266
twi_setClock(_200KHZ);
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
//DVEDEBUG not used right now, was for blynk first test
//pinMode(buttonPin, INPUT);
/*****************************the following is to use the robot independent from blynk connection, it prevents that the ESP keeps trying to connect to blynk while there is no wifi******/
//connection to blynk now managed by first trying to connect to wifi, skip blynk if no wifi is connected
WiFi.begin(SSID1, PASS1);
int counter = 0;
while (WiFi.status() != WL_CONNECTED && (counter <= WIFI_TIMEOUT))
{
delay(500);
Serial.print(".");
counter++;
}
Blynk.config(auth); // in place of Blynk.begin(auth, ssid, pass);
Blynk.connect(3333); // timeout set to 10 seconds and then continue without Blynk
/****************************end Blynk connection code*********************************************************************************************************************************/
//setup the ticker controlled function to do PINGs
measure_distance.attach_ms(50,do_ping); //do a ping measurement every x mS using ticker
//Set ZERO pos
zeropoz();
delay(3000);
//check if the robot is connected to blynk, if not then run in the loop() the autonomous program version instead of the remote controlled version via Blynk app
if(!Blynk.connected())
autonomous=true;
else
autonomous=false;
}
//this process is running constantly it does not need to be called in the loop()
//V1 is virtual pin 1 so it does not take a pin resource
BLYNK_WRITE(V1)
{
joy_x = param[0].asInt();
joy_y = param[1].asInt();
}
//this process is running constantly too and on V0 (virtual pin 0) it gets the slider position from my Blynk app, here it is assigned to a variable and used in the code
//to control the robots speed
BLYNK_WRITE(V0)
{
DELAY_BETWEEN_MOVES = param.asInt();
Serial.print("Sliderposition = ");
Serial.println(DELAY_BETWEEN_MOVES);
}
void loop()
{
if (autonomous == true)
{
forward();
}
else //we control the robot via Blynk remote control app
{
Blynk.run();
if(joy_y > 1015)
forward();
else if(joy_x > 1015)
turnleft();
else if(joy_y <= 10)
backward();
else if(joy_x <= 10)
turnright();
else
forward();
}
}
void zeropoz()
{
pwm.setPWM(0, 0, 375);
pwm.setPWM(1, 0, 375);
pwm.setPWM(2, 0, 375);
pwm.setPWM(3, 0, 375);
pwm.setPWM(4, 0, 375);
pwm.setPWM(5, 0, 375);
pwm.setPWM(6, 0, 375);
pwm.setPWM(7, 0, 375);
pwm.setPWM(8, 0, 375);
pwm.setPWM(9, 0, 375);
pwm.setPWM(10, 0, 375);
pwm.setPWM(11, 0, 375);
pwm.setPWM(12, 0, 375); //the servo which controls the scanning of the PING sensor
}
void walkpoz()
{
int pdelta1 = 60;
pwm.setPWM(0, 0, 375);
pwm.setPWM(1, 0, 375);
pwm.setPWM(2, 0, 375-pdelta1);
pwm.setPWM(3, 0, 375);
pwm.setPWM(4, 0, 375+pdelta1);
pwm.setPWM(5, 0, 375);
pwm.setPWM(6, 0, 375);
pwm.setPWM(7, 0, 375);
pwm.setPWM(8, 0, 375-pdelta1);
pwm.setPWM(9, 0, 375);
pwm.setPWM(10, 0, 375+pdelta1);
pwm.setPWM(11, 0, 375);
}
void forward()
{
Group1Up();
delay(DELAY_BETWEEN_MOVES);
Group2Bw();
delay(DELAY_BETWEEN_MOVES);
Group1Fw();
delay(DELAY_BETWEEN_MOVES);
Group1Down();
delay(DELAY_BETWEEN_MOVES);
Group2Up();
delay(DELAY_BETWEEN_MOVES);
Group1Bw();
delay(DELAY_BETWEEN_MOVES);
Group2Fw();
delay(DELAY_BETWEEN_MOVES);
Group2Down();
delay(DELAY_BETWEEN_MOVES);
}
void backward()
{
Group1Up();
delay(DELAY_BETWEEN_MOVES);
Group2Fw();
delay(DELAY_BETWEEN_MOVES);
Group1Bw();
delay(DELAY_BETWEEN_MOVES);
Group1Down();
delay(DELAY_BETWEEN_MOVES);
Group2Up();
delay(DELAY_BETWEEN_MOVES);
Group1Fw();
delay(DELAY_BETWEEN_MOVES);
Group2Bw();
delay(DELAY_BETWEEN_MOVES);
Group2Down();
delay(DELAY_BETWEEN_MOVES);
}
void turnright(void)
{
Group1Up();
delay(DELAY_BETWEEN_MOVES);
Group2Right();
delay(DELAY_BETWEEN_MOVES);
Group1Left();
delay(DELAY_BETWEEN_MOVES);
Group1Down();
delay(DELAY_BETWEEN_MOVES);
Group2Up();
delay(DELAY_BETWEEN_MOVES);
Group1Right();
delay(DELAY_BETWEEN_MOVES);
Group2Left();
delay(DELAY_BETWEEN_MOVES);
Group2Down();
delay(DELAY_BETWEEN_MOVES);
}
void turnleft(void)
{
Group1Up();
delay(DELAY_BETWEEN_MOVES);
Group2Left();
delay(DELAY_BETWEEN_MOVES);
Group1Right();
delay(DELAY_BETWEEN_MOVES);
Group1Down();
delay(DELAY_BETWEEN_MOVES);
Group2Up();
delay(DELAY_BETWEEN_MOVES);
Group1Left();
delay(DELAY_BETWEEN_MOVES);
Group2Right();
delay(DELAY_BETWEEN_MOVES);
Group2Down();
delay(DELAY_BETWEEN_MOVES);
}
//groups down dept is arranged by delta1 value and rise hight by delta12 value
void Group1Up()
{
for(int jk=375+delta2; jk>=375-delta1; jk--)
{
pwm.setPWM(1, 0, jk); //knee left back
pwm.setPWM(5, 0, jk); //knee left front
pwm.setPWM(9, 0, jk); //knee right middle
//delay(5);
}
}
void Group2Up()
{
for(int jk=375+delta2; jk>=375-delta1; jk--)
{
pwm.setPWM(3, 0, jk); //knee left middle
pwm.setPWM(7, 0, jk); //knee right front
pwm.setPWM(11, 0, jk); //knee righ back
//delay(5);
}
}
void Group1Down()
{
for(int jk=375-delta1; jk<=375+delta2; jk++)
{
pwm.setPWM(1, 0, jk); //knee left back
pwm.setPWM(5, 0, jk); //knee left front
pwm.setPWM(9, 0, jk); //knee right middle
//delay(5);
}
}
void Group2Down()
{
for(int jk=375-delta1; jk<=375+delta2; jk++)
{
pwm.setPWM(3, 0, jk); //knee left middle
pwm.setPWM(7, 0, jk); //knee right front
pwm.setPWM(11, 0, jk); //knee right back
//delay(5);
}
}
void Group1Fw()
{
//pwm.setPWM(0, 0, (staticpos - (delta3-10))); //hip left back //DVE this was finetuned: endvalue needs to be slightly higher so it goes less forward, prevent it from slamming to the hip left middle
pwm.setPWM(0, 0, (staticpos - delta3)); //hip left back
pwm.setPWM(4, 0, (staticpos - delta3)); //hip left front (was 500)
pwm.setPWM(8, 0, (staticpos + delta3)); //hip right middle
}
//this one was not worked out, I derived it from Group1Fw()
void Group2Fw()
{
pwm.setPWM(2, 0, (staticpos - delta3)); //hip left middle
pwm.setPWM(6, 0, (staticpos + delta3)); //hip right front
pwm.setPWM(10, 0, (staticpos + delta3)); //hip right back
}
void Group1Bw()
{
pwm.setPWM(0, 0, (staticpos + delta3)); //hip left back
//pwm.setPWM(4, 0, (staticpos + (delta3+10))); //hip left front //finetuned: goes slightly more backward then the others
pwm.setPWM(4, 0, (staticpos + delta3)); //hip left front
pwm.setPWM(8, 0, (staticpos - delta3)); //hip right middle
}
void Group2Bw()
{
//pwm.setPWM(2, 0, (staticpos + (delta3-10))); //hip left middle. //slightly finetuned to solve slamming into leg Left back by slightly making end value lower, makes it go less backward
pwm.setPWM(2, 0, (staticpos + delta3)); //hip left middle.
pwm.setPWM(6, 0, (staticpos - delta3)); //original was 375 (hip right front)
pwm.setPWM(10, 0, (staticpos - delta3));//original was 285 (hip right back)
}
void Group1Right() //go right is acomplished by moving the left 2 legs forward while the middle right leg goes backward
{
pwm.setPWM(0, 0, (staticpos - delta3)); //hip left back
pwm.setPWM(4, 0, (staticpos - delta3)); //hip left front (was 500)
pwm.setPWM(8, 0, (staticpos - delta3)); //hip right middle //to make the right middle leg go backward we need - delta3
}
void Group2Right() //to go right with group2 the 2 right legs of the group need to move backward and the 1 left needs to move forward
{
pwm.setPWM(2, 0, (staticpos - delta3)); //hip left middle //going forward now
pwm.setPWM(6, 0, (staticpos - delta3)); //original was 375 (hip right front)
pwm.setPWM(10, 0, (staticpos - delta3));//original was 285 (hip right back)
}
void Group1Left() //go left is acomplished by moving the left 2 legs backward while the middle right leg goes forward
{
pwm.setPWM(0, 0, (staticpos + delta3)); //hip left back
pwm.setPWM(4, 0, (staticpos + delta3)); //hip left front
pwm.setPWM(8, 0, (staticpos + delta3)); //hip right middle now going forward to make going left possible
}
void Group2Left() //to go right with group2 the 2 right legs of the group need to move forward and the 1 left needs to move backward
{
pwm.setPWM(2, 0, (staticpos + delta3)); //hip left middle, now moving backward ( + delta instead of -) so that the robot can go left
pwm.setPWM(6, 0, (staticpos + delta3)); //hip right front
pwm.setPWM(10, 0, (staticpos + delta3)); //hip right back
}
void do_ping(void)
{
distance = sonar.ping_cm();
}