Brain/sensory.neurobot.cpp at main · Web4application/Brain · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
#include <Servo.h>

// ----- MOTOR SETUP -----
Servo leftMotor;
Servo rightMotor;
int leftMotorPin = 9;
int rightMotorPin = 10;

// ----- SENSOR SETUP -----
int ultrasonicTrig = 7;
int ultrasonicEcho = 6;
float distance = 0;

void setup() {
  Serial.begin(115200);

  // Motors
  leftMotor.attach(leftMotorPin);
  rightMotor.attach(rightMotorPin);

  // Sensors
  pinMode(ultrasonicTrig, OUTPUT);
  pinMode(ultrasonicEcho, INPUT);
}

void loop() {
  // Read Ultrasonic Sensor
  digitalWrite(ultrasonicTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(ultrasonicTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(ultrasonicTrig, LOW);

  long duration = pulseIn(ultrasonicEcho, HIGH);
  distance = duration * 0.034 / 2; // cm

  // Send sensor data to Python
  Serial.println(distance);

  // Receive motor commands from Python
  if (Serial.available() > 0) {
    String command = Serial.readStringUntil('\n');
    if (command == "FORWARD") { moveForward(); }
    else if (command == "BACKWARD") { moveBackward(); }
    else if (command == "LEFT") { turnLeft(); }
    else if (command == "RIGHT") { turnRight(); }
    else if (command == "STOP") { stopMotors(); }
  }

  delay(50);
}

// ----- MOTOR FUNCTIONS -----
void moveForward() {
  leftMotor.write(180);
  rightMotor.write(0);
}

void moveBackward() {
  leftMotor.write(0);
  rightMotor.write(180);
}

void turnLeft() {
  leftMotor.write(0);
  rightMotor.write(0);
  rightMotor.write(0);
}

void turnRight() {
  leftMotor.write(180);
  rightMotor.write(180);
}

void stopMotors() {
  leftMotor.write(90);
  rightMotor.write(90);
}