diff --git a/jacobot/arduino/Car.cpp b/jacobot/arduino/Car.cpp new file mode 100644 index 0000000..3663645 --- /dev/null +++ b/jacobot/arduino/Car.cpp @@ -0,0 +1,88 @@ +// Made by Linux_Hat +#include "Car.h" +#include "MotorEncoder.h" +#define pi 3.1415 + +Car::Car(MotorEncoder* m1, MotorEncoder* m2, float distance) + : m_m1(m1), m_m2(m2), m_distance(distance) { +} + +void Car::forward(float distance, double setPoint) { //distance : mm + const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1 + const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2 + double m1_duration = 0; + double m2_duration = 0; + m_m1->setPoint = setPoint; + m_m2->setPoint = setPoint/m2_mmp*m1_mmp; + while(m1_durationduration; + m_m1->compute(0); + m_m1->advance(); + m_m1->duration = 0; + } else {m_m1->stop();} + if(m2_durationduration; + m_m2->compute(0); + m_m2->advance(); + m_m2->duration = 0; + } else {m_m2->stop();} + } + m_m1->stop(); + m_m2->stop(); +} + +void Car::turn(float angle, double setPoint) { //angle : degre + float distance = pi*m_distance/360*angle; + const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1 + const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2 + double m1_duration = 0; + double m2_duration = 0; + m_m1->setPoint = setPoint; + m_m2->setPoint = setPoint/m2_mmp*m1_mmp; + boolean m1_tmp = false; + boolean m2_tmp = false; + if(angle>0) { + while(!m1_tmp && !m2_tmp) { + if(m1_durationduration); + m1_duration+=m_m1->duration; + m_m1->compute(0); + m_m1->advance(); + m_m1->duration = 0; + } else {m1_tmp = true; m_m1->stop();} + if(m2_durationduration); + m2_duration-=m_m2->duration; + m_m2->compute(0); + m_m2->back(); + m_m2->duration = 0; + } else {m2_tmp = true; m_m2->stop();} + } + } else if(angle<0) { + while(!m1_tmp && !m2_tmp) { + if(m1_durationduration; + m_m1->compute(0); + m_m1->back(); + m_m1->duration = 0; + } else {m1_tmp = true; m_m1->stop();} + if(m2_durationduration; + m_m2->compute(0); + m_m2->advance(); + m_m2->duration = 0; + } else {m2_tmp = true; m_m2->stop();} + } + } + Serial.println(m1_tmp); + Serial.println("End"); + m_m1->stop(); + m_m2->stop(); +} \ No newline at end of file diff --git a/jacobot/arduino/Car.h b/jacobot/arduino/Car.h new file mode 100644 index 0000000..d3cd4ae --- /dev/null +++ b/jacobot/arduino/Car.h @@ -0,0 +1,14 @@ +// Made by Linux_Hat +#pragma once +#include "MotorEncoder.h" + +class Car { + private: + float m_distance;// mm + MotorEncoder* m_m1; + MotorEncoder* m_m2; + public: + Car(MotorEncoder* m1, MotorEncoder* m2, float distance); + void forward(float distance, double setPoint); + void turn(float angle, double setPoint); +}; \ No newline at end of file diff --git a/jacobot/arduino/MotorEncoder.cpp b/jacobot/arduino/MotorEncoder.cpp index 85b3f08..5c84609 100644 --- a/jacobot/arduino/MotorEncoder.cpp +++ b/jacobot/arduino/MotorEncoder.cpp @@ -1,14 +1,17 @@ +// Made by Linux_Hat #include #include #include "MotorEncoder.h" -MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left) { +MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, boolean orientation, 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_orientation = orientation; + 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); @@ -23,22 +26,21 @@ 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 + if(val == HIGH){ + if(!m_orientation) duration--; + else duration++; } - else if(val == HIGH && !direction){ - direction = true; //Forward + else if(val == LOW){ + if(!m_orientation) duration++; + else duration--; } } 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); } @@ -66,7 +68,6 @@ void MotorEncoder::compute(int debug) { Serial.print(duration); Serial.print(" Power: "); Serial.println(val_output); - } duration = 0; } diff --git a/jacobot/arduino/MotorEncoder.h b/jacobot/arduino/MotorEncoder.h index 180be7c..f5e3898 100644 --- a/jacobot/arduino/MotorEncoder.h +++ b/jacobot/arduino/MotorEncoder.h @@ -1,3 +1,4 @@ +// Made by Linux_Hat #pragma once #include #include @@ -5,22 +6,24 @@ class MotorEncoder { private: uint8_t encoder0PinALast; //the number of the pulses - boolean direction; //the rotation direction + boolean m_orientation; //the rotation orientation 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; + double abs_duration; public: double setPoint; + float m_ratio; // how many pulse for a complete turn + float m_radius; // wheel radius in mm + double duration; - MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left); + MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, boolean orientation, float ratio, float radius); void EncoderHandlerInit(void (*ISR)(void)); void wheelSpeed(); diff --git a/jacobot/arduino/Radio.cpp b/jacobot/arduino/Radio.cpp new file mode 100644 index 0000000..476a5f6 --- /dev/null +++ b/jacobot/arduino/Radio.cpp @@ -0,0 +1,30 @@ +// Made by Linux_Hat +#include +#include +#include +#include "Radio.h" +#define tunnel "PIPE1" + +Radio::Radio(int canal, int pinCE, int pinCSN) { + m_canal = canal; + m_pinCE = pinCE; + m_pinCSN = pinCSN; + radio = new RF24(m_pinCE, m_pinCSN); + if (!radio->begin()) { + Serial.println("Error while initialise the NRF module"); + while (1); + } + Serial.println("Module initialised"); + radio->openReadingPipe(0, (uint8_t*)tunnel); + radio->setChannel(m_canal); + radio->setPALevel(RF24_PA_MIN); + radio->startListening(); +} + +char Radio::read() { + if (radio->available()) { + radio->read(&message, sizeof(message)); + return *message; + } + return 0; +} \ No newline at end of file diff --git a/jacobot/arduino/Radio.h b/jacobot/arduino/Radio.h new file mode 100644 index 0000000..1616354 --- /dev/null +++ b/jacobot/arduino/Radio.h @@ -0,0 +1,17 @@ +// Made by Linux_Hat +#pragma once +#include +#include +#include + +class Radio { + public: + char* message; + Radio(int canal, int pinCE, int pinCSN); + char read(); + private: + int m_canal; + int m_pinCE; + int m_pinCSN; + RF24* radio; +}; \ No newline at end of file diff --git a/jacobot/arduino/main.cpp b/jacobot/arduino/main.cpp index 4fa9ab0..5bf98c5 100644 --- a/jacobot/arduino/main.cpp +++ b/jacobot/arduino/main.cpp @@ -1,33 +1,33 @@ -//The sample code for driving one way motor encoder +// Made by Linux_Hat #include -#include "radio.h" +#include "Radio.h" #include "MotorEncoder.h" +#include "Car.h" -MotorEncoder m1(2, 8, 5, 4); //Initialise motor with pins -MotorEncoder m2(3, 9, 11, 10); //Initialise motor with pins - -Radio radio(120, 6, 7); +MotorEncoder m1(2, A4, 5, 4, false, 120, 3.3); // Initialise motor with pins +MotorEncoder m2(3, 9, 10, 8, true, 120, 3.3); // Initialise motor with pins void encoder_handler_m1() { - m1.wheelSpeed(); // Call wheelSpeed() of the m1 MotorEncoder instance + m1.wheelSpeed(); // Call wheelSpeed() of the m1 MotorEncoder instance } void encoder_handler_m2() { - m2.wheelSpeed(); // Call wheelSpeed() of the m2 MotorEncoder instance + m2.wheelSpeed(); // Call wheelSpeed() of the m2 MotorEncoder instance } +//Radio radio(120, 6, 7); + +Car car(&m1, &m2, 110); // Initialise the car with motors and distance + 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; - } + Serial.println("Car started"); + car.turn(90, 20); + //car.forward(100,50); +} void loop() { - m1.compute(1);//compute - m1.advance();//Motor Forward - m2.compute(1);//compute - m2.advance();//Motor Forward } \ No newline at end of file