Robobug / Hexapod Walking and Following Obstacles

/******************************************************************
Created with PROGRAMINO IDE for Arduino
Project     : MAKERFACTORY Ultrasonic Demo
Author      : UlliS

Description : 
The demo shows how the MAKERFACTORY can follow a obstacles in front of the robot 
with the Pan & Tilt unit and an ultrasonic sensor HC-SR04.

Userboard is ARDUINO UNO
---------------------------
- IR Daten      7
- SU1           6 Pan 
- SU2           5 Tilt
- SU3           9 +/- WS2812
- A0            US Trigger
- A1            US Echo
- A4            free              
- A5            free
- 10            free

******************************************************************/

#include <Adafruit_NeoPixel.h> // https://github.com/adafruit/Adafruit_NeoPixel
#ifdef __AVR__
    #include <avr/power.h>
#endif

#include <Servo.h>

// Arduino
#define ARDUINO
#include <Hexapod_Lib.h>

/******************************************************************************
WS2812 RGB-LED
******************************************************************************/
// WS2812/13 (Neopixel) smart RGB LED
#define WS2812_PIN      SU3

// Parameter 1 = number of pixels in strip
// Parameter 2 = Arduino pin number (most are valid)
// Parameter 3 = pixel type flags, add together as needed:
// NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
// NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
// NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
// NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
// NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)

Adafruit_NeoPixel pixel = Adafruit_NeoPixel(1, WS2812_PIN, NEO_GRB + NEO_KHZ800);

/******************************************************************************
HC-SR04 ULTRASONIC SENSOR
******************************************************************************/
// pins for ultrasonic sensor
#define SR04_TRG_PIN    A0
#define SR04_ECHO_PIN   A1

/******************************************************************************
SERVO PAN & TILT
******************************************************************************/
// declare your servo as an object
#define SERVO_PAN_PIN   SU2
#define SERVO_TILT_PIN  SU1
Servo servo_pan;
Servo servo_tilt;

#define PAN_CENTER      100 // lower value scanner more left
#define TILT_CENTER     95  // lower value scanner more up

#define DIST_MIN    10
#define DIST_MAX    20
#define DIST_STOP   60

/******************************************************************************
SETUP
******************************************************************************/
void setup()
{
    // WS2812 RGB-LED    
    pixel.begin();  // this initializes the NeoPixel library.
    WS2812_SET_RGB(255,255,255);

    // high-Z for the audio output
    pinMode(PA_PIN,INPUT);
    digitalWrite(PA_PIN,LOW);

    // switches T1 and T2
    pinMode(T1,INPUT);
    pinMode(T2,INPUT);

    // HC-SR04
    pinMode(SR04_TRG_PIN, OUTPUT);
    pinMode(SR04_ECHO_PIN, INPUT);

    // open serial communications and wait for port to open:
    Serial.begin(SERIAL_STD_BAUD);
    while(!Serial);

    // set the data rate for the SoftwareSerial port (User-Board to Locomotion-Controller)
    SERIAL_CMD.begin(SERIAL_CMD_BAUD);

    // reset the Locomotion-Controller
    ROBOT_RESET();
    delay(250);
    ROBOT_RESET();
    delay(150);
    ROBOT_RESET();

    // wait for Boot-Up
    delay(1500);
    ROBOT_INIT();

    // pan & tilt servos
    servo_pan.attach(SERVO_PAN_PIN);
    servo_tilt.attach(SERVO_TILT_PIN);
    servo_pan.write(PAN_CENTER);
    servo_tilt.write(TILT_CENTER);

    // rnd init
    randomSeed(analogRead(0));

    // RBG LED
    for(int i = 0; i < 765; i++)
    {
        WS2812_ColorSweep(i);
        delay(2);
    }
    WS2812_SET_RGB(0,255,0); // green

    // print a hello world over the USB connection
    Serial.println("> Hello here is the MAKERFACTORY Robobug");   
}

/******************************************************************************
MAIN LOOP
******************************************************************************/
void loop()
{
    // start demo
    if(!digitalRead(T1)) 
    {
        delay(50);
        if(!digitalRead(T1)) 
        {
            MSound(1, 100, 1000);

            // basic robot movement parameter
            ROBOT_PWR_ON();
            delay(1000);
            ROBOT_SPEED(30);
            ROBOT_HEIGHT(40);

            while(1)
            {  
                int dist = SR04_DISTANCE();
                if (dist < DIST_MIN)
                {
                    WS2812_SET_RGB(255,0,0); // red
                    ROBOT_WALK_BWD();              
                }
                else if (dist > DIST_MAX && dist < DIST_STOP)
                {
                    WS2812_SET_RGB(0,0,255); // blue
                    ROBOT_WALK_FWD();                   
                }
                else if (dist < DIST_MAX && dist > DIST_MIN)
                {
                    WS2812_SET_RGB(0,255,0); // green
                    ROBOT_STOP();
                }
                else
                    ROBOT_STOP();

                delay(500);             

            }

        }
    }
}

/******************************************************************************
HC-SR04 FUNCTIONS
******************************************************************************/
int SR04_DISTANCE()
{
    long duration = 0;
    int distance  = 0;

    // AVG value (2 samples)
    for (int i = 0; i < 2; i++)
    {
        // Clears the trigPin
        digitalWrite(SR04_TRG_PIN, LOW);
        delayMicroseconds(2);

        // Sets the trigPin on HIGH state for 10 micro seconds
        digitalWrite(SR04_TRG_PIN, HIGH);
        delayMicroseconds(10);
        digitalWrite(SR04_TRG_PIN, LOW);

        // Reads the echoPin, returns the sound wave travel time in microseconds
        duration = pulseIn(SR04_ECHO_PIN, HIGH);

        // Calculating the avg distance
        distance += duration*0.034/2;
        delay(5);

    }

    return round(distance/2); // cm

}

/******************************************************************************
WS2812 RGB-LED FUNCTIONS
******************************************************************************/
void WS2812_ColorSweep(int i)
{
    // WS2812 color change
    // Farbwerte mit Vorbesetzung, begonnen wird mit rot
    static int red   = 255;
    static int green = 0;
    static int blue  = 0;

    if (i < 255)      // Phase 1: von rot nach grün
    {
        red--;       // red down
        green++;     // green up
        blue = 0;    // blue low
    }
    else if (i < 510) // Phase 2: von grün nach blau
    {
        red = 0;     // red low
        green--;     // green down
        blue++;      // blue up
    } 
    else if (i < 766) // Phase 3: von blau nach rot
    {
        red++;       // red up
        green = 0;   // green low
        blue--;      // blue down
    }

    pixel.setPixelColor(0, pixel.Color(red, green, blue)); 
    pixel.show();
}

void WS2812_SET_RGB(byte r, byte g, byte b)
{
    // WS2812 color change
    pixel.setPixelColor(0, pixel.Color(r, g, b)); 
    pixel.show();
}