Merge pull request #5 from Association-INTech/Asservissement

Asservissement

Author: MatthiasBienvenu

scripts/masterI2C.py

@@ -1,18 +1,25 @@
+#masteri2c est la pour debug la communication entre l'arduino et la pi. 
+
 import smbus #type: ignore #ignore the module could not be resolved error because it is a linux only module
 import time
-
+import numpy as np
+import struct
 # Create an SMBus instance
 bus = smbus.SMBus(1)  # 1 indicates /dev/i2c-1
 
 # I2C address of the slave
 SLAVE_ADDRESS = 0x08
 
-def write_data(data):
+
+vitesse = 200 # en millimetre par seconde
+direction = 100 # en degré
+
+
+def write_data(vitesse,direction):
     # 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)
+    data = struct.pack('<ff', float(vitesse), float(direction))
+    bus.write_i2c_block_data(SLAVE_ADDRESS, 0, list(data))
 
-<<<<<<< HEAD:scripts/masterI2C.py
 import struct
 
 def read_data(length):
@@ -24,23 +31,15 @@ def read_data(length):
         return float_value
     else:
         raise ValueError("Not enough data received from I2C bus")
-=======
-def read_data(length):
-    # Read a block of data from the slave
-    data = bus.read_i2c_block_data(SLAVE_ADDRESS, 0, length)
-    # Convert list of ASCII values to string
-    return ''.join(chr(byte) for byte in data)
->>>>>>> origin/oled_annimation:HL/masterI2C.py
 
 if __name__ == "__main__":
-    try:
-        # Send data to the slave
-        write_data("Hello Slave!")
-        time.sleep(1)  # Wait for the slave to process the data
-
-        # Request data from the slave
-        received = read_data(13)  # Adjust length as needed
+    while(True):
+        vitesse= float(input("vitesse en millimetre par seconde:"))
+        rotation= float(input("rotation en degré:"))
+        write_data(vitesse,rotation)
+        time.sleep(0.1)  # Wait for the slave to process the data
+        received = read_data(8)  # Adjust length as needed
         print("Received from slave:", received)
 
-    except Exception as e:
-        print("Error:", e)
+        # Request data from the slave
+        

src/LL/Asserv2bazar/Asserv2bazar.ino

@@ -5,7 +5,7 @@
 
 #define PIN_DIR 10
 #define PIN_MOT 9
-#define PIN_FOURCHE 14
+#define PIN_FOURCHE A0
 
 Servo moteur;
 Servo direction;
@@ -24,21 +24,22 @@ char command;
 float Vcons=0; 
 float old_Vcons ;//consigne
 float vitesse=0; //vitesse de la voiture
-
+int dir = 1881;
+float dir_recue = 90.0;
 //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 Ki=0.02; //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
