From 579623f7abcbca4ef48fbc1afc7144f08915ab87 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Viktor=20R=C3=B6nnb=C3=A4ck?= <vikro145@student.liu.se>
Date: Sun, 19 Sep 2021 12:58:48 +0200
Subject: [PATCH] Final code for bluetooth_reciever_basic

---
 .../bluetooth_reciever_basic.ino              | 144 +++++++++---------
 super_basic_movement/super_basic_movement.ino |  54 ++++---
 2 files changed, 104 insertions(+), 94 deletions(-)

diff --git a/bluetooth_reciever_basic/bluetooth_reciever_basic.ino b/bluetooth_reciever_basic/bluetooth_reciever_basic.ino
index 2b60e5a..c2716a0 100644
--- a/bluetooth_reciever_basic/bluetooth_reciever_basic.ino
+++ b/bluetooth_reciever_basic/bluetooth_reciever_basic.ino
@@ -43,10 +43,12 @@ const int LEFT = 1;
 const int UP = 2;
 const int DOWN = 3;
 
-const int PWM_MAX = 150; // Max speed of motor (PWM) in range (0-255)
-const int PWM_TURN_SPEED = PWM_MAX / 2; // Max speed of motor (PWM) in range (0-255)
+// Speed constant
+const int PWM_MAX = 90; // Max speed of motor (PWM) in range (0-255)
+const int PWM_TURN_SPEED = 40; // Max speed of motor (PWM) in range (0-255)
 
-long int TIMEOUT = 250; // Timeout in ms until motors are disabled (after not recieving any command)
+
+long int TIMEOUT = 150; // Timeout in ms until motors are disabled (after not recieving any command)
 
 // Commands
 unsigned int SHORTEST_CMD_LEN = 6; // For optimisation purposes (shortest current command is <stop> : len = 6)
@@ -63,7 +65,6 @@ unsigned long timeLastCmd = 0;
 // Autonomous = distance command (ex: f3, forward 3 meters)
 int drivingState = 0;
 
