first edition of the code for a wall-following robot

Dependencies:   motorController HC-SR04 UoY-serial

main.cpp

Committer:
snapo
Date:
2021-11-03
Revision:
0:e8a382e50908

File content as of revision 0:e8a382e50908:

#include "mbed.h"
#include "HC-SR04.h"
#include "motor.h"

#define uint unsigned int

motor motor1 (D1, D2, D3);
motor motor2 (D8, D9, D10);

HCSR04 ultraSoundF ( D5 , D6);
HCSR04 ultraSoundL ( D12 , D13);
HCSR04 ultraSoundR ( D14 , D15);

int main(){
    
    uint uSReadingF = 0;
    uint uSReadingL = 0;
    uint uSReadingR = 0;
    
    while(true){
        
        ultraSoundF.fastTimeReading();
        uSReadingF = ultraSoundF.getTime();
        ultraSoundL.fastTimeReading();
        uSReadingL = ultraSoundL.getTime();
        ultraSoundR.fastTimeReading();
        
       
        if (uSReadingF > 450){
            motor1 = 0.5;
            motor2 = 0.5;
            } else if (uSReadingF > 300) {
                motor1 = 0.2;
                } else {
                    motor1 = 0;
                    }
        
        
        if (uSReadingL > 300){
            motor1 = -0.5;
            motor2 = 0.5;
            thread_sleep_for(500);
            motor1 = 0;
            motor2 = 0;
            ultraSoundF.fastTimeReading();
            if (ultraSoundF.getTime() > 450){
                motor1 = 0.5;
                motor2 = 0.5;
                }
            }
        
    
        thread_sleep_for(500);
        motor1 = 0;
        motor2 = 0;
        thread_sleep_for(3000);
        
        }           
}