ESP32

mail@pastecode.io avatar
unknown
c_cpp
2 years ago
6.0 kB
1
Indexable
Never
#include "BluetoothSerial.h"
 
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
 
BluetoothSerial SerialBT;
char message = '\0';
bool linetracking = false;
 
// Ultrasound
const int trigPin = 33;
const int echoPin = 32;
 
//define sound speed in cm/uS
#define SOUND_SPEED 0.034
 
long duration;
float distanceCm;
 
// Motor A
int motor1Pin1 = 27; 
int motor1Pin2 = 26; 
int enable1Pin = 14;
 
// Motor B
int motor2Pin1 = 5; 
int motor2Pin2 = 18; 
int enable2Pin = 19;
 
//Left sensor
int Left_Sensor = 34;
int Left_Sensor_Val = 0;
 
//Right sensor
int Right_Sensor = 35;
int Right_Sensor_Val = 0;
 
 
// Setting PWM properties
const int freq = 30000;
const int pwm1 = 0;
const int pwm2 = 1;
const int resolution = 8;
float dutyCycle = 200;
 
void forward(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);

  // Forward pin-arrangement
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, HIGH); 
  digitalWrite(motor2Pin1, LOW);
  digitalWrite(motor2Pin2, HIGH);
}
 
void backwards(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);
 
  // Backwards pin-arrangement
  digitalWrite(motor1Pin1, HIGH);
  digitalWrite(motor1Pin2, LOW); 
  digitalWrite(motor2Pin1, HIGH);
  digitalWrite(motor2Pin2, LOW); 
}
 
void left(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);
 
  // Left pin-arrangement
  digitalWrite(motor1Pin1, HIGH);digitalWrite(motor1Pin2, LOW); 
  digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, HIGH); 
}
 
void leftOnly(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);
 
  // Left Only pin-arrangement
  digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); 
  digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, HIGH); 
}
 
void right(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);
 
  // Right pin-arrangement
  digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, HIGH); 
  digitalWrite(motor2Pin1, HIGH);digitalWrite(motor2Pin2, LOW);  
}
void rightOnly(float velocity) {
  // Velocity multiplier
  ledcWrite(pwm1, dutyCycle*velocity);
  ledcWrite(pwm2, dutyCycle*velocity);
 
  // Right Only pin-arrangement
  digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, HIGH); 
  digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);  
}
 
void stopmotors() {
  // Stop pin-arrangement
  digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); 
  digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);  
}
 
void setup() {
  Serial.begin(115200);
  SerialBT.begin("Thomas The Tank Engine"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
 
  // sets the pins as outputs:
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(enable1Pin, OUTPUT);
 
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  pinMode(enable2Pin, OUTPUT);
 
  pinMode(Left_Sensor, INPUT_PULLUP);
  pinMode(Right_Sensor, INPUT_PULLUP);
 
 
  // configure LED PWM functionalitites
  ledcSetup(pwm1, freq, resolution);
  ledcSetup(pwm2, freq, resolution);
 
  // attach the channel to the GPIO to be controlled
  ledcAttachPin(enable1Pin, pwm1);
  ledcAttachPin(enable2Pin, pwm2);
 
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
 
  Serial.println("The device started, now you can pair it with bluetooth!");
 
}
 
void loop() {
  
  // Line-trackers
  Left_Sensor_Val = digitalRead(Left_Sensor);
  Right_Sensor_Val = digitalRead(Right_Sensor);
 
  if (SerialBT.available()) {
    char incomingChar = SerialBT.read();
 
    if (incomingChar == '\n'){ message = '\0';}
    else{message = incomingChar;}
 
    Serial.println(incomingChar);
 
    if (message == 'Y') {linetracking = true;} else if (message =='N') {linetracking = false;stopmotors();} else {}

  } 
  
  //Serial.println(linetracking);
  if (linetracking == false) {
 
    // Bluetooth Car Movement Commands
 
    switch (message) {
      case 'F': 
        forward(1);
        break;
      case 'B': 
        backwards(1);
        break;
     case 'L': 
        left(0.9);
        break;
     case 'R': 
        right(0.9);
        break;
     case 'S': 
       stopmotors();
        break;
      default:
        break;
    }
  } else if (linetracking == true) {
 
    //Serial.println(Left_Sensor_Val);
    //Serial.println(Right_Sensor_Val);

    // FORWARD
    if (Left_Sensor_Val == HIGH && Right_Sensor_Val == HIGH) {
      forward(0.8);
    }

    // LEFT
    if (Left_Sensor_Val == LOW && Right_Sensor_Val == HIGH) { 
      leftOnly(0.75);
    }

    // RIGHT
    if (Left_Sensor_Val == HIGH && Right_Sensor_Val == LOW) {
      rightOnly(0.75);
    }
 
  } else {
      stopmotors();
  }
 
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
 
  // Calculate the distance
  distanceCm = duration * SOUND_SPEED/2;
 
 
  // Prints the distance in the Serial Monitor
  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);
  SerialBT.println(distanceCm);
 
  if (distanceCm<10) {
    Serial.println("Oh shet");
    stopmotors();
  }

}