Merge branch 'Asservissement'

Author: Danura30082

.DS_Store

@@ -0,0 +1 @@
+Page asservissement

Licence

@@ -5,6 +5,7 @@ nav:
   - Lidar.md
   - Simulateur.md
   - IA.md
+  - Asservissement.md
   - How to create documentation: building_docs.md
   - Déroulement de la course: Deroulement_course.md
   - Regle de la course: https://ajuton-ens.github.io/CourseVoituresAutonomesSaclay/

README.md

@@ -0,0 +1,50 @@
+from time import sleep
+import smbus #type: ignore #ignore the module could not be resolved error because it is a linux only module
+import struct
+
+SLAVE_ADDRESS = 0x08
+# Create an SMBus instance
+bus = smbus.SMBus(1)  # 1 indicates /dev/i2c-1
+
+def write_data(float_data):
+    # Convert the float to bytes
+    print(float_data)
+    byte_data = struct.pack('f', float_data)
+    # Convert the bytes to a list of integers
+    int_data = list(byte_data)
+    print(int_data)
+    int_data.append(0)
+    # Write the data to the I2C bus
+    bus.write_i2c_block_data(SLAVE_ADDRESS, int_data[0], int_data[1:4])
+
+def read_data(num_floats=3):
+
+    # Each float is 4 bytes
+    length = num_floats * 4
+    # Read a block of data from the slave
+    data = bus.read_i2c_block_data(SLAVE_ADDRESS, 0, length)
+    # Convert the byte data to floats
+    if len(data) >= length:
+        float_values = struct.unpack('f' * num_floats, bytes(data[:length]))
+        return list(float_values)
+    else:
+        raise ValueError("Not enough data received from I2C bus")
+    
+def read_data(num_floats=3):
+    # Each float is 4 bytes
+    length = num_floats * 4
+    # Read a block of data from the slave
+    data = bus.read_i2c_block_data(SLAVE_ADDRESS, 0, length)
+    # Convert the byte data to floats
+    if len(data) >= length:
+        float_values = struct.unpack('f' * num_floats, bytes(data[:length]))
+        return list(float_values)
+    else:
+        raise ValueError("Not enough data received from I2C bus")
+
+i=0.56
+while True :
+    write_data(i)
+    sleep(0.5)
+    i+=1
+

docs/Asservissement.md

@@ -5,6 +5,12 @@
 import numpy as np
 from gpiozero import LED, Button
 import logging as log
+import smbus # type: ignore #ignore the module could not be resolved error because it is a linux only module
+import struct
+
+SLAVE_ADDRESS = 0x08
+# Create an SMBus instance
+bus = smbus.SMBus(1)  # 1 indicates /dev/i2c-1
 
 
 # Import constants from HL.Autotech_constant to share them between files and ease of use

mkdocs.yml

@@ -65,10 +65,16 @@ def display_combined_im(text):
     with canvas(device) as draw:
         draw.bitmap((0, 0), im, fill="white")
 
-def write_data(data):
-    # Convert string to list of ASCII values
-    data_list = [ord(char) for char in data]
-    bus.write_i2c_block_data(SLAVE_ADDRESS, 0, data_list)
+def write_data(float_data):
+    # Convert the float to bytes
+    print(float_data)
+    byte_data = struct.pack('f', float_data)
+    # Convert the bytes to a list of integers
+    int_data = list(byte_data)
+    print(int_data)
+    int_data.append(0)
+    # Write the data to the I2C bus
+    bus.write_i2c_block_data(SLAVE_ADDRESS, int_data[0], int_data[1:4])
 
 def read_data(num_floats=3):
 

scripts/sender.py

