[JacoBot] Motor encoder basic Arduino implementation with PID

jacobot/arduino/MotorEncoder.cpp

@@ -0,0 +1,73 @@
+#include <PID_v1.h>
+#include <Arduino.h>
+#include "MotorEncoder.h"
+
+MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left) {
+    m_encoder0pinA = encoder0pinA; // pin 2 or 3
+    m_encoder0pinB = encoder0pinB;
+    m_E_left = E_left;// pwm pin !
+    m_M_left = M_left;
+    setPoint = 0;
+    direction = true;  // Default -> Forward
+    pinMode(m_encoder0pinB, INPUT);
+    pinMode(m_M_left, OUTPUT);  // L298P Control port settings DC motor driver board for the output mode
+    pinMode(m_E_left, OUTPUT);
+
+    // Initialize PID controller
+    myPID = new PID(&abs_duration, &val_output, &setPoint, Kp, Ki, Kd, DIRECT);
+    myPID->SetMode(AUTOMATIC);  // PID is set to automatic mode
+    myPID->SetSampleTime(100);  // Set PID sampling frequency to 100ms
+}
+
+void MotorEncoder::wheelSpeed() {
+  int Lstate = digitalRead(m_encoder0pinA);
+  if((encoder0PinALast == LOW) && Lstate==HIGH){
+    int val = digitalRead(m_encoder0pinB);
+    if(val == LOW && direction){
+      direction = false; //Reverse
+    }
+    else if(val == HIGH && !direction){
+      direction = true;  //Forward
+    }
+  }
+  encoder0PinALast = Lstate;
+
+  if(!direction)  duration++;
+  else  duration--;
+
+}
+
+void MotorEncoder::EncoderHandlerInit(void (*ISR)(void)) {
+    direction = true;  // Default -> Forward
+    pinMode(m_encoder0pinB, INPUT);  // Ensure encoder pin A is set as INPUT
+    attachInterrupt(m_encoder0pinA-2, ISR, CHANGE);
+}
+
+void MotorEncoder::advance() {
+    digitalWrite(m_M_left, LOW);
+    analogWrite(m_E_left, abs(val_output));
+}
+
+void MotorEncoder::back() {
+    digitalWrite(m_M_left, HIGH);
+    analogWrite(m_E_left, abs(val_output));
+}
+
+void MotorEncoder::stop() {
+    analogWrite(m_E_left, 0); 
+}
+
+void MotorEncoder::compute(int debug) {
+    abs_duration = abs(duration);
+    result = myPID->Compute();
+    if (result) {
+        if (debug) {
+            Serial.print("Pluse: ");
+            Serial.print(duration);
+            Serial.print("  Power: ");
+            Serial.println(val_output);
+            
+        }
+        duration = 0;
+    }
+}

jacobot/arduino/MotorEncoder.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <PID_v1.h>
+#include <Arduino.h>
+
+class MotorEncoder {
+    private:
+        uint8_t encoder0PinALast; //the number of the pulses
+        boolean direction; //the rotation direction
+        boolean result;
+        PID* myPID;
+        double val_output; //Power supplied to the motor PWM value.
+        double Kp = 0.6, Ki = 5, Kd = 0;
+        double abs_duration;
+        double duration;
+        uint8_t m_encoder0pinA; //A pin
+        uint8_t m_encoder0pinB; //B pin 
+        int m_E_left; //The enabling of L298PDC motor driver board connection to the digital interface port 5
+        int m_M_left;
+
+    public:
+        double setPoint;
+
+        MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left);
+
+        void EncoderHandlerInit(void (*ISR)(void));
+        void wheelSpeed();
+        void advance();
+        void back();
+        void stop();
+        void compute(int debug);
+};

jacobot/arduino/README.md

@@ -0,0 +1 @@
+# JacoBot Arduino

jacobot/arduino/main.cpp

@@ -0,0 +1,31 @@
+//The sample code for driving one way motor encoder
+#include <PID_v1.h>
+#include <Arduino.h>
+#include "MotorEncoder.h"
+
+MotorEncoder m1(2, 8, 5, 4); //Initialise motor with pins
+MotorEncoder m2(3, 9, 11, 10); //Initialise motor with pins
+
+void encoder_handler_m1() {
+    m1.wheelSpeed();  // Call wheelSpeed() of the m1 MotorEncoder instance
+}
+
+void encoder_handler_m2() {
+    m2.wheelSpeed();  // Call wheelSpeed() of the m2 MotorEncoder instance 
+}
+
+void setup() {
+   //Initialise motor with pins
+  Serial.begin(9600);//Initialize the serial port
+  m1.EncoderHandlerInit(encoder_handler_m1);
+  m1.setPoint = 80;
+  m2.EncoderHandlerInit(encoder_handler_m2);
+  m2.setPoint = 80;
+  }
+
+void loop() {
+  m1.compute(1);//compute
+  m1.advance();//Motor Forward
+  m2.compute(1);//compute
+  m2.advance();//Motor Forward
+}