Robot Autobilanciato

Salve a tutti, sto provando da un po’ di tempo a fare un robot che cammina su due ruote. Ho iniziato facendo una “struttura” a forma di T rovesciata dove ho posizionato questi motori http://www.hobbyking.com/hobbyking/store/_26255__Geared_Motor_Offset_Shaft_2Pcs_Bag.html e questo accelerometro http://www.hobbyking.com/hobbyking/store/__26849__Arduino_ADXL335_Angle_Sensor_Module.html . Adesso voglio controllarlo con il PID ma non riesco a farlo stare per niente in piedi.

Questo è il codice che ho scritto:

float time = 0;                               // Tempo auttuale, dall'accensione del dispositivo
float previousTime = 0;                       // Tempo del precedente ciclo
float interval = 0;                           // Tempo impiegato per l'esecuzione di un ciclo di programma
float previousErrore = 0;                       // Tempo del precedente ciclo

float P = 0;                                  // Variabile PROPORZIONALE  del PID
float I = 0;                                  // Variabile INTEGRATIVA del PID
float D = 0;                                  // Variabile DERIVATIVA del PID
float PID = 0; 

int MOVE;

int pwm;

float kP = 60; // Coefficiente PROPORZIONALE del PID 

float kI = 7; // Coefficiente INTEGRATIVA del PID 

float kD = 3 ; // Coefficiente DERIVATIVA del PID

//Motors Controll
#define Forw1 10
#define Forw2 12
#define Backw1 11
#define Backw2 13

//GIORO VARIABLES
const float alpha = 0.5;

double fXg = 0;
double fYg = 0;
double fZg = 0;
double angle;

void setup() { 
  Serial.begin(115200);
  /////////////////////////Motors
  pinMode(Forw1, OUTPUT);
  pinMode(Forw2, OUTPUT);
  pinMode(Backw1, OUTPUT);
  pinMode(Backw2, OUTPUT);
}

void loop() {
  
  Serial.print( "  Errore =  " );          // Stampa dei dati 
  Serial.print( angle ); 
  Serial.print( "  Intervallo =  " );    
  Serial.print( interval );
  Serial.print( "  P =  " );    
  Serial.print( P ); 
  Serial.print( "  I =  " );    
  Serial.print( I ); 
  Serial.print( "  D =  " );    
  Serial.print( D ); 
  Serial.print( "  PID =  " );    
  Serial.print( PID ); 
  Serial.print( "  PWM =  " );
  Serial.println( pwm );
  
  time = millis();                         // Salva il tempo corrente
  interval = (time - previousTime)/1000;   // Calcola il tempo impiegato per eseguire un ciclo in secondi
  
  Giro();
  getPID();
 
  previousTime = time;                     // Memorizza il tempo attuale
  previousErrore = angle;                 // Memorizza posizione attuale
}


//////////////////////////////////////////////////MOTORS Setup
void goForward(byte a){
  digitalWrite(Forw1,a);
  digitalWrite(Forw2,a);
  digitalWrite(Backw1, LOW);
  digitalWrite(Backw2, LOW);
}

void goBackward(byte a){
  digitalWrite(Forw1,LOW);
  digitalWrite(Forw2,LOW);
  digitalWrite(Backw1,a);
  digitalWrite(Backw2,a);
}

void turnLeft(){
  digitalWrite(Forw1,LOW);
  digitalWrite(Forw2,HIGH);
  digitalWrite(Backw1,HIGH);
  digitalWrite(Backw2,LOW);
}

void turnRigth(){
  digitalWrite(Forw1,HIGH);
  digitalWrite(Forw2,LOW);
  digitalWrite(Backw1,LOW);
  digitalWrite(Backw2,HIGH);
}

void Fermo(){
  getPID();
}

//////////////////////////////////////////////////GIRO
void Giro(){
  double pitch, roll, Xg, Yg, Zg;
  //Low Pass Filter
  fXg = digitalRead(A0) * alpha + (fXg * (1.0 - alpha));
  fYg = digitalRead(A1) *  + (fYg * (1.0 - alpha));
  fZg = digitalRead(A2) * alpha + (fZg * (1.0 - alpha));
  
  //Roll & Pitch Equations
  roll  = (atan2(-fYg, fZg)*180.0)/M_PI;
  pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;
  
  angle =map(pitch,38,50,0,90)-90+MOVE;
}

//////////////////////////////////////////////////PID

void ProporzionalAction(){
  P=kP*angle;
}

void IntegralAction(){
  float array[50];
  float somma;
  for(int i = 0; i<=50; i++){
    array[i]=angle;
    somma = somma+array[i];
  }
  float MError= somma/50;
  
  I=(kI*MError);
}

void DerivateAction(){
  
  D = kD * ( angle - previousErrore ) / interval;  // Calcola la variabile derivativa
}

void getPID(){
  //P = Errore * kP;                                  // Calcola la variabile proporzionale 
  //I = I + (P * interval * kI) ;                     // Calcola la variabile integrativa
  //D = kD * ( Errore - previousErrore ) / interval;  // Calcola la variabile derivativa
  
  
  ProporzionalAction();
  IntegralAction();
  DerivateAction();
  
  PID = P+I+D;
  
  pwm = constrain(PID,-255,255);
  
  if(pwm<0){
    goForward(pwm*-1);
  }else{
    goBackward(pwm);
  }
  
}

Potreste aiutarmi ,grazie !!

void goForward(byte a){
  digitalWrite(Forw1,a);
  digitalWrite(Forw2,a);
  digitalWrite(Backw1, LOW);
  digitalWrite(Backw2, LOW);
}

Il PWM non funziona con digitalWrite() ma con analogWrite(). Non so se il resto é corretto. Ciao Uwe

Okk grazie lo cambierò e vi faccio sapere come va ;)