farhansadaf

Untitled

Oct 19th, 2021
873
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // Pin 1(RX) & Pin 2(TX) used for Serial -> Bluetooth
  2.  
  3. /*     -- Sonar --     */
  4. const int trigPin = 7, echoPin = 6;     // For ultrasound sensor
  5. int distance = -1;
  6. bool isDistanceSent = false;
  7.  
  8. const int loopPeriod = 250;    // a period of 250ms = a frequency of 4Hz || Checks distance in every -> loopPeriod
  9. unsigned long timeLoopDelay = 0;
  10.  
  11. /*     -- Motor --     */
  12. const int enA = 10, in1 = A3, in2 = A2;      // Motor A connections
  13. const int enB = 11, in3 = A1, in4 = A0;      // Motor B connections
  14.  
  15. /*     -- Variables --     */
  16. int motorDirection = 0, motorSpeed = 0, motorRunFor = 3, dirX = 0, dirY = 0;
  17. float speedX = 0, speedY = 0;
  18. bool isSpeechDelayDone = false;
  19.  
  20. void setup() {
  21.   /*     -- Sonar --     */
  22.   pinMode(trigPin, OUTPUT);
  23.   pinMode(echoPin, INPUT);
  24.  
  25.   /*     -- Motor --     */
  26.   pinMode(enA, OUTPUT);
  27.   pinMode(enB, OUTPUT);
  28.   pinMode(in1, OUTPUT);
  29.   pinMode(in2, OUTPUT);
  30.   pinMode(in3, OUTPUT);
  31.   pinMode(in4, OUTPUT);
  32.  
  33.   Serial.begin(9600);
  34. }
  35.  
  36. void loop() {
  37.   isDistanceSent = false;
  38.  
  39.   if (isSpeechDelayDone) {
  40.     Stop();
  41.     isSpeechDelayDone = false;
  42.   }
  43.  
  44.   if (Serial.available()) {
  45.     String command = Serial.readStringUntil('#');
  46.     if (command.length() == 9) {    // Joystick or Gesture
  47.       if (command.substring(0, 1).toInt() == 1) {
  48.         // Joystick
  49.         /* Format : 1[signX]XXX[signY]YYY
  50.            e.g. 100201005 means X = +20 and Y = -5*/
  51.         speedX = command.substring(2, 5).toFloat();
  52.         speedY = command.substring(6, 9).toFloat();
  53.         motorSpeed = sqrt((speedX*speedX) + (speedY*speedY));
  54.         setMotorSpeed(map(motorSpeed, 0, 100, 90, 255));
  55.  
  56.         dirX = command.substring(1, 2).toInt();
  57.         dirY = command.substring(5, 6).toInt();
  58.  
  59.         if (dirY == 1) forward();
  60.         else if (dirY == 0) backward();
  61.         else if (dirX == 0) right();
  62.         else if (dirX == 1) left();
  63.       }
  64.       else {    // Gesture
  65.         /* Format : 2[signX]XXX[signY]YYY
  66.            e.g. 210910015 means X = -91 and Y = +15*/
  67.         speedX = command.substring(2, 5).toFloat();
  68.         speedY = command.substring(6, 9).toFloat();
  69.         motorSpeed = sqrt((speedX*speedX) + (speedY*speedY));
  70.         setMotorSpeed(map(motorSpeed, 0, 100, 90, 255));
  71.  
  72.         dirX = command.substring(1, 2).toInt();
  73.         dirY = command.substring(5, 6).toInt();
  74.  
  75.         if (dirY == 1) forward();
  76.         else if (dirY == 0) backward();
  77.         else if (dirX == 1) right();
  78.         else if (dirX == 0) left();
  79.       }
  80.       sendDistanceAndWait(10);
  81.       isDistanceSent = true;
  82.     }
  83.    
  84.     if (command.length() == 4) {    // Stop or Voice
  85.       // Stop
  86.       if (command == "0000") Stop();
  87.  
  88.       // Voice
  89.       motorDirection = command.substring(0, 1).toInt();
  90.       motorRunFor = command.substring(1, 4).toInt();      // delay time (second)
  91.       setMotorSpeed(255);
  92.  
  93.       if (motorDirection == 5) forward();
  94.       else if (motorDirection == 6) right();
  95.       else if (motorDirection == 7) backward();
  96.       else if (motorDirection == 8) left();
  97.      
  98.       sendDistanceAndWait(motorRunFor * 1000);
  99.       isDistanceSent = true;
  100.  
  101.       isSpeechDelayDone = true;
  102.     }
  103.   }
  104.  
  105.   if (!isDistanceSent) sendDistance();
  106. }
  107.  
  108. float getDistance() {
  109.   // Returns distance in cm
  110.   /*  Sends out an 8 cycle sonic burst from the transmitter (Trig Pin),
  111.       which then bounces of an object and hits the receiver (Echo Pin)  */
  112.   digitalWrite(trigPin, LOW);
  113.   delayMicroseconds(2);
  114.   digitalWrite(trigPin, HIGH);
  115.   delayMicroseconds(10);
  116.   digitalWrite(trigPin, LOW);
  117.  
  118.   float duration = pulseIn(echoPin, HIGH);
  119.   return (duration*.0343)/2;
  120. }
  121.  
  122. void sendDistance() {
  123.   // Checks distance every 250ms
  124.   if (millis() - timeLoopDelay >= loopPeriod) {
  125.     distance = round(getDistance());
  126.     timeLoopDelay = millis();
  127.   }
  128.  
  129.   if (distance != -1) {
  130.     int len = String(distance).length();
  131.     if (len < 4) {
  132.       Serial.print("abc"[len - 1]);
  133.       Serial.print(distance);
  134.       Serial.print('#');
  135.     }
  136.   }
  137. }
  138.  
  139. void sendDistanceAndWait(int ms) {
  140.   unsigned long timeStart = millis();
  141.   while (millis() - timeStart <= ms) {
  142.     sendDistance();
  143.   }
  144. }
  145.  
  146. void Stop() {
  147.   digitalWrite(in1, LOW);
  148.   digitalWrite(in2, LOW);
  149.   digitalWrite(in3, LOW);
  150.   digitalWrite(in4, LOW);
  151. }
  152.  
  153. void forward() {
  154.   digitalWrite(in1, HIGH);
  155.   digitalWrite(in2, LOW);
  156.   digitalWrite(in3, HIGH);
  157.   digitalWrite(in4, LOW);
  158. }
  159.  
  160. void backward() {
  161.   digitalWrite(in1, LOW);
  162.   digitalWrite(in2, HIGH);
  163.   digitalWrite(in3, LOW);
  164.   digitalWrite(in4, HIGH);
  165. }
  166.  
  167. void left() {
  168.   digitalWrite(in1, LOW);
  169.   digitalWrite(in2, LOW);
  170.   digitalWrite(in3, HIGH);
  171.   digitalWrite(in4, LOW);
  172. }
  173.  
  174. void right() {
  175.   digitalWrite(in1, HIGH);
  176.   digitalWrite(in2, LOW);
  177.   digitalWrite(in3, LOW);
  178.   digitalWrite(in4, LOW);
  179. }
  180.  
  181. void setMotorSpeed(int value) {
  182.   // PWM(0-255)
  183.   analogWrite(enA, value);
  184.   analogWrite(enB, value);
  185. }
RAW Paste Data