1
1
forked from MDL29/JacoBot

[JacoBot] Motor encoder basic Arduino implementation with PID

This commit is contained in:
Linux_Hat 2024-04-06 15:37:39 +02:00
parent fc9430fc2b
commit 97f126835c
5 changed files with 136 additions and 0 deletions

View File

@ -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;
}
}

View File

@ -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);
};

View File

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

31
jacobot/arduino/main.cpp Normal file
View File

@ -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
}