From a734728aac2707efb6628ec476b4b3e6417979ac Mon Sep 17 00:00:00 2001
From: Alexandertp <sandertp@gmail.com>
Date: Wed, 29 Nov 2023 16:00:54 +0100
Subject: [PATCH] Integrate motor control

---
 arduino/arduino.ino | 62 ++++++++++++++++++++++++++++++++++-----------
 1 file changed, 47 insertions(+), 15 deletions(-)

diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index e7c90ef..dece6d2 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -8,12 +8,17 @@ MKRIoTCarrier carrier;
 WiFiSSLClient wifi_client;
 MqttClient mqtt_client(wifi_client);
 
-extern const char *wifi_ssid;
-extern const char *wifi_password;
+// Motor pins
+int motor_pin1 = 8;  // Blue   - 28BYJ48 pin 1
+int motor_pin2 = 9;  // Pink   - 28BYJ48 pin 2
+int motor_pin3 = 10; // Yellow - 28BYJ48 pin 3
+int motor_pin4 = 11; // Orange - 28BYJ48 pin 4
 
-extern const char *mqtt_server;
-extern const char *mqtt_username;
-extern const char *mqtt_password;
+// Motor variables
+int motor_speed = 1200;
+int motor_count = 0;
+int motor_count_max = 256;
+int motor_lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};
 
 void setup(void)
 {
@@ -52,10 +57,23 @@ void setup(void)
 	mqtt_client.subscribe("dispense");
 
 	carrier.display.fillScreen(0x000);
+
+  //pinouts til motor
+  pinMode(motor_pin1, OUTPUT);
+  pinMode(motor_pin2, OUTPUT);
+  pinMode(motor_pin3, OUTPUT);
+  pinMode(motor_pin4, OUTPUT);
 }
 
 void loop(void)
 {
+  // Rotate motor if needed
+  if (motor_count) {
+    motor_clockwise();
+    motor_count--;
+  }
+
+  // Receive MQTT messages
 	int message_size = mqtt_client.parseMessage();
 	if (message_size) {
 		Serial.print("Got message with topic: ");
@@ -64,17 +82,31 @@ void loop(void)
 		if (mqtt_client.messageTopic() == "dispense") {
 			Serial.println("Dispensing...");
 
-			// TODO Connect to motor
-
-			carrier.display.fillScreen(0x000);
-			carrier.display.setCursor(20, 100);
-			carrier.display.setTextSize(4);
-			carrier.display.print("Dispense");
-
-			delay(500);
-
-			carrier.display.fillScreen(0x000);
+			// Start rotating motor
+      motor_count = motor_count_max;
 		}
 	}
 }
 
+void motor_clockwise()
+{
+  for (int i = 7; i >= 0; i--) {
+    motor_set_output(i);
+    delayMicroseconds(motor_speed);
+  }
+}
+void motor_anticlockwise()
+{
+  for (int i = 0; i < 8; i++) {
+    motor_set_output(i);
+    delayMicroseconds(motor_speed);
+  }
+}
+
+void motor_set_output(int out)
+{
+  digitalWrite(motor_pin1, bitRead(motor_lookup[out], 0));
+  digitalWrite(motor_pin2, bitRead(motor_lookup[out], 1));
+  digitalWrite(motor_pin3, bitRead(motor_lookup[out], 2));
+  digitalWrite(motor_pin4, bitRead(motor_lookup[out], 3));
+}