@@ -0,0 +1,192 @@
+#include <Servo.h>
+#include <EnableInterrupt.h>
+#include "I2Cslave.hpp"
+
+#define PIN_DIR 10
+#define PIN_MOT 9
+#define PIN_FOURCHE 14
+
+
+Servo moteur;
+Servo direction;
+
+
+
+//declaration des broches des differents composants
+const int pinMoteur=PIN_MOT;
+const int pinDirection=PIN_DIR;
+const int pinFourche=PIN_FOURCHE;
+
+//constantes utiles
+const int nb_trous=16*2; //nb de trous dans la roue qui tourne devant la fourche optique
+const int distanceUnTour=79; //distance parcourue par la voiture apres un tour de roue en mm
+
+//variables
+char command;
+float Vcons=0; 
+float old_Vcons ;//consigne
+float vitesse=0; //vitesse de la voiture
+
+//PID
+float vieuxEcart=0;
+float vieuxTemps=0; //variable utilisee pour mesurer le temps qui passe
+float Kp=0.05; //correction prop
+float Ki=0.1; //correction integrale
+float Kd=0.; //correction derivee
+float integral=0;//valeur de l'integrale dans le PID
+float derivee=0; //valeur de la derivee dans le PID
+
+//mesures
+volatile int count=0; //variable utilisee pour compter le nombre de fronts montants/descendants
+volatile int vieuxCount=0; //stocke l'ancienne valeur de count pour ensuite faire la difference
+volatile byte state=LOW;
+float mesures[10]; // tableau de mesures pour lisser
+
+float getMeanSpeed(float dt){
+  int length = sizeof(mesures)/sizeof(mesures[0]);
+  //ajout d'une nouvelle mesure et suppression de l'ancienne
+  for (int i=length-1;i>0;i--){
+      mesures[i]=mesures[i-1];
+  }
+  mesures[0] = getSpeed(dt);
+
+  //Calcul d'une moyenne pour lisser les mesures qui sont trop dipersées sinon
+  float sum=0;
+  for (int i=0;i<length;i++){
+    sum+=mesures[i];
+  }
+
+  //affichage debug
+  #if 0
+  for(int i=0;i<length;i++){
+    Serial.print(mesures[i]);
+    Serial.print(" , ");
+  }
+  Serial.println(sum/length);
+  #endif
+
+  return sum/length;
+}
+
+float getSpeed(float dt){  
+  int N = count - vieuxCount; //nombre de fronts montant et descendands après chaque loop
+  float V = ((float)N/(float)nb_trous)*distanceUnTour/(dt*1e-3); //16 increments -> 1 tour de la roue et 1 tour de roue = 79 mm 
+  vieuxCount=count;
+  vieuxTemps=millis();
+  return V;
+}
+void blink(){ //on compte tous les fronts
+  //Serial.print(count);
+  count++;
+}
+float PID(float cons, float mes, float dt) {
+  // Adjust the measured speed based on the sign of the desired speed
+  float adjustedMes = (cons < 0) ? -mes : mes;
+
+  // Calculate the error
+  float e = cons - adjustedMes;
+
+  // Proportional term
+  float P = Kp * e;
+
+  // Integral term
+  integral = integral + e * dt;
+  float I = Ki * integral;
+
+  #if 0
+  // Derivative term
+  derivee = (e - vieuxEcart) / dt;
+  vieuxEcart = e;
+  float D = Kd * derivee;
+  #endif
+
+  return P + I;
+}
+void setup() {
+  I2Cslave::setup();
+  Serial.begin(115200);
+
+  pinMode(pinMoteur,OUTPUT);
+  moteur.attach(pinMoteur,0,2000);
+
+  pinMode(pinDirection,OUTPUT);
+  direction.attach(pinDirection,0,2000);
+  
+  pinMode(pinFourche,INPUT_PULLUP);
+  enableInterrupt(PIN_FOURCHE, blink, CHANGE); //on regarde a chaque fois que le signal de la fourche change (Montants et Descendants)
+
+  moteur.writeMicroseconds(1500);
+  delay(2000);
+  moteur.writeMicroseconds(1590);
+
+
+  delay(10);
+  Serial.println("init");
+}
+void loop() {
+  // Commandes pour debugger
+  command=Serial.read();
+  switch (command){ //pour regler les parametres
+    case 'a':
+    Vcons+=200;
+    break;
+    case 'z':
+    Vcons-=200;
+    break;
+    case 's':
+    Vcons=-1000;
+    break;
+    case 'd':
+    Vcons=2000;
+    break;
+    case 'q':
+    Vcons=0;
+    break;
+  }
+  
+  //lecture depuis l'I2C
+  Vcons = I2CSlave::getReadValue();
+  
+  int deltaT = millis()-vieuxTemps; //temps qui est passé pendant un loop (en millisecondes)
+  vitesse=getMeanSpeed(deltaT); // on recup la vitesse lissée
+  
+  int out;
+
+  if (Vcons>=0){
+
+    out = PID(Vcons,vitesse,float(deltaT)/1e3);
+    moteur.writeMicroseconds(constrain(1500 + out,1500,2000));
+
+  } else if ( Vcons<0 && old_Vcons>=0 ){
+    
+    out = PID(-8000,vitesse,float(deltaT)/1e3);
+    moteur.writeMicroseconds(constrain(1500 + out,500,1500));
+    delay(200);
+    out = PID(0,vitesse,float(deltaT)/1e3);
+    moteur.writeMicroseconds(constrain(1500 + out,1500,2000));
+    delay(10);
+
+  } else {
+
+    out = PID(Vcons,vitesse,float(deltaT)/1e3);
+    moteur.writeMicroseconds(constrain(1500 + out,500,1500));
+  }
+
+  old_Vcons = Vcons;
+
+  //print debug
+  #if 0
+  Serial.print("");
+  Serial.print(Vcons);
+  Serial.print(", ");
+  Serial.print(vitesse);
+  Serial.print(", ");
+  Serial.print(Ki);
+  Serial.print(", ");
+  Serial.print(Kp);
+  Serial.print(", ");
+  Serial.print("out=");
+  Serial.println(out);
+  #endif
+  delay(10);
+}