Tomasz Ostrzeniewski
Created September 13, 2019

Smars Quadruped

IntermediateProtip14
Smars Quadruped

Things used in this project

Hardware components

Wemos D1 Mini
Espressif Wemos D1 Mini
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
×8
Ultrasonic Sensor - HC-SR04 (Generic)
Ultrasonic Sensor - HC-SR04 (Generic)
×1
SparkFun Pi Servo HAT
SparkFun Pi Servo HAT
×1

Code

program-smars-quadropod.ino

C/C++
#include <Blynk.h>

#include <HCSR04.h>

UltraSonicDistanceSensor distanceSensor(D6, D7);  // Initialize sensor that uses digital pins 13 and 12.

/*Wemos Sterownik
 * D1    SCL
 * D2     SDA 
 * 3V3   - VCC
 * 5V     - 5V
 */


#define WIFI_NAME "..."
#define WIFI_PASSWORD "..."
#define DEVICE_ID 1
#define DEVICE_NAME "..."
#define TOKEN "..."

#include <RemoteMe.h>
#include <RemoteMeSocketConnector.h>
#include <ESP8266WiFi.h>

RemoteMe& remoteMe = RemoteMe::getInstance(TOKEN, DEVICE_ID);

//*************** CODE FOR CONFORTABLE VARIABLE SET *********************

inline void setWave(int32_t i) {remoteMe.getVariables()->setInteger("wave", i); }

//*************** IMPLEMENT FUNCTIONS BELOW *********************





#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

#define MIN_PULSE_WIDTH       650
#define MAX_PULSE_WIDTH       2350
#define DEFAULT_PULSE_WIDTH   1500
#define FREQUENCY             50


// our servo # counter
uint8_t servonum = 0;
int zmiana=90;


int v0;
int v1;
int v2;
int v3;
int v4;
int v5;
int v6;
int v7;
int v8;
int v9;
int v10;
int v12;
int voice;



void onWaveChange(int32_t i) {
    Serial.printf("onWaveChange: i: %d\n",i);
    wave();
}


int lewezmiana=90;
int prawazmiana=90;

#define BLYNK_PRINT Serial


#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>

// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "b522def33e9c427098a5f00019040395";

// Your WiFi credentials.
// Set password to "" for open networks.
char ssid[] = "RENIFER 2.4 ODU";
char pass[] = "Mjc4OEQ1";
void setup()
{
 

     WiFi.begin(WIFI_NAME, WIFI_PASSWORD);

    while (WiFi.status() != WL_CONNECTED) {
        delay(100);
    }

    remoteMe.getVariables()->observeInteger("wave" ,onWaveChange);

    remoteMe.setConnector(new RemoteMeSocketConnector());

    remoteMe.sendRegisterDeviceMessage(DEVICE_NAME);
    
    
  // Debug console
  Serial.begin(9600);
    Serial.begin(9600);
  Serial.println("8 channel Servo test!");

  pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  
  ruch(5,115);
  ruch(7,115);
  ruch(9,115);
  ruch(11,115);
  
  //     
  lewezmiana=45;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
       pwm.setPWM(4, 0, lewezmiana); //45 przod 135 tyl
        lewezmiana=135;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
       pwm.setPWM(6, 0, lewezmiana); //135 przod 45 tyl
        lewezmiana=135;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
       pwm.setPWM(8, 0, lewezmiana); //45 przod 135 tyl

       lewezmiana=45;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
       pwm.setPWM(10, 0, lewezmiana); //135 przod 45 tyl

//
    
  delay(1000);
  
  Blynk.begin(auth, ssid, pass);
}


// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz //60
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution  //4096
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert to us
  pulse /= pulselength;
  Serial.println(pulse);

  pwm.setPWM(n, 0, pulse);

}



