Show page

Differences

This shows you the differences between two versions of the page.

--- pm:prj2021:abasoc:dariuscell [2021/06/03 04:27]
radu.nichita [Software Design]
+++ pm:prj2021:abasoc:dariuscell [2021/06/03 16:54] (current)
radu.nichita [Software Design]
@@ Line 82: / Line 82: @@
 <spoiler Cod Arduino>
 <code cpp>
-#include <AFMotor.h>
+#include <SoftwareSerial.h>
-#include <NewPing.h>
-#include <Servo.h>
-#define TRIG_PIN A4
-#define ECHO_PIN A5
-#define MAX_DISTANCE 200
-#define MAX_SPEED 190 // sets speed of DC  motors
-#define MAX_SPEED_OFFSET 20
-NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
+#define DELTA_SPEED 10 // increase / decrease in speed;
+#define MIN_SPEED_ALLOWED 120
+#define BALANCE_DIFFERENCE 17
-AF_DCMotor motor1(1, MOTOR12_1KHZ);
+#define SPEED 127
-AF_DCMotor motor2(3, MOTOR12_1KHZ);
+#define SPEED_TURNING 100
-Servo myservo;
+#define UP_DIRECTION 'w'
+#define DOWN_DIRECTION 's'
+#define STOP_DIRECTION 'q'
+#define LEFT_DIRECTION 'a'
+#define RIGHT_DIRECTION 'd'
-boolean goesForward=false;
+int In3 = 8; // left wheel - int3
-int distance = 100;
+int In4 = 9; // left wheel - int4
-int speedSet = 0;
+int In2 = 12; // right wheel - int2
+int In1 = 13; // right wheel - int1
+int ENA = 5; // left wheel
+int ENB = 6; // right wheel
+int robot_speed = 0;
-void setup() {
-  myservo.attach(9);
+bool goesForward = false;
-  myservo.write(115);
+bool moving = false;
-  delay(2000);
+int rx_pin = 4; // pin 4 as RX
-  distance = readPing();
+int tx_pin = 2; // pin 2 as TX
-  delay(100);
-  distance = readPing();
-  delay(100);
+SoftwareSerial BTSerial(tx_pin, rx_pin);
-  distance = readPing();
-  delay(100);
+char bt_data;
-  distance = readPing();
+String arduino_data;
-  delay(100);
+void setup()
+{
+  pinMode(In1, OUTPUT);
+  pinMode(In2, OUTPUT);
+  pinMode(ENA, OUTPUT);
+  pinMode(In3, OUTPUT);
+  pinMode(In4, OUTPUT);
+  pinMode(ENB, OUTPUT);
+  digitalWrite(In1, LOW);
+  digitalWrite(In2, HIGH);
+  digitalWrite(In3, LOW);
+  digitalWrite(In4, HIGH);
+  analogWrite(ENA, robot_speed);
+  analogWrite(ENB, robot_speed);
+  BTSerial.begin(9600);
+  Serial.begin(9600);
+  delay(1000);
 }
-void loop() {
- int distanceR = 0;
- int distanceL =  0;
- delay(40);
- if(distance<=15)
- {
-  moveStop();
-  delay(100);
-  moveBackward();
-  delay(300);
-  moveStop();
-  delay(200);
-  distanceR = lookRight();
-  delay(200);
-  distanceL = lookLeft();
-  delay(200);
-  if(distanceR>=distanceL)
+void moveForward() {
-  {
-    turnRight();
+ if(!goesForward || !moving) {
-    moveStop();
-  }else
-  {
-    turnLeft();
     moveStop();
+    goesForward = true;
+    moving = true;
+    digitalWrite(In1, LOW);
+    digitalWrite(In2, HIGH);
+    digitalWrite(In3, LOW);
+    digitalWrite(In4, HIGH);
+    for (; robot_speed < SPEED; robot_speed +=2) {
+      analogWrite(ENA, robot_speed + BALANCE_DIFFERENCE);
+      analogWrite(ENB, robot_speed);
+      delay(5);
+   }
   }
- }else
- {
-  moveForward();
- }
- distance = readPing();
 }
-int lookRight()
+void moveStop() {
-{
+    for (; robot_speed > 0; robot_speed -=2) {
-    myservo.write(50);
+      analogWrite(ENA, robot_speed + BALANCE_DIFFERENCE);
-    delay(500);
+      analogWrite(ENB, robot_speed);
-    int distance = readPing();
+      delay(5);
-    delay(100);
+   }
-    myservo.write(115);
+    robot_speed = 0;
-    return distance;
+    analogWrite(ENA, robot_speed);
+    analogWrite(ENB, robot_speed);
+    moving = false;
 }
-int lookLeft()
+void moveBackward() {
-{
+ if(goesForward || !moving) {
-    myservo.write(170);
+    moveStop();
-    delay(500);
+    moving = true;
-    int distance = readPing();
+    goesForward=false;
-    delay(100);
+    digitalWrite(In1, HIGH);
-    myservo.write(115);
+    digitalWrite(In2, LOW);
-    return distance;
+    digitalWrite(In3, HIGH);
-    delay(100);
+    digitalWrite(In4, LOW);
+    for (; robot_speed <SPEED; robot_speed +=2) {
+      analogWrite(ENA, robot_speed + BALANCE_DIFFERENCE);
+      analogWrite(ENB, robot_speed);
+      delay(5);
+   }
+  }
 }
-int readPing() {
+void turnLeft() {
-  delay(70);
+  if (moving) {
-  int cm = sonar.ping_cm();
+    analogWrite(ENA, 0);
-  if(cm==0)
+    analogWrite(ENB, SPEED);
-  {
+    delay(300);
-    cm = 250;
+    robot_speed = SPEED;
+    for (; robot_speed > 0; robot_speed -=2) {
+      analogWrite(ENB, robot_speed);
+      delay(5);
+   }
+    analogWrite(ENA, 0);
+    analogWrite(ENB, 0);
+    moving = false;
   }
-  return cm;
 }
-void moveStop() {
-  motor1.run(RELEASE);
-  motor2.run(RELEASE);
-  }
-v
 void turnRight() {
-  motor1.run(FORWARD);
+  if (moving) {
-  motor2.run(BACKWARD);
+    analogWrite(ENA, SPEED);
-  delay(300);
+    analogWrite(ENB, 0);
-  motor1.run(FORWARD);
+    delay(300);
-  motor2.run(FORWARD);
+    robot_speed = SPEED;
-}
+    for (; robot_speed > 0; robot_speed -=2) {
+      analogWrite(ENA, robot_speed);
-void turnLeft() {
+      delay(5);
-  motor1.run(BACKWARD);
+   }
-  motor2.run(FORWARD);
+    analogWrite(ENA, 0);
-  delay(300);
+    analogWrite(ENB, 0);
-  motor1.run(FORWARD);
+    moving = false;
-  motor2.run(FORWARD);
+  }
 }
+void loop() {
+  if (BTSerial.available()) {
+    bt_data = BTSerial.read();
+    if (bt_data == UP_DIRECTION) {
+      moveForward();
+    } else if (bt_data == DOWN_DIRECTION) {
+      moveBackward();
+    } else if (bt_data == STOP_DIRECTION){
+      moveStop();
+    } else if (bt_data == LEFT_DIRECTION) {
+      turnLeft();
+    } else if (bt_data == RIGHT_DIRECTION) {
+      turnRight();
+    }
+  }
+}
 </code>