![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
s2 project 1st try
Dependencies: TB6612 HCSR04 UoY-serial
Diff: main.cpp
- Revision:
- 5:57196959be02
- Parent:
- 4:137b119e5198
--- a/main.cpp Tue May 11 18:35:32 2021 +0000 +++ b/main.cpp Sun May 08 20:12:11 2022 +0000 @@ -1,80 +1,71 @@ #include "mbed.h" -#include "hcsr04.h" +#include "HCSR04.h" +#include "tb6612.h" +//Library for controlling ultrasonic module HCSR04 +//Ported by hiawoood from arduino library orgininally created by ITead studio. +HCSR04 sensorF(D7,D2); //Create sensor object of type HCSR04 (class borrowed HCSR04.cpp), with pins D2 and D3 assigned to trig and echo respectively +HCSR04 sensorL(D12,D13); +//HCSR04 sensorR(D9,D10); +TB6612 motorL(D9,D10,D11); +TB6612 motorR(D3,D4,D5); +Thread turnL; +Thread turnR; +Thread turnN; -HCSR04 hcsr04(D2,D3); //Create hcsr04 object of type HCSR04 (class made in hcsr04.cpp), with pins D2 and D3 assigned to trig and echo respectively -DigitalOut beep(D4); //using code made for motor modified to be just a pwm object, create beep of this type and assign D4 to it, this can then be used to control beep speed -DigitalIn gButton(BUTTON1); +void turnLFN() { + motorL.setSpeed(-1); + motorR.setSpeed(1); + thread_sleep_for(1500); + } + +void turnRFN() { + motorR.setSpeed(-1); + motorL.setSpeed(1); + thread_sleep_for(1500); + } +void turnNFN() { + motorL.setSpeed(1); + motorR.setSpeed(1); + } + +//long distanceL = sensorL.distance(CM); +int turn = 5000; int main() { + long distanceF; + long distanceL; + while(true) { + distanceF = sensorF.distance(CM); + distanceL = sensorL.distance(CM); - if (gButton == 0) { - hcsr04.distance(); - while (gButton == 1) { + if (distanceL<10 && distanceF>10) { + /*motorL.setSpeed(0.50); + motorR.setSpeed(0.50); + printf ("forward \r\n");*/ + turnN.start(turnNFN); + } - if (hcsr04.distance() <= 15) { //retrive the distance value and compare against a condition, depending on condition it meets print a message and set the beep speed - printf("Uncomfortably Close \r\n"); - beep = true; - thread_sleep_for(100); - } else { - if (hcsr04.distance() <= 30) { - printf("Very Close \r\n"); - beep = true ; - thread_sleep_for(100); - beep = false; - thread_sleep_for(100); - } else { - if (hcsr04.distance() <= 60) { - printf("Close \r\n"); - beep = true; - thread_sleep_for(100); - beep = false; - thread_sleep_for(200); - } else { - if (hcsr04.distance() <= 100) { - printf("Safe \r\n"); - beep = true; - thread_sleep_for(100); - beep = false; - thread_sleep_for(400); - } else { - if (hcsr04.distance() <= 200) { - printf("Far \r\n"); - beep = true; - thread_sleep_for(100); - beep = false; - thread_sleep_for(700); - } else { - if (hcsr04.distance() <= 300) { - printf("Very Far \r\n"); - beep = true; - thread_sleep_for(100); - beep = false; - thread_sleep_for(1200); - } else { - if (hcsr04.distance() <= 400) { - printf("Distant \r\n"); - beep = false; - } else { - if (hcsr04.distance() > 400) { - printf("Out of Range \r\n"); - beep = false; - } - } - } - } - } - } - } - } - } - } else { - beep = false; + if (distanceL>=10) { + /*motorL.setSpeed(-0.50); + motorR.setSpeed(0.50); + printf ("turn left \r\n"); + thread_sleep_for(turn); + + motorL.setSpeed(0.50); + motorR.setSpeed(0.50); + thread_sleep_for(300);*/ + turnL.start(turnLFN); } - //thread_sleep_for(500); //put the thread to sleep for 500ms so the distance isn't read uncontrollably fast. + + if (distanceL<10 && distanceF<10) { + /*motorL.setSpeed(0.50); + motorR.setSpeed(-0.50); + printf("turn right \r\n"); + thread_sleep_for(turn);*/ + turnR.start(turnRFN); + } } -} - -//AAAAAAAAAAAAAAAAAa +} \ No newline at end of file