-
 void setup() {
   Serial.begin(9600);
 
@@ -86,59 +87,57 @@ void setup() {
 }
 
 void loop() {
-  if (drivingState == 0) {
-    // Manual mode
-    bool isValidCommand = false;
-  
-    // Read Serial command char byte as a char (returns -1 if not available)
-    char cmd = Serial.read();
-  
-    // Run if valid serial data is read
-    if (cmd > 0) {
-      // If active command is not set
-      if (activeCommand == 0) {
-        activeCommand = cmd;
-  
-        // Execute command (returns false if it is an invalid command)
-        isValidCommand = executeCmd(cmd, false);
-  
-        //Serial.println("New command: " + cmd); // Doesnt always print perfectly
-      } else if (activeCommand != cmd) {
-        // If command has changed (and the new command is valid)
-        // De-execute previous command
-        executeCmd(activeCommand, true);
-  
-        // Set new command to be active command
-        activeCommand = cmd;
-  
-        // Execute new command (returns false if it is an invalid command)
-        isValidCommand = executeCmd(cmd, false);
-      } else {
-        //Serial.println("Same command: " + cmd);
-        timeLastCmd = millis();
-      }
-  
-      if (isValidCommand) {
-        // Update timout timer if valid command registered
-        timeLastCmd = millis(); // Time in ms since the program started
-  
-        //Serial.println("Reset timeout");
-      }
-    }
-  
-    if (activeCommand > 0 && (millis() - timeLastCmd) > TIMEOUT) {
-      // De-execute last command if time since it was last recieved exceeds timeout
-      // Maybe stop instead?
+  // Manual mode
+  bool isValidCommand = false;
+
+  // Read Serial command char byte as a char (returns -1 if not available)
+  char cmd = Serial.read();
+
+  // Run if valid serial data is read
+  if (cmd > 0) {
+    // If active command is not set
+    if (activeCommand == 0) {
+      activeCommand = cmd; // Set new active command
+
+      // Execute command (returns false if it is an invalid command)
+      isValidCommand = executeCmd(cmd, false);
+
+      Serial.println("New command");
+    } else if (activeCommand != cmd) {
+      // If command has changed (and the new command is valid)
+      // De-execute previous command
       executeCmd(activeCommand, true);
-      
-      //Serial.println("Command timeout! time: " + String(millis() - timeLastCmd));
-      //Serial.println(activeCommand);
-  
-      activeCommand = 0;
+
+      // Set new command to be active command
+      activeCommand = cmd;
+
+      // Execute new command (returns false if it is an invalid command)
+      isValidCommand = executeCmd(cmd, false);
+      Serial.println("Replace command!");
+      //Serial.println("New command, replace old");
+    } else {
+      timeLastCmd = millis();
+      Serial.println("Same command, reset timeout");
+    }
+
+    if (isValidCommand) {
+      // Update timout timer if valid command registered
+      timeLastCmd = millis(); // Time in ms since the program started
+
+      Serial.println("Valid command entered, reset timeout");
     }
-  } else if (drivingState == 1) {
-    // Autonomous mode
-    // TODO
+  }
+
+  if (activeCommand > 0 && (millis() - timeLastCmd) > TIMEOUT) {
+    // De-execute last command if time since it was last recieved exceeds timeout
+    // Maybe stop instead?
+    executeCmd(activeCommand, true);
+    
+    //Serial.println("Command timeout! time: " + String(millis() - timeLastCmd));
+    //Serial.println(activeCommand);
+    Serial.println("Command has timed out");
+
+    activeCommand = 0;
   }
 }
 
@@ -154,24 +153,24 @@ bool executeCmd(char cmd, bool clearCmd) {
   if (cmd == 's') {
     // Stop
     if (clearCmd) {
-      //Serial.println("Clearing stop!");
+      Serial.println("Clearing stop!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
     } else {
-      //Serial.println("Stopping robot!");
+      Serial.println("Stopping robot!");
       updateSpeed(0, RIGHT, false);
       updateSpeed(0, LEFT, false);
     }
   } else if (cmd == 'b') {
     // Backwards
     if (clearCmd) {
-      //Serial.println("Clearing backwards!");
+      Serial.println("Clearing backwards!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
       digitalWrite(RVS_RIGHT, LOW);
       digitalWrite(RVS_LEFT, LOW);
     } else {
-      //Serial.println("Going backwards!");
+      Serial.println("Going backwards!");
       updateSpeed(PWM_MAX, RIGHT, true);
       updateSpeed(PWM_MAX, LEFT, true);
     }
@@ -183,53 +182,53 @@ bool executeCmd(char cmd, bool clearCmd) {
       digitalWrite(EN_LEFT, LOW);
       digitalWrite(RVS_RIGHT, LOW);
     } else {
-      //Serial.println("Turning right!");
+      Serial.println("Turning right!");
       updateSpeed(PWM_TURN_SPEED, RIGHT, true);
       updateSpeed(PWM_TURN_SPEED, LEFT, false);
     }
   } else if (cmd == 'f') {
     // Forward
     if (clearCmd) {
-      //Serial.println("Clearing forward!");
+      Serial.println("Clearing forward!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
     } else {
-      //Serial.println("Going forward!");
+      Serial.println("Going forward!");
       updateSpeed(PWM_MAX, RIGHT, false);
       updateSpeed(PWM_MAX, LEFT, false);
     }
   } else if (cmd == 'l') {
     // Left
     if (clearCmd) {
-      //Serial.println("Clearing left!");
+      Serial.println("Clearing left!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
       digitalWrite(RVS_LEFT, LOW);
     } else {
-      //Serial.println("Turning left!");
+      Serial.println("Turning left!");
       updateSpeed(PWM_TURN_SPEED, RIGHT, false);
       updateSpeed(PWM_TURN_SPEED, LEFT, true);
     }
   } else if (cmd == 'o') {
     // Forward-left
     if (clearCmd) {
-     //Serial.println("Clearing forward-left!");
+     Serial.println("Clearing forward-left!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
     } else {
-      //Serial.println("Going forward-left!");
+      Serial.println("Going forward-left!");
       updateSpeed(PWM_MAX, RIGHT, false); // Max speed right
-      updateSpeed(PWM_MAX/4, LEFT, false); // Go 1/4 max speed with left wheel
+      updateSpeed(PWM_MAX/2, LEFT, false); // Go 1/4 max speed with left wheel
     }
   } else if (cmd == 'p') {
     // Forward-right
     if (clearCmd) {
-      //Serial.println("Clearing forward-right!");
+      Serial.println("Clearing forward-right!");
       digitalWrite(EN_RIGHT, LOW);
       digitalWrite(EN_LEFT, LOW);
     } else {
-      //Serial.println("Going forward-right!");
-      updateSpeed(PWM_MAX/4, RIGHT, false); // Go 1/4 max speed with right wheel
+      Serial.println("Going forward-right!");
+      updateSpeed(PWM_MAX/2, RIGHT, false); // Go 1/4 max speed with right wheel
       updateSpeed(PWM_MAX, LEFT, false); // Max speed left
     }
   }
@@ -242,6 +241,8 @@ bool executeCmd(char cmd, bool clearCmd) {
   return true;
 }
 
+
+
 /*
  * Function for updating motor speed
  * int motor_speed, is PWM value between 0-255
@@ -296,7 +297,6 @@ void toggleEnable(const int motor) {
 
   // Toggle enable
   digitalWrite(pin, LOW); // Disable motor
-  delay(1); // Wait 1 ms
-  
+  delay(1); 
   digitalWrite(pin, HIGH); // Enable motor
 }
diff --git a/super_basic_movement/super_basic_movement.ino b/super_basic_movement/super_basic_movement.ino
index 4dd760d..16cbb7c 100644
--- a/super_basic_movement/super_basic_movement.ino
+++ b/super_basic_movement/super_basic_movement.ino
@@ -19,7 +19,7 @@ const int LEFT = 1;
 const int UP = 2;
 const int DOWN = 3;
 
-const int PWM_MAX = 150; // Max speed of motor (PWM) in range (0-255)
+const int PWM_MAX = 100; // Max speed of motor (PWM) in range (0-255)
 const int PWM_TURN_SPEED = 50; // Max speed of motor (PWM) in range (0-255)
 
 
@@ -38,15 +38,13 @@ void setup() {
   pinMode(PWM_LEFT, OUTPUT);
 
   digitalWrite(RVS_RIGHT, LOW);
-  digitalWrite(STP_RIGHT, LOW);
   
   digitalWrite(RVS_LEFT, LOW);
-  digitalWrite(STP_LEFT, LOW);
 }
 
 void loop() {
   // Forward
-  updateSpeed(PWM_MAX, RIGHT, false);
+  /*updateSpeed(PWM_MAX, RIGHT, false);
   updateSpeed(PWM_MAX, LEFT, false);
 
   Serial.println("Going forward!");
@@ -59,31 +57,45 @@ void loop() {
   delay(2000);
 
   Serial.println("Turning left!");
-  // Turn left
+  // Go forward-left
   updateSpeed(PWM_MAX, RIGHT, false);
-  updateSpeed(PWM_MAX, LEFT, true);
-  
-  delay(2000);
+  updateSpeed(PWM_MAX/3, LEFT, false);*/
+  digitalWrite(RVS_RIGHT, HIGH);
+  digitalWrite(RVS_LEFT, HIGH);
+
+  Serial.println("Half speed forward 4 seconds");
+  analogWrite(PWM_RIGHT, PWM_MAX / 2);
+  analogWrite(PWM_LEFT, PWM_MAX / 2);
+  toggleEnable(RIGHT);
+  toggleEnable(LEFT);
+
+  delay(4000);
 
-  // Clear
   digitalWrite(EN_RIGHT, LOW);
   digitalWrite(EN_LEFT, LOW);
 
-  delay(2000);
+  delay(150);
   
-  Serial.println("Turning right!");
-  // Turn right
-  updateSpeed(PWM_MAX, RIGHT, true);
-  updateSpeed(PWM_MAX, LEFT, false);
+  Serial.println("Max speed forward 3 seconds");
+  analogWrite(PWM_RIGHT, PWM_MAX);
+  analogWrite(PWM_LEFT, PWM_MAX);
   
-  delay(2000);
+  toggleEnable(RIGHT);
+  toggleEnable(LEFT);
+
+  delay(3000);
+  
+  Serial.println("Stop 4 seconds");
+  analogWrite(PWM_RIGHT, 0);
+  analogWrite(PWM_LEFT, 0);
+  
+  toggleEnable(RIGHT);
+  toggleEnable(LEFT);
+
+  delay(4000);
   
-  // Clear
   digitalWrite(EN_RIGHT, LOW);
   digitalWrite(EN_LEFT, LOW);
-
-  delay(2000);
-
   Serial.println("Waiting 10 seconds!");
 
   delay(10000);
@@ -129,7 +141,7 @@ void updateSpeed(int motor_speed, const int motor, bool reverse) {
   }
 
   // Toggle enable for correct motor to update new PWM value
-  toggleEnable(motor);
+  //toggleEnable(motor);
 }
 
 /*
@@ -151,7 +163,5 @@ void toggleEnable(const int motor) {
 
   // Toggle enable
   digitalWrite(pin, LOW); // Disable motor
-  delay(1); // Wait 1 ms
-  
   digitalWrite(pin, HIGH); // Enable motor
 }
-- 
GitLab