Robot balansujący

[patryk0493]

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