example code

Dependencies:   mbed HCSR04

main.cpp

Committer:
abdeokar23
Date:
2019-11-18
Revision:
95:a82ac1cd5f6a
Parent:
88:bea4f2daa48c

File content as of revision 95:a82ac1cd5f6a:

#include "mbed.h"
#include "hcsr04.h"

Serial pc(USBTX,USBRX);

HCSR04  csensor(A0, A1);    //Trigger, Echo
HCSR04  lsensor(A2, A3);    //Trigger, Echo
HCSR04  rsensor(A4, A5);    //Trigger, Echo

PwmOut motor_l1(D13);
PwmOut motor_l2(D12);
PwmOut motor_r1(D11);
PwmOut motor_r2(D10);

unsigned int dist;

void initialise()
{
    motor_l1.write(0);
    motor_r1.write(0);
}

void forward()
{
    motor_l1.period_ms(50);
    motor_l1.write(0.75f);
    motor_l2.write(0);
    motor_l2.period_ms(50);
    motor_r1.write(0.75f);
    motor_r2.write(0);
}
void stop()
{
    motor_l1.write(0);
    motor_l2.write(0);
    motor_r1.write(0);
    motor_r2.write(0);
}

int cping()
{
    csensor.start();
    wait_ms(200);
    return(csensor.get_dist_cm());
}
int lping()
{
    lsensor.start();
    wait_ms(200);
    return(lsensor.get_dist_cm());
}
int rping()
{
    rsensor.start();
    wait_ms(200);
    return(rsensor.get_dist_cm());
}

// main() runs in its own thread in the OS
int main()
{
    int cdistance, ldistance, rdistance;                 //Variable to store distance from an object
    initialise();
    while (true)
    {
        cdistance = cping();   // ping function
        ldistance = lping();   // ping function
        rdistance = rping();   // ping function
        if (cdistance <20 || ldistance <20 || rdistance <20)
        {
            pc.printf("\n c-cm:%ld, l-cm:%ld, r-cm:%ld\n halting",cdistance, ldistance, rdistance);
            wait_ms(100);
            stop();
        }
        else
        {
            pc.printf("\n no object");
            forward();
            wait_ms(100);
        }
    }
}