Adrien/JacoBot
1
1
Fork 0
forked from MDL29/JacoBot
Code Pull Requests Activity

fixes and optimisations

Browse Source
This commit is contained in:
Linux_Hat 2024-05-11 21:38:01 +02:00
parent 20ae6250fd
commit 7d8ceba7bf
6 changed files with 116 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
jacobot/arduino/jacobot-pio-project/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,73 @@
{
"C_Cpp_Runner.cCompilerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-gcc",
"C_Cpp_Runner.cppCompilerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-g++",
"C_Cpp_Runner.debuggerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-gdb",
"C_Cpp_Runner.cStandard": "gnu11",
"C_Cpp_Runner.cppStandard": "gnu++11",
"C_Cpp_Runner.msvcBatchPath": "",
"C_Cpp_Runner.useMsvc": false,
"C_Cpp_Runner.warnings": [
"-Wall",
"-Wextra",
"-Wpedantic",
"-Wshadow",
"-Wformat=2",
"-Wcast-align",
"-Wconversion",
"-Wsign-conversion",
"-Wnull-dereference"
],
"C_Cpp_Runner.msvcWarnings": [
"/W4",
"/permissive-",
"/w14242",
"/w14287",
"/w14296",
"/w14311",
"/w14826",
"/w44062",
"/w44242",
"/w14905",
"/w14906",
"/w14263",
"/w44265",
"/w14928"
],
"C_Cpp_Runner.enableWarnings": true,
"C_Cpp_Runner.warningsAsError": false,
"C_Cpp_Runner.compilerArgs": [],
"C_Cpp_Runner.linkerArgs": [],
"C_Cpp_Runner.includePaths": [
"/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/include",
"/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/src",
"/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/PID",
"/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/RF24",
"/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/RF24/utility",
"/home/adrien/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
"/home/adrien/.platformio/packages/framework-arduino-avr/cores/arduino",
"/home/adrien/.platformio/packages/framework-arduino-avr/variants/standard",
"/home/adrien/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
"/home/adrien/.platformio/packages/framework-arduino-avr/libraries/HID/src",
"/home/adrien/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
"/home/adrien/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
""
],
"C_Cpp_Runner.includeSearch": [
"*",
"**/*"
],
"C_Cpp_Runner.excludeSearch": [
"**/build",
"**/build/**",
"**/.*",
"**/.*/**",
"**/.vscode",
"**/.vscode/**"
],
"C_Cpp_Runner.useAddressSanitizer": false,
"C_Cpp_Runner.useUndefinedSanitizer": false,
"C_Cpp_Runner.useLeakSanitizer": false,
"C_Cpp_Runner.showCompilationTime": false,
"C_Cpp_Runner.useLinkTimeOptimization": false,
"C_Cpp_Runner.msvcSecureNoWarnings": false
}

21
jacobot/arduino/jacobot-pio-project/src/Car.cpp → jacobot/arduino/jacobot-pio-project/src/car.cpp
View File

@ -1,6 +1,6 @@
// Made by Linux_Hat
#include "Car.h"
#include "MotorEncoder.h"
#include "car.h"
#include "motorEncoder.h"
#define pi 3.1415
Car::Car(MotorEncoder* left, MotorEncoder* right, float distance_beetween_motors)
@ -16,16 +16,13 @@ void Car::forward(float distance, double setPoint) { //distance : mm
m_right->setPoint = setPoint/m2_mmp*m1_mmp;
while(m1_duration<distance/m1_mmp && m2_duration<distance/m2_mmp) {
if(m1_duration<distance/m1_mmp) {
m1_duration+=m_left->duration;
m_left->compute(0);
m1_duration+=m_left->compute(0);
m_left->advance();
m_left->duration = 0;
} else {m_left->stop();}
if(m2_duration<distance/m2_mmp) {
m2_duration+=m_right->duration;
m_right->compute(0);
m2_duration+m_right->compute(0);
m_right->advance();
m_right->duration = 0;
} else {m_right->stop();}
}
m_left->stop();
@ -42,8 +39,7 @@ void Car::turn(float angle, double setPoint) { //angle : degre
if(angle>0) {
const double pulse_goal = distance/m1_mmp;
while(duration<pulse_goal) {
duration+=m_left->duration;
m_left->compute(0);
duration+=m_left->compute(0);
m_left->advance();
m_right->compute(0);
m_right->back();
@ -51,11 +47,10 @@ void Car::turn(float angle, double setPoint) { //angle : degre
} else if(angle<0) {
const double pulse_goal = distance/m2_mmp;
while(duration<pulse_goal) {
duration+=m_right->duration;
duration+=m_right->compute(0);
m_right->advance();
m_left->compute(0);
m_left->back();
m_right->compute(0);
m_right->advance();
}
}
m_left->stop();

2
jacobot/arduino/jacobot-pio-project/src/Car.h → jacobot/arduino/jacobot-pio-project/src/car.h
View File

@ -1,6 +1,6 @@
// Made by Linux_Hat
#pragma once
#include "MotorEncoder.h"
#include "motorEncoder.h"
/**
* @brief Create a new instance of car

10
jacobot/arduino/jacobot-pio-project/src/main.cpp
View File

@ -1,14 +1,13 @@
// Made by Linux_Hat
#include <Arduino.h>
#include "radio.h"
#include "MotorEncoder.h"
#include "Car.h"
#include "motorEncoder.h"
#include "car.h"
#define MLEFT_ENC_PIN_A 3
#define MLEFT_ENC_PIN_B A4
#define MLEFT_E_MOTOR 5
#define MLEFT_M_MOTOR 4
#define MLEFT_ORIENTATION false
#define MLEFT_RATIO 120
#define MLEFT_RADIUS 3.3
@ -16,7 +15,6 @@
#define MRIGHT_ENC_PIN_B A3
#define MRIGHT_E_MOTOR 10
#define MRIGHT_M_MOTOR 8
#define MRIGHT_ORIENTATION false
#define MRIGHT_RATIO 120
#define MRIGHT_RADIUS 3.3
@ -25,8 +23,8 @@
#define SIZE 500
#define SPEED 50
MotorEncoder m1(MLEFT_ENC_PIN_A, MLEFT_ENC_PIN_B, MLEFT_E_MOTOR, MLEFT_M_MOTOR, MLEFT_ORIENTATION, MLEFT_RATIO, MLEFT_RADIUS); // Initialise motor
MotorEncoder m2(MRIGHT_ENC_PIN_A, MRIGHT_ENC_PIN_B, MRIGHT_E_MOTOR, MRIGHT_M_MOTOR, MRIGHT_ORIENTATION, MRIGHT_RATIO, MRIGHT_RADIUS); // Initialise motor with pins
MotorEncoder m1(MLEFT_ENC_PIN_A, MLEFT_ENC_PIN_B, MLEFT_E_MOTOR, MLEFT_M_MOTOR, MLEFT_RATIO, MLEFT_RADIUS); // Initialise motor
MotorEncoder m2(MRIGHT_ENC_PIN_A, MRIGHT_ENC_PIN_B, MRIGHT_E_MOTOR, MRIGHT_M_MOTOR, MRIGHT_RATIO, MRIGHT_RADIUS); // Initialise motor with pins
Car car(&m1, &m2, 110);

5
jacobot/arduino/jacobot-pio-project/src/motorEncoder.cpp
View File

@ -59,7 +59,7 @@ void MotorEncoder::stop() {
analogWrite(m_E_left, 0);
}
void MotorEncoder::compute(int debug) {
double MotorEncoder::compute(int debug) {
abs_duration = abs(duration);
result = myPID->Compute();
if (result) {
@ -70,6 +70,9 @@ void MotorEncoder::compute(int debug) {
Serial.println(val_output);
}
double old_duration = duration;
duration = 0;
return old_duration;
}
return duration;
}

95
jacobot/arduino/jacobot-pio-project/src/motorEncoder.h
View File

@ -1,75 +1,32 @@
#pragma once
#include <PID_v1.h>
#include <Arduino.h>
#include "motorEncoder.h"
MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, float ratio, float radius) {
m_encoder0pinA = encoder0pinA; // pin 2 or 3
m_encoder0pinB = encoder0pinB;
m_E_left = E_left;// pwm pin !
m_M_left = M_left;
m_ratio = ratio;
m_radius = radius;
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);
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;
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;
// 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
}
public:
float m_radius, m_ratio;
double duration;
double setPoint;
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;
MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, float ratio, float radius);
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;
}
}
void EncoderHandlerInit(void (*ISR)(void));
void wheelSpeed();
void advance();
void back();
void stop();
double compute(int debug);
};