From 90e160228fa30ec038a1515cdd5637fae0081e89 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 31 Jan 2021 13:45:28 +0100
Subject: [PATCH] implement up down movement with sense print

---
 src/main.cpp | 74 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/src/main.cpp b/src/main.cpp
index c664126..44dcbb9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,6 +6,33 @@
 
 //follow firmware flash guide for new wemos motor shield v1.0  https://github.com/thomasfredericks/wemos_motor_shield
 
+#define BUTTON_DEBOUNCE 200
+#define PIN_BUTTON D5
+unsigned long last_pin_button=0;
+bool pin_button_down=0;
+bool last_pin_button_down=0;
+
+bool manual_drive_direction=false;
+
+#define PIN_SENSE A0
+unsigned long last_print=0;
+#define SENSE_CLEAR_LOWER 40 //adc value lower limit for clear part. clear is around 70
+#define SENSE_CLEAR_UPPER 100 //adc value upper limit for clear part
+
+#define SENSE_OPAQUE_LOWER 600 //adc value lower limit for opaque part. opaque is around 675
+#define SENSE_OPAQUE_UPPER 750 //adc value upper limit for opaque part
+
+#define SENSE_END_LOWER 850 //adc value lower limit for end marker
+#define SENSE_END_UPPER 1024 //adc value upper limit for end marker
+
+//define directions for motors
+#define _M1_UP = _CCW;
+#define _M1_DOWN = _CW;
+
+#define _M2_UP = _CCW;
+#define _M2_DOWN = _CW;
+
+int pwm;
 
 //Motor shield default I2C Address: 0x30
 //PWM frequency: 1000Hz(1kHz)
@@ -19,9 +46,55 @@ void setup() {
   Serial.begin(57600);
   Serial.println("Starting demo");
 
+  pinMode(PIN_BUTTON, INPUT_PULLUP);
+  pinMode(PIN_SENSE, INPUT);
+
+  M1.setmotor(_STOP);
+  M2.setmotor(_STOP);
+
+  pwm=100;
   
 }
 
+
+void loop() {
+
+  if (millis() > last_pin_button + BUTTON_DEBOUNCE) {
+    bool new_pin_button_down=!digitalRead(PIN_BUTTON);
+    if (pin_button_down != new_pin_button_down) { //changed
+      pin_button_down = new_pin_button_down; //update
+      last_pin_button=millis(); //delay next check
+    }    
+  }
+
+  if (pin_button_down) { //button currently pressed
+    if (last_pin_button_down!=pin_button_down) { //value changed. executed one time when entering if
+      if (manual_drive_direction) {
+        M1.setmotor( _CW, pwm);
+        Serial.print("CW PWM: ");
+      }else{
+        M1.setmotor( _CCW, pwm);
+        Serial.print("CCW PWM: ");
+      }
+      Serial.println(pwm);
+      manual_drive_direction=!manual_drive_direction; //switch direction every new press
+    }
+  }else{
+    if (last_pin_button_down!=pin_button_down) { //value changed
+      Serial.println("Motor STOP");
+      M1.setmotor(_STOP);
+    }
+  }
+  last_pin_button_down=pin_button_down;
+
+  if (millis() > last_print + 100) {
+    int sense=analogRead(PIN_SENSE);
+    Serial.println(sense);
+    last_print=millis();
+  }
+}
+
+/*
 void loop() {
 
   int pwm;
@@ -59,3 +132,4 @@ void loop() {
   delay(1000);
 
 }
+*/
\ No newline at end of file