Pages: 1 ... 84 85 [86] 87 88 ... 101   Go Down
Author Topic: [Multicotteri] Elettronica : IMU, MCU, Sensori ed algoritmi di controllo  (Read 118131 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ho provato ad implementare un timer interrupt come qui illustrato http://arduinomega.blogspot.it/2011/05/timer2-and-overflow-interrupt-lets-get.html e il tutto funziona alla perfezione, solamente che integrandolo nella funzione PID fa a pugni con la libreria che implementa la comunicazione I2C (wire.h) e il dispositivo non risponde più.

Io ho bisogno di avere 4 interrupt per la ricezione dalla trasmittente, 1 interrupt per la comunicazione I2C e 1 interrupt per sincronizzare la funzione PID. Giusto?!

Se così fosse ci sto al pelo, perché arduino Mega 2560 ha giusto giusto 5 interrupt. Soluzioni?!

Mi viene in mente che la libreria Servo.h utilizza l'interrupt 2... quindi non ci sto con gli interrupt..SOLUZIONI???  smiley-cry
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

per il PID non ti serve la precisione di un timer interrupt, va bene fareun check con millis() per vedere se è trascorso il giusto tempo. Considera che il PPM "classico" lavora sui 50Hz, quindi cambiare più spesso la velocità non serve a nulla
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Considera che il PPM "classico" lavora sui 50Hz, quindi cambiare più spesso la velocità non serve a nulla

Ma ti devo cazziare subito oppure preferisci che ti faccio recapitare un invito premio per il quizzettone ?  smiley-grin
Regola prima, il ciclo pid DEVE essere almeno 10 volte più veloce della costante di tempo del sistema, meglio se 20 volte.
Regola seconda, lo slicing del pid non solo deve essere molto preciso, non deve essere affetto da jitter, la millis,e pure la micros, buttale in quel posto e tira la catena che non ci fa nulla.
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

nel mio codice l'update del PID e dei motori(Servo) sono fatti nello stesso update e ho trovato vantaggi nel farlo andare a 50Hz invece che ad ogni loop.

Invece la DCM (che al suo interno usa dei PID) calcolo il tempo in micros() trascorso dall'ultimo update. Funziona meglio così che in un loop a tempo "fisso" (richiesta dati i2c, update, stampa su seriale, flush seriale) però non ho usato i timer.

Se facciamo girare il PID a un tempo fisso, più in fretta dell'attuatore, non "inganniamo" la parte integrale del pid?
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Se facciamo girare il PID a un tempo fisso, più in fretta dell'attuatore, non "inganniamo" la parte integrale del pid?

Il PID deve essere molto più veloce del sistema, è il presupposto base per il suo funzionamento.
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

uff questo non era spiegato sulle cose che ho letto.. mannaggia  smiley-mr-green
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Considera che il PPM "classico" lavora sui 50Hz, quindi cambiare più spesso la velocità non serve a nulla

Ma ti devo cazziare subito oppure preferisci che ti faccio recapitare un invito premio per il quizzettone ?  smiley-grin
Regola prima, il ciclo pid DEVE essere almeno 10 volte più veloce della costante di tempo del sistema, meglio se 20 volte.
Regola seconda, lo slicing del pid non solo deve essere molto preciso, non deve essere affetto da jitter, la millis,e pure la micros, buttale in quel posto e tira la catena che non ci fa nulla.

Allora mi merito un voto positivo!!! smiley

Dunque il tutto non funzionava perchè grazie all'interrupt chiamavo la funzione pid che a sua volta usando la comunicazione I2C legge i sensori dal gyro ma lo fa tramite interrupt e quindi essendo chiamato da un interrupt andava in stallo poiché gli interrupt sono bloccanti. Quindi é corretto acquisire i dati dal gyro nel loop???

Ora di interrupt ne dovrei avere abbastanza, 4 per i canali radio, uno per il pid e uno per la libreria servo.h!!

Voi come acquisite i segnali dalla radio?!
Conoscete un qualcosa di già pronto per programmare il timer nome nel link che ho postato prima ma tipo les timer3 che nn va in conflitto con servo.h????

Grazie!!
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

io per leggere la radio uso un codice modificato dal bironpilot che usa gli interrupt sui pin e non sugli interrupt, quindi dovrebbe essere ok
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

io per leggere la radio uso un codice modificato dal bironpilot che usa gli interrupt sui pin e non sugli interrupt, quindi dovrebbe essere ok
Esatto, tipo prendi l'interrupt 0 che é associato al pinX quando si alza ti salvi il micros del momento attuale, quando il segnale scende fai la differenza fra il micros vecchio e quello attuale e sai quanto é durato il segnale e questo ti da il PPM, giusto? Lo fai con attachInterrupt?
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

no, ci sono 3 interrupt che gestiscono tutti i pin a gruppi di 8. Quindi ricevi uno di questi 3 interrupt (io ne attivo solo uno rendendo usabili quindi solo 8 pin, più che sufficienti per una TX), fai uno xor del valore attuale dei pin con quello precedente e ottieni i pin che sono cambiati. A questo punto elabori il cambiamento
Io attualmente leggo 4 pin: 2 4 5 6

https://github.com/lestofante/arduinoSketch/blob/master/QuadricotteroCompleto/RX/InputPin.cpp
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

no, ci sono 3 interrupt che gestiscono tutti i pin a gruppi di 8. Quindi ricevi uno di questi 3 interrupt (io ne attivo solo uno rendendo usabili quindi solo 8 pin, più che sufficienti per una TX), fai uno xor del valore attuale dei pin con quello precedente e ottieni i pin che sono cambiati. A questo punto elabori il cambiamento
Io attualmente leggo 4 pin: 2 4 5 6

https://github.com/lestofante/arduinoSketch/blob/master/QuadricotteroCompleto/RX/InputPin.cpp

Ma quindi hai un integrato che ti fa questo XOR?
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

no, è un'operazione logica smiley creca su wikipedia o qualche manuale di C
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

no, è un'operazione logica smiley creca su wikipedia o qualche manuale di C

Si l'operazione logica la conosco, non avevo capito il fatto degli interupt legati a N bit di arduino, in questo modo se non ho capito male riesci con un solo interupt a gestire tutti i 4 canali.

Hai qualche link di documentazione?! A questo punto sto pensando di cambiare scheda, prenderei uno mini pro 05 più leggera e ha tutto quello che serve se alla fine ogni pin di arduino può essere usato come interupt.
Logged

0
Offline Offline
Shannon Member
****
Karma: 130
Posts: 10453
:(){:|:&};:
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

sì, la mini va benissimo, ma per sviluppare l'arduino classica è più comoda. Ti consiglio un doppio sistema, uno su UNO con cui fai i test e sviluppi la libreria e la mini che sta a bordo su cui carichi il codice. tra l'altro puoi evitare di comprare il chip FTDI se usi la uno come programmatore della mini (vedi guida di menniti)

Come ti dicevo gli interrupt sui pin sono gestiti 8 alla volta, perchè così in 3 registri di 1 byte ciascuno, ogni bit corrisponde ad un pin. In realtà per ogni gruppo ci sono 3 registri: lettura, scrittura(o pull up se settato a input), tipo(input/output), più i vari registri per gli interrupt.

una guida per iniziare a capirci qualcosa: http://www.arduino.cc/en/Reference/PortManipulation
Logged

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"
#include <avr/interrupt.h> 
#include <avr/io.h>

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo motoreSX;
Servo motoreDX;
float velSX;
float velDX;


float P=0;
float I=0;
float D=0;
float PID;

float kP=25;
float kI=0;
float kD=0;

float MaxP=30;
float MinP=-30;
float MaxI=40;
float MinI=-40;
float MaxD=15; //15
float MinD=-15;

int roll=0;
int rollOld=0;
float val_Consegna=0;

//interrupt timer, call PID_callback() every 5 ms
ISR(TIMER2_OVF_vect) {
  count++;               //Increments the interrupt counter
  if(count > 4){
     PID_callback();
    count = 0;           //Resets the interrupt counter
  }
  TCNT2 = 130;           //Reset Timer to 130 out of 255
  TIFR2 = 0x00;          //Timer2 INT Flag Reg: Clear Timer Overflow Flag
}; 

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  Serial.begin(9600);
 
  // initialize device

  accelgyro.initialize();

  /* verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
   */
  motoreDX.attach(8);
  motoreDX.writeMicroseconds(800);
  motoreSX.attach(7);
  motoreSX.writeMicroseconds(800);
  delay(5000);

  for(int x=800; x<=1200; x++){
    motoreDX.writeMicroseconds(x);
    motoreSX.writeMicroseconds(x);
  }
  velSX=velDX=1200;
 
  TCCR2B = 0x00;        //Disbale Timer2 while we set it up
  TCNT2  = 130;         //Reset Timer Count to 130 out of 255
  TIFR2  = 0x00;        //Timer2 INT Flag Reg: Clear Timer Overflow Flag
  TIMSK2 = 0x01;        //Timer2 INT Reg: Timer2 Overflow Interrupt Enable
  TCCR2A = 0x00;        //Timer2 Control Reg A: Normal port operation, Wave Gen Mode normal
  TCCR2B = 0x05;        //Timer2 Control Reg B: Timer Prescaler set to 128

}