-
+int old_out = 0;
 //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 mesures[3]; // tableau de mesures pour lisser
 
 ////I2C
 union floatToBytes {
@@ -61,6 +62,12 @@ const float r2_NiMh = 1000; // resistance of the second resistor
 float voltage_LiPo = 0;     // variable to store the value read
 float voltage_NiMh = 0;     // variable to store the value read
 
+
+
+//direction millieu 1851
+// tout a gaucge 1231
+// tout a droite 2471
+
 float getMeanSpeed(float dt){
   int length = sizeof(mesures)/sizeof(mesures[0]);
   //ajout d'une nouvelle mesure et suppression de l'ancienne
@@ -94,11 +101,24 @@ float getSpeed(float dt){
   vieuxTemps=millis();
   return V;
 }
+
+
 void blink(){ //on compte tous les fronts
   //Serial.print(count);
   count++;
 }
+
+
 float PID(float cons, float mes, float dt) {
+
+
+  if ( old_out <= 0 && cons > 0){     // pour pouvoir sauter directement dans la plage de pwm ou la roue bouge et une transition plus fluide entre marche arriere et avant 
+    integral = 2500;                  // valeur experimentale
+  }
+  else if (old_out >= 0 && cons <0 ){ // pour pouvoir sauter directement dans la plage de pwm où la roue bouge et une transition plus fluide entre marche avant et arriere
+    integral = -5000;                 // valeur experimentale
+  }
+
   // Adjust the measured speed based on the sign of the desired speed
   float adjustedMes = (cons < 0) ? -mes : mes;
 
@@ -112,24 +132,27 @@ float PID(float cons, float mes, float dt) {
   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;
+  // Garde en mémoir pour passer out à 0 directe lorsque l'on change de sens pour ne pas attendre qu'elle change de sens tout seul (que c'est long mdr
+  old_out = P + I;
+  
+  return P + I + D;
 }
+
+
 void calculateVoltage(){
   //read from the sensor
   // and convert the value to voltage
   voltage_LiPo = analogRead(sensorPin_Lipo);
   voltage_NiMh = analogRead(sensorPin_NiMh);
   voltage_LiPo = voltage_LiPo * (5.0 / 1023.0) * ((r1_LiPo + r2_LiPo) / r1_LiPo);
   voltage_NiMh = voltage_NiMh * (5.0 / 1023.0) * ((r1_NiMh + r2_NiMh) / r1_NiMh);
-  Serial.println(voltage_LiPo);
-  Serial.println(voltage_NiMh);
+  //Serial.println(voltage_LiPo);
+  //Serial.println(voltage_NiMh);
 }
 void setup() {
   Serial.begin(115200);
@@ -138,23 +161,24 @@ void setup() {
   moteur.attach(pinMoteur,0,2000);
 
   pinMode(pinDirection,OUTPUT);
-  direction.attach(pinDirection,0,2000);
+  direction.attach(pinDirection);
 
   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);
 
   Wire.begin(8);                  // Join I2C bus with address #8
-  Wire.onReceive(receiveEvent); // Register receive event
-  Wire.onRequest(requestEvent); // Register request event
+  Wire.onReceive(receiveEvent);   // Register receive event
+  Wire.onRequest(requestEvent);   // Register request event
   pinMode(13,OUTPUT);
 
   delay(10);
   Serial.print("init");
 }
+
+
 void loop() {
   calculateVoltage();
   // Commandes pour debugger
@@ -175,21 +199,42 @@ void loop() {
     case 'q':
     Vcons=0;
     break;
+    case 'b':
+    dir_recue+=10;
+    break;
+    case 'n':
+    dir_recue-=100;
+    break;
+    case 'j':
+    dir_recue+=10;
+    break;
+    case 'k':
+    dir_recue-=10;
+    break;
   }
-
 
+
+  // Propulsion de la voiture
   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){
+  if (Vcons == 0){
+    out = 0;
+    moteur.writeMicroseconds(out+1500);
+    Serial.print("out:");
+    Serial.print(out);
+  }
+  else if (Vcons>0){
 
     out = PID(Vcons,vitesse,float(deltaT)/1e3);
+    
     moteur.writeMicroseconds(constrain(1500 + out,1500,2000));
-
-  } else if ( Vcons<0 && old_Vcons>=0 ){
+    Serial.print("out:");
+    Serial.print(constrain(1500 + out,500,2000));
 
+  } else if ( Vcons<0 && old_Vcons>=0 ){
     out = PID(-8000,vitesse,float(deltaT)/1e3);
     moteur.writeMicroseconds(constrain(1500 + out,500,1500));
     delay(200);
@@ -205,33 +250,64 @@ void loop() {
 
   old_Vcons = Vcons;
 
+
+  // Direction de la voiture
+  dir = map(dir_recue,0,180,1231,2471); // remape en degré
+  direction.writeMicroseconds(dir);
+
   //print debug
   #if 1
-  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);
+     Serial.print(",integrale");
+     Serial.print(integral);
+     Serial.print(",const:");
+     Serial.print(200);
+     Serial.print(",Vcons:");
+     Serial.print(Vcons);
+     Serial.print(",Vitesse:");
+     Serial.print(vitesse);
+     Serial.print(",Directino en degré:");
+     Serial.print(dir_recue);
+//   Serial.print(", Ki :  ");
+//   Serial.print(Ki);
+//   Serial.print(", Kp: ");
+//   Serial.print(Kp);
+//   Serial.print(", ");
+     Serial.print(",out2:");
+     Serial.println(out);
   #endif
   delay(10);
-}
+   // tout a droite
+  //delay(1000);
+  //direction.writeMicroseconds(1761); au millieu
+  //delay(1000);
+  //direction.writeMicroseconds(2140);tout a gauche
+  //delay(1000);
+  
+  }
 void receiveEvent(int byteCount){
-  for(uint8_t index = 0; index<byteCount; index++){
-      converter.valueBuffer[index] = Wire.read();
+  // Ignorer le premier octet "commande" du Raspberry
+  if (Wire.available()) Wire.read(); // skip cmd byte
+
+  if (byteCount >= 9) { // 1 cmd + 8 data
+    byte buffer[8];
+    for (int i = 0; i < 8 && Wire.available(); i++) {
+      buffer[i] = Wire.read();
+    }
+
+    float* vals = (float*)buffer;
+    Vcons = vals[0];  // reçue en milimetre par secondes
+    dir_recue = vals[1];  //reçue en degré. 
+  } else {
+    while (Wire.available()) Wire.read(); // vide le buffer
   }
-  Vcons = converter.valueReading;
-
 }
+
+
 void requestEvent(){
   const int numFloats = 2; // Number of floats to send
   float data[numFloats] = {voltage_LiPo, voltage_NiMh}; // Example float values to send
   byte* dataBytes = (byte*)data;
+  
 
   Wire.write(dataBytes, sizeof(data));
-}
+}