From c4d0df4730f96dcf6b4e503857131100f796d885 Mon Sep 17 00:00:00 2001
From: Stedd <tor.andre.paulsen@gmail.com>
Date: Thu, 19 Dec 2019 22:40:04 +0100
Subject: [PATCH] Quadrature encoder reading working

Read encoders with interrupt routines
---
 Main/Main.ino         | 117 +++++++++++++++++++++++++++++++++++-------
 Main/motorControl.ino |  32 ++++++------
 2 files changed, 114 insertions(+), 35 deletions(-)

diff --git a/Main/Main.ino b/Main/Main.ino
index d4e353c..b6f2822 100644
--- a/Main/Main.ino
+++ b/Main/Main.ino
@@ -2,9 +2,12 @@
 #include "GY_85.h"
 #include <Wire.h>
 
+//System variables
+unsigned long now = micros();
 
+
+//IMU variables
 GY_85 GY85;     //create the object
-
 int   ax_, ay_, az_;
 int   ax, ay, az;
 int   cx, cy, cz;
@@ -12,12 +15,76 @@ float gx, gy, gz, gt;
 float acc_pitch;
 signed int speed_setp;
 
+//Random
 int i, modifier;
 
-int m1_in1 = 33;
-int m1_in2 = 25;
-int m2_in1 = 13;
-int m2_in2 = 14;
+//Motor Control variables
+const byte m1_in1 = 18;
+const byte m1_in2 = 19;
+//int m2_in1 = 16;
+//int m2_in2 = 17;
+
+//Encoders variables
+long int m1Raw, m1RawLast;
+const byte pin_m1_A = 34;
+const byte pin_m1_B = 35;
+volatile bool A_state;
+volatile bool B_state;
+
+//Interrupt routines
+//  CW =  INCREASE
+//  CCW = DECREASE
+
+void IRAM_ATTR m1_A_changed() {
+  A_state = digitalRead(pin_m1_A);
+  B_state = digitalRead(pin_m1_B);
+  
+  //Rising
+  if (A_state == HIGH) {
+    if (B_state == HIGH) {
+      m1Raw = m1Raw - 1;
+    }
+    else if (B_state == LOW) {
+      m1Raw = m1Raw + 1;
+    }
+  }
+  
+  //Falling
+  else if (A_state == LOW) {
+    if (B_state == HIGH) {
+      m1Raw = m1Raw + 1;
+    }
+    else if (B_state == LOW) {
+      m1Raw = m1Raw - 1;
+    }
+  }
+}
+
+void IRAM_ATTR m1_B_changed() {
+  A_state = digitalRead(pin_m1_A);
+  B_state = digitalRead(pin_m1_B);
+  
+  //Rising
+  if (B_state == HIGH) {
+    if (A_state == HIGH) {
+      m1Raw = m1Raw + 1;
+    }
+    else if (A_state == LOW) {
+      m1Raw = m1Raw - 1;
+    }
+  }
+
+  //Falling
+  else if (B_state == LOW) {
+    if (A_state == HIGH) {
+      m1Raw = m1Raw - 1;
+    }
+    else if (A_state == LOW) {
+      m1Raw = m1Raw + 1;
+    }
+  }
+}
+
 
 void setup() {
   //Initialize serial
@@ -33,25 +100,45 @@ void setup() {
   GY85.init();
   delay(10);
 
+  //Initialize Interrupts
+  pinMode(pin_m1_A, INPUT_PULLUP);
+  pinMode(pin_m1_B, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(pin_m1_A), m1_A_changed, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(pin_m1_B), m1_B_changed, CHANGE);
+
+  //Initialize Encoders
+  m1Raw = 0;
+  m1RawLast = 100;
+
   // Initialize PWM channels
   // channels 0-15, resolution 1-16 bits, freq limits depend on resolution
   // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
 
   ledcAttachPin(m1_in1, 1);
   ledcAttachPin(m1_in2, 2);
-  ledcAttachPin(m2_in1, 3);
-  ledcAttachPin(m2_in2, 4);
+  //  ledcAttachPin(m2_in1, 3);
+  //  ledcAttachPin(m2_in2, 4);
 
-  ledcSetup(1, 12000, 8); // 12 kHz PWM, 8-bit resolution
-  ledcSetup(2, 12000, 8);
-  ledcSetup(3, 12000, 8);
-  ledcSetup(4, 12000, 8);
+  ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution
+  ledcSetup(2, 12000, 12);
+  //  ledcSetup(3, 12000, 8);
+  //  ledcSetup(4, 12000, 8);
 
+  //Random
   i = 0;
   modifier = 2;
 }
 
 void loop() {
+  //Update system variables
+  now = micros();
+
+  //Only print encoder value if value changed since last print
+  if (m1Raw != m1RawLast) {
+    Serial.println(m1Raw);
+    m1RawLast = m1Raw;
+  }
+
   //Sense
   readIMU();
 
@@ -62,14 +149,6 @@ void loop() {
   //Act
   motorControl();
 
-  i=i+modifier;
-
-    if (i>=4096){
-      modifier = -2;
-    }
-    else if (i<=0){
-      modifier = 2;
-    }
 
   //Delay
   delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
diff --git a/Main/motorControl.ino b/Main/motorControl.ino
index fed3feb..c7e57be 100644
--- a/Main/motorControl.ino
+++ b/Main/motorControl.ino
@@ -1,21 +1,21 @@
 void motorControl() {
-//  //Motor 1
-//  speed_setp = round(map(acc_pitch, -90.0, 90.0, -255, 255));
-//  if (speed_setp > 0) {
-//    ledcWrite(1, speed_setp);
-//    ledcWrite(2, 0);
-//  }
-//  else if (speed_setp < 0) {
-//    ledcWrite(1, 0);
-//    ledcWrite(2, -1*speed_setp);
-//  }
-//  Serial.print  (acc_pitch);
-//  Serial.print  ( "," );
-//  Serial.println(speed_setp);
+  //Motor 1
+  speed_setp = round (map(acc_pitch, -90.0, 90.0, -4096, 4096));
+  if (speed_setp > 0) {
+    ledcWrite(1, speed_setp);
+    ledcWrite(2, 0);
+  }
+  else if (speed_setp < 0) {
+    ledcWrite(1, 0);
+    ledcWrite(2, -1 * speed_setp);
+  }
+  //  Serial.print  (acc_pitch);
+  //  Serial.print  ( "," );
+  //  Serial.println(speed_setp);
 
-
-  ledcWrite(1, 0);
-  ledcWrite(2, 0);
+  //
+  //  ledcWrite(1, 0);
+  //  ledcWrite(2, 0);
 
 
   //Motor 2