Kontrola UT390B laserskog mjerača udaljenost preko arduina

I prvi kod

#include <SoftwareSerial.h>
#define PowerPin 3

SoftwareSerial mySerial = SoftwareSerial(5, 4);    //!< RX is 0. Pin 4 is TX
long inLoop = 0;

int strstart_P(const char *s1, const char * PROGMEM s2)
{
    return strncmp_P(s1, s2, strlen_P(s2)) == 0;
}

int getdist(void)
{
    char buf[64];
    char *comma;
    int dist;
    int rc;
    inLoop = millis();
    while (millis()-inLoop < 5000) {
        mySerial.print("*004545#");
        rc = mySerial.readBytesUntil('\n', buf, sizeof(buf));
        buf[rc] = '\0';
        if (!strstart_P(buf, PSTR("Dist: ")))
            continue;
        comma = strchr(buf, ',');
        if (comma == NULL)
            continue;
        *comma = '\0';
        dist = atoi(buf + strlen_P(PSTR("Dist: ")));
        return dist;
    }
}
void setup(void)
{
    Serial.begin(115200);
    pinMode(5,INPUT);
    pinMode(PowerPin,OUTPUT);
    digitalWrite(PowerPin,LOW);
 //   Serial.println(digitalRead(PowerPin));
    mySerial.begin(115200);
}
void loop(void)
{
   digitalWrite(PowerPin,HIGH);
   delay(1000);
   digitalWrite(PowerPin,LOW);
   Serial.println("Start Automatic Messure");
    int dist_mm;
    int dist_m;
    char buf[128];
    dist_mm = getdist();
    dist_m = dist_mm / 1000;
    snprintf_P(buf, sizeof(buf),
        PSTR("Laser distance: %d.%dm"),
        dist_m, dist_mm % 1000);
    Serial.println(buf);
    delay(1000);
    mySerial.print("r");
    delay(10000);
}