Skocz do zawartości

Robot balansujący


patryk0493

Pomocna odpowiedź

Witam,

przychodzę o kilka porad w pisaniu programu oraz ogólnych porad w budowie robota balansującego.

Opisze jego budowę:

- silniki Pololu 25D 34:1 165obr/min z enkoderami

- sterownik silnika Pololu Dual MC33926

- arduino Leonardo

- żyroskop + akceerometr - AltIMU-10 v4

- pakiet 7,4V, 2200mA

- koła o średnicy

Program dla arduino:

clude <Encoder.h>
#include <Servo.h>
#include <Wire.h>
#include <LSM303.h>
#include <Kalman.h>
#include <L3G.h> //Zyroskop
#include "DualMC33926MotorShield.h"

DualMC33926MotorShield md;
Servo servo; 
L3G gyro;
LSM303 compass;
Encoder motorL(0, 5);
Encoder motorP(1, 6);
Kalman kalX(4.0, 2.1, 0.5);
Kalman kalY(4.0, 2.1, 0.5);
Kalman kalZ(4.0, 2.1, 0.5);
Kalman gX(5.0, 1.05, 1.5);
Kalman FAngle(2.0, 1.1, 0.5);
// Process noise variance for the accelerometer  Q_angle
// Process noise variance for the gyro bias  Q_bias
// Measurement noise variance - this is actually the variance of the measurement noise R_measure = q_r;

void setup(){
 delay(1000);
 Serial.begin(19200);
 Wire.begin();
 compass.init();
 compass.enableDefault();
 gyro.init();
 gyro.enableDefault(); 
 md.init();
}

unsigned long timenow, timelast;
float acceleroZ = 0.00, acceleroX = 0.00, acceleroY = 0.00, g = 0, dT;
float gyroXangle = 0, rate_gyr_x;
float CurrentAngle;
float ANGLE = 90.2; //stabiny kat

int v; // predkosc silnikow

void getData() {
 gyro.read();
 compass.read();
}

void stopIfFault() {
 if (md.getFault()) {
   Serial.println("Blad");
   while(1);
 }
}

// ================================ PID ==============================
float Kp = 30;
float Ki = 0;
float Kd = 5;
float pTerm, iTerm, dTerm, integrated_error = 0, last_error, error;
const float K = 1.3;
#define   GUARD_GAIN   10.0

void Pid(){
 error = ANGLE - CurrentAngle;  // -4 = level
 pTerm = Kp * error;
 integrated_error += error;
 iTerm = Ki * integrated_error;
 //iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);
 last_error = error;
 v = constrain (K * (pTerm + iTerm + dTerm), -400, 400);
}

int startInt;
void loop() {
 // czas petli oraz jego ograniczenie
 startInt = millis();
 timenow = micros();
 dT = (timenow - timelast) / 1000.0;
 timelast = timenow;

 getData();

// skalibrowany akcelerometr
 acceleroX = map((compass.a.x - 250) * 1.014070, -16144, 16144, -10000, 10000) / 100.0;
 acceleroY = map((compass.a.y - 50) * 1.0058566, -16144, 16144, -10000, 10000) / 100.0;
 acceleroZ = map((compass.a.z - 850) * 1.015345, -16144, 16144, -10000, 10000) / 100.0; 

 float angle = (atan2(acceleroZ,acceleroY) + PI) * 57.29577;
 Serial.print(angle);
 Serial.print(", ");

 //rate_gyr_x = (gyro.g.x + 300) * 0.00875;
 //500 /s
 rate_gyr_x = (gyro.g.x + 300) * 0.00175;  // deg/s 

 //gyroXangle = rate_gyr_x * (double)dT/1000.00;
 gyroXangle = rate_gyr_x * 1;
 Serial.print(gyroXangle);
 Serial.print(", ");
 //gyroXangle = gX.getAngle(gyroXangle, angle);
 CurrentAngle = FAngle.getAngle(angle, gyroXangle, (double)dT/1000.0);
 //CurrentAngle = angle;
 Serial.print(CurrentAngle);

 Serial.print(", ");
 Serial.print((double)dT/1000.0 , 3);

 Serial.print(", ");
 Serial.print(v);

 Serial.print(", ");
 Serial.print(motorL.read()/48);
 Serial.print(", ");
 Serial.println(motorP.read()/48);


 if (CurrentAngle >= ANGLE + 1 && CurrentAngle <= ANGLE - 1 ) {
   md.setM1Speed(0);
   md.setM2Speed(0);
   stopIfFault();
 } else {
   if (CurrentAngle < ANGLE + 50 && CurrentAngle > ANGLE - 50) {
     Pid();
     md.setM1Speed(v);
     md.setM2Speed(v);
     stopIfFault();
   } else {
     Pid();
     md.setM1Speed(0);
     md.setM2Speed(0);
     stopIfFault();
   }
 }

 //wykonywanie petli co 20ms
 while(millis() - startInt < 20) { delayMicroseconds(100); } 
}

Co do konstrukcji - ma ona 20cm wysokości, środek ciężkości położony jest w 2/3 wys.

Postanowiłem przefiltrować wstępnie dane z każdej osi, lecz ten pomysł w zamyśle był chybiony.

Generalnie mam problem z doborem parametrów konstruktora filtra Kalmana. Generalnie nie jestem pewien czy też właściwie obliczam jego prędkość kątową. Czy działanie pętli z częstotliwością 50Hz jest optymalne ?

Miałem też problem z doborem parametrów PID, lecz na razie nie ma raczej sensu tego rozgrzebywać jeśli nie mam prawidłowo innych rzeczy. Zwykle robot oscylował zbyt szybko, nie zdołał utrzymać równowagi dłużej niż 1s.

Z góry dziękuję za wskazówki

Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.