//Implementatio of PID
float Pid(float val_Consegna){

  if(kP!=0){
    P =  roll / kP;
    if(P>MaxP) P=MaxP;
    if(P<MinP) P=MinP;
  }
 
  if(kI!=0){
    I = I + ((roll/2) * kI);
    if(I>MaxI) I=MaxI;
    if(I<MinI) I=MinI;
  }
 
  if(kD!=0){
    D = (roll - rollOld) * kD;
    if(D>MaxD) D=MaxD;
    if(D<MinD) D=MinD;
  }
 
  float PID = P + I + D;

  rollOld = roll;

  return PID;
}
//calback for PID and update motor state
void PID_callback(){
 
  velDX=1200-PID;
  velSX=1200+PID;

  //Guardie, fa rimanere la velocità nei limiti di sicurezza
  if(velDX>1400) velDX=1400;
  if(velSX>1400) velSX=1400;
  if(velDX<1100) velDX=1100;
  if(velSX<1100) velSX=1100;

  motoreSX.writeMicroseconds(int(velSX));
  motoreDX.writeMicroseconds(int(velDX));

  PID=Pid(0);

}

void loop() {
  // read raw accel/gyro measurements from device
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  roll=map(constrain(ay, -16000, 16000), -16000, 16000, -1000, 1000);
  /*   Serial.print(P);
   Serial.print("\t");
   Serial.print(I);
   Serial.print("\t");
   Serial.print(D);
   Serial.print("\t");
   Serial.print(interval);
   Serial.print("\t");
   Serial.print(roll);
   Serial.print("\t");Serial.print(velSX); Serial.print("\t");Serial.println(velDX);
   */


  Serial.print(velSX);
  Serial.print(" ");
  Serial.print(velDX);
  Serial.print("\n");
  //delay(500);

}

Questo è il mio programma che implementa il PID, non ci salto fuori, mi è difficilissimo regolare il tutto, magari compio qualche errore grossolano che non vedo...

Il valore ay corrisponde al valore del giroscopio, se lo inclino è stabile quindi dovrebbe essre quello.

Grazie!
Logged

Pages: 1 ... 84 85 [86] 87 88 ... 101   Go Up
Jump to: