Merge branch 'main' of git.afpy.org:Adrien/JacoBot into experimental

This reverts commit ea3a44c024.

	modified:   jacobot/arduino/Car.cpp
	modified:   jacobot/arduino/MotorEncoder.cpp
	modified:   jacobot/arduino/main.cpp
	deleted:    jacopad/arduino/.gitkeep

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_duration<distance/m1_mmp && m2_duration<distance/m2_mmp) {
+        if(m1_duration<distance/m1_mmp) {
+            m1_duration+=m_m1->duration;
+            m_m1->compute(0);
+            m_m1->advance();
+            m_m1->duration = 0;
+        } else {m_m1->stop();}
+        if(m2_duration<distance/m2_mmp) {
+            m2_duration+=m_m2->duration;
+            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_duration<distance/m1_mmp) {
+//                Serial.print("M1 cycle ");
+//                Serial.print(m1_duration);
+//                Serial.print("  ");
+//                Serial.println(m_m1->duration);
+                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_duration<distance/m2_mmp) {
+//                Serial.print("M2 cycle ");
+//                Serial.print(m2_duration);
+//                Serial.print("  ");
+//                Serial.println(m_m2->duration);
+                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_duration<distance/m1_mmp) {
+                m1_duration-=m_m1->duration;
+                m_m1->compute(0);
+                m_m1->back();
+                m_m1->duration = 0;
+            } else {m1_tmp = true; m_m1->stop();}
+            if(m2_duration<distance/m2_mmp) {
+                m2_duration+=m_m2->duration;
+                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();
+}

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

jacobot/arduino/MotorEncoder.cpp

@@ -1,14 +1,17 @@
+// Made by Linux_Hat
 #include <PID_v1.h>
 #include <Arduino.h>
 #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;
     }

jacobot/arduino/MotorEncoder.h

@@ -1,3 +1,4 @@
+// Made by Linux_Hat
 #pragma once
 #include <PID_v1.h>
 #include <Arduino.h>
@@ -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();

jacobot/arduino/Radio.cpp

@@ -0,0 +1,30 @@
+// Made by Linux_Hat
+#include <RF24.h>
+#include <SPI.h>
+#include <Arduino.h>
+#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;
+}

jacobot/arduino/Radio.h

@@ -0,0 +1,17 @@
+// Made by Linux_Hat
+#pragma once
+#include <RF24.h>
+#include <SPI.h>
+#include <Arduino.h>
+
+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;
+};

jacobot/arduino/main.cpp

@@ -1,33 +1,33 @@
-//The sample code for driving one way motor encoder
+// Made by Linux_Hat
 #include <Arduino.h>
-#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
 }