void lewosetpwm(int nrserwa,int kat,int opoznienie)
{
      servonum=nrserwa;
     lewezmiana=kat;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
     pwm.setPWM(servonum, 0, lewezmiana);

}
void prawosetpwm(int nrserwa,int kat,int opoznienie)
{
     kat=180-kat;
      servonum=nrserwa;
     lewezmiana=kat;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
     pwm.setPWM(servonum, 0, lewezmiana);

}
void ruch(int servo,int kat)
{
  if(servo==5||servo==4||servo==8||servo==11)
  {
    lewezmiana=kat;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
       pwm.setPWM(servo, 0, lewezmiana); 
  }
  else
  {
     lewezmiana=180-kat;
     lewezmiana  = map(lewezmiana,0, 180, SERVOMIN, SERVOMAX);
      pwm.setPWM(servo, 0, lewezmiana); 
  }
}
void przod()
{
  ruch(6,90);
  delay(90);
  ruch(10,90);
  
  delay(90);
  ruch(7,45);
  ruch(6,45);
  delay(90);
  ruch(7,115);
delay(90);
ruch(4,90);
ruch(10,135);
ruch(9,45);
delay(90);
ruch(8,90);
ruch(9,115);
//
delay(90);
//faza 2
ruch(5,45);
delay(90);
ruch(4,45);
delay(90);
ruch(5,115);
delay(90);
ruch(6,90);
ruch(8,135);
ruch(11,45);
delay(90);
ruch(10,90);
ruch(11,115);
delay(90);
}
void nowyprzod()
{
  defaultpoz();
   delay(90);
   
   ruch(7,90);
   ruch(9,90);
   ruch(6,15);
   ruch(8,105);
   delay(90);
    ruch(4,75);
   ruch(10,165);
   delay(90);
   ruch(7,115);
   ruch(9,115);
    delay(90);
   ruch(6,45);
   ruch(8,135);
      delay(90);
   ruch(4,45);
   ruch(10,135);
   delay(90);
//1 faza
   ruch(5,90);
   ruch(11,90);
    delay(90);
   ruch(4,15);
   ruch(10,105);
      delay(90);
    ruch(6,75);
   ruch(8,165);
   delay(90);
   ruch(5,115);
   ruch(11,115);
    delay(90);
  ruch(6,45);
   ruch(8,135);
      delay(90);
   ruch(4,45);
   ruch(10,135);
   delay(90);

   

   
   
}
void tyl()
{
  ruch(8,90);
delay(90);
  ruch(4,90);
  
delay(90);
  ruch(9,45);
  ruch(8,135);
delay(90);
  ruch(9,115);
delay(90);
ruch(10,90);
ruch(4,45);
ruch(7,45);
delay(90);
ruch(6,90);
ruch(7,115);
//
delay(90);
//faza 2
ruch(11,45);
delay(90);
ruch(10,135);
delay(90);
ruch(11,115);
delay(90);
ruch(8,90);
ruch(6,45);
ruch(5,45);
delay(90);
ruch(4,90);
ruch(5,115);
delay(90);


  
}
void nowytyl()
{
  defaultpoz();
   delay(90);
  
   ruch(9,90);
   ruch(7,90);
   ruch(8,165);
   ruch(6,75);
   delay(90);
   ruch(10,105);
   ruch(4,15);
   delay(90);

    ruch(9,115);
   ruch(7,115);
   delay(90);
   ruch(8,135);
   ruch(6,45);
    delay(90);
   ruch(4,45);
   ruch(10,135);
   delay(90);
   //1 faza
   
    ruch(5,90);
   ruch(11,90);
   delay(90);
   ruch(10,165);
   ruch(4,75);
   delay(90);
   ruch(8,105);
   ruch(6,15);
   delay(90);

    ruch(5,115);
   ruch(11,115);
   delay(90);
   ruch(8,135);
   ruch(6,45);
    delay(90);
   ruch(4,45);
   ruch(10,135);
   delay(90);
   

   
   
}
void defaultpoz()
{
ruch(4,45);
ruch(6,45);
ruch(8,135);
ruch(10,135);
ruch(5,115);
ruch(7,115);
ruch(9,115);
ruch(11,115);
  
}
void prawo()
{


 defaultpoz();
delay(90);
ruch(10,90); 
ruch(7,45);
delay(90);
ruch(6,90);
delay(90);
ruch(7,115);

delay(90);
ruch(4,90);
ruch(6,45);
ruch(8,180);
ruch(11,45);
delay(90);
ruch(10,180);
delay(90);
ruch(11,115);

delay(90);
ruch(9,45);
delay(90);
ruch(8,135);
delay(90);
ruch(9,115);
delay(90);
ruch(4,45);
ruch(10,135);
delay(90);
}
void noweprawo()
{

defaultpoz();
delay(90);
ruch(6,15);
ruch(8,165);
delay(90);
ruch(5,90);
ruch(11,90);
delay(90);
ruch(4,15);
ruch(10,165);
delay(90);
ruch(5,115);
ruch(11,115);
delay(90);
ruch(4,45);
ruch(10,135);
delay(90);
ruch(7,90);
ruch(9,90);
delay(90);
ruch(6,45);
ruch(8,135);
delay(90);
ruch(7,115);
ruch(9,115);

}
void nowelewo()
{

defaultpoz();
delay(90);
ruch(4,15);
ruch(10,165);
delay(90);
ruch(7,90);
ruch(9,90);
delay(90);
ruch(6,15);
ruch(8,165);
delay(90);
ruch(7,115);
ruch(9,115);
delay(90);
ruch(6,45);
ruch(8,135);
delay(90);
ruch(5,90);
ruch(11,90);
delay(90);
ruch(4,45);
ruch(10,135);
delay(90);
ruch(5,115);
ruch(11,115);

}

void lewo()
{

 defaultpoz();
delay(90);
ruch(8,90);
ruch(5,45);
delay(90);
ruch(4,90);
delay(90);
ruch(5,115);

delay(90);
ruch(9,45);
delay(90);
ruch(4,45);
ruch(6,90);
ruch(10,180);
ruch(8,180);
delay(90);
ruch(9,115);

delay(90);
ruch(11,45);
delay(90);
ruch(10,135);
delay(90);
ruch(11,115);
delay(90);
ruch(8,135);
ruch(6,45);
delay(90);

}
void skladanie()
{
 defaultpoz();
 ruch(4,90);
 ruch(6,90);
 ruch(8,90);
 ruch(10,90);
 ruch(5,180);
 ruch(7,180);
 ruch(9,180);
 ruch(11,180);
}
void lookup()
{
ruch(11,45);
delay(90);
ruch(10,170);
delay(90);
ruch(11,90);
delay(90);
ruch(9,45);
delay(90);
ruch(8,170);
delay(90);
ruch(11,45);
delay(90); 
ruch(5,170);
ruch(7,170); 
}
void wave()
{
  ruch(8,90);
  delay(90);
  ruch(5,45);
  delay(90);
  ruch(4,5);
  delay(90);
  ruch(5,115);
  delay(90);

  ruch(7,45);
  delay(90);
  ruch(6,5);
  delay(90);
  ruch(6,45);
  delay(90);
  ruch(6,5);
  delay(90);
  ruch(6,45);
  delay(90);
  ruch(6,115);
}
void laydown()
{
 ruch(5,45);
 delay(100);
  ruch(7,45);
  delay(100);
    ruch(9,45);
    delay(100);
      ruch(11,45);
      delay(100);
  
  
}
void odleglosc()
{
   
}
BLYNK_WRITE(V0){
v0=param.asInt();
if(v0==1)
{
  nowyprzod();
}
}

BLYNK_WRITE(V1){
 v1=param.asInt();
if(v1==1)
{
  nowelewo();
}
}

BLYNK_WRITE(V2){
 v2=param.asInt();
if(v2==1)
{
  noweprawo();
}
}

BLYNK_WRITE(V3){
 v3=param.asInt();
if(v3==1)
{
  skladanie();
}
}

BLYNK_WRITE(V4){
 v4=param.asInt();
if(v4==1)
{
  defaultpoz();
}
}


BLYNK_WRITE(V5){
 v5=param.asInt();
if(v5==1)
{
  nowytyl();
}
}


BLYNK_WRITE(V6){
 v6=param.asInt();
if(v6==1)
{
  lookup();
}
}

BLYNK_WRITE(V7){
 v7=param.asInt();
if(v7==1)
{
  wave();
}
}
BLYNK_WRITE(V8){
 v8=param.asInt();
if(v8==1)
{
  laydown();
}
}

BLYNK_WRITE(V10){
 v10=param.asInt();
if(v10==1)
{
  odleglosc();
}
}
 BLYNK_WRITE(V12){
 v12=param.asInt();
if(v12==1)
{
  Blynk.virtualWrite(V11, distanceSensor.measureDistanceCm());
}
}

BLYNK_WRITE(V9){
 v9=param.asInt();
if(v9==1)
{
  voice=1;
}
else
{
  voice=0;
}
}

void loop() {
Blynk.run();
    float pomiar = distanceSensor.measureDistanceCm();
          Blynk.virtualWrite(V11, pomiar);
   if(v10==1)
   {
    // Serial.println(distanceSensor.measureDistanceCm());
    if(pomiar<=15)
    {
      nowytyl();
    }
    if(pomiar>=50)
    {
      nowyprzod();
    }
   }
   
 if(voice==1)
 {
  remoteMe.loop();
 }
 
 if(v0==0&&v1==0&&v2==0&&v3==0&&v5==0&&v6==0&&v8==0&&v12==0&&v10==0)
 {
  defaultpoz();
 }



}

Credits

Tomasz Ostrzeniewski

Tomasz Ostrzeniewski

0 projects • 0 followers

Comments