Skocz do zawartości

Robot Balansujący


kryska016

Pomocna odpowiedź

Cześć, Od dłuższego czasu zamierzałem zbudować robota balansującego i w końcu zabrałem się za budowę. Od razu zaznaczę że nie jestem zbyt dużo doświadczony w budowaniu robotów i programowaniu i będzie to mój 1 robot. Aby przybliżyć temat na początku wypiszę elementy które użyłem:

silniki nema17

sterowniki do silników: a4988

żyroskop GY-521

bluetooth: HC-05

Arduino UNO

Kwestia mechaniczna nie sprawiła mi sporo kłopotów, w miarę udało mi się to przyzwoicie złożyć. Problem pojawił się przy napisaniu programu. Znalazłem przykładowe programy do żyroskopu, silników i komunikacji. Jeżeli używam danego programu do obsługi konkretnej rzeczy to wszystko działa ok. Problem pojawia się gdy próbuję uzależnić obrót silników od żyroskopu. Do żyroskopu użyłem znalezionego w internecie programu a mianowicie filtr kalmana. Dodałem do niego sterowanie silnikami uzależnione od wychylenia a mianowicie silnik włącza się przy danym wychyleniu w daną stronę a wyłącza się gdy żyroskop wskaże 0. Niestety silnik nie chodzi poprawnie, kręci się dużo wolniej niż powinien dodatkowo dziwnie bucząc oraz jego obrót nie jest "czysty" ( minimalnie zmienia częstotliwość obracania się), dodatkowo wartość odczytywana z żyrokoskopu w momencie załączenia silnika albo gwałtownie skoczy lub zmaleje o parę stopni, albo wartość ta zacznie sobie pulsować wraz z pulsowaniem buczenia silnika. Poniżej zamieszczam okrojony program a mianowicie uwzględniający tylko 1 silnik który ma uruchomić się przy osiągnięciu wychylenia -10 lub 10. (masy połączyłem ze sobą)

program:

 #include <Wire.h>
#include <Kalman.h> 
#define RESTRICT_PITCH 

Kalman kalmanX; 
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine

void setup() {
 pinMode(4,OUTPUT); 
pinMode(3,OUTPUT);
pinMode(13,OUTPUT); 
pinMode(12,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);

 Serial.begin(115200);
 Wire.begin();
#if ARDUINO >= 157
 Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
 TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
 i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
 i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
 i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
 while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
 while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

 while (i2cRead(0x75, i2cData, 1));
 if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
   Serial.print(F("Error reading sensor"));
   while (1);
 }

 delay(100); // Wait for sensor to stabilize

 /* Set kalman and gyro starting angle */
 while (i2cRead(0x3B, i2cData, 6));
 accX = (i2cData[0] << 8) | i2cData[1];
 accY = (i2cData[2] << 8) | i2cData[3];
 accZ = (i2cData[4] << 8) | i2cData[5];

 // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
 // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
 // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
 double roll  = atan2(accY, accZ) * RAD_TO_DEG;
 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
 double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

 kalmanX.setAngle(roll); // Set starting angle
 kalmanY.setAngle(pitch);
 gyroXangle = roll;
 gyroYangle = pitch;
 compAngleX = roll;
 compAngleY = pitch;

 timer = micros();
}

void loop() {
 /* Update all the values */
 while (i2cRead(0x3B, i2cData, 14));
 accX = ((i2cData[0] << 8) | i2cData[1]);
 accY = ((i2cData[2] << 8) | i2cData[3]);
 accZ = ((i2cData[4] << 8) | i2cData[5]);
 tempRaw = (i2cData[6] << 8) | i2cData[7];
 gyroX = (i2cData[8] << 8) | i2cData[9];
 gyroY = (i2cData[10] << 8) | i2cData[11];
 gyroZ = (i2cData[12] << 8) | i2cData[13];

 double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
 timer = micros();

 // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
 // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
 // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
 double roll  = atan2(accY, accZ) * RAD_TO_DEG;
 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
 double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

 double gyroXrate = gyroX / 131.0; // Convert to deg/s
 double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
   kalmanX.setAngle(roll);
   compAngleX = roll;
   kalAngleX = roll;
   gyroXangle = roll;
 } else
   kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

 if (abs(kalAngleX) > 90)
   gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
   kalmanY.setAngle(pitch);
   compAngleY = pitch;
   kalAngleY = pitch;
   gyroYangle = pitch;
 } else
   kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

 if (abs(kalAngleY) > 90)
   gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

 gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
 gyroYangle += gyroYrate * dt;
 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
 //gyroYangle += kalmanY.getRate() * dt;

 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

 // Reset the gyro angle when it has drifted too much
 if (gyroXangle < -180 || gyroXangle > 180)
   gyroXangle = kalAngleX;
 if (gyroYangle < -180 || gyroYangle > 180)
   gyroYangle = kalAngleY;

 /* Print Data */
#if 0 // Set to 1 to activate
 Serial.print(accX); Serial.print("\t");
 Serial.print(accY); Serial.print("\t");
 Serial.print(accZ); Serial.print("\t");

 Serial.print(gyroX); Serial.print("\t");
 Serial.print(gyroY); Serial.print("\t");
 Serial.print(gyroZ); Serial.print("\t");

 Serial.print("\t");
#endif

 Serial.print(roll); Serial.print("\t");
 Serial.print(gyroXangle); Serial.print("\t");
 Serial.print(compAngleX); Serial.print("\t");
 Serial.print(kalAngleX); Serial.print("\t");

 Serial.print("\t");

 Serial.print(pitch); Serial.print("\t");
 Serial.print(gyroYangle); Serial.print("\t");
 Serial.print(compAngleY); Serial.print("\t");
 Serial.print(kalAngleY); Serial.print("\t");

#if 0 // Set to 1 to print the temperature
 Serial.print("\t");

 double temperature = (double)tempRaw / 340.0 + 36.53;
 Serial.print(temperature); Serial.print("\t");
#endif

 Serial.print("\r\n");
 delay(2);
 {if (kalAngleY > 10  )
digitalWrite((9),HIGH);
if (kalAngleY < 0)
digitalWrite((9),LOW);

if (kalAngleY < -10 )
digitalWrite((10),HIGH);
if (kalAngleY > 0)
digitalWrite((10), LOW);}


if (digitalRead(9)==HIGH )
{digitalWrite(3,HIGH);
{ digitalWrite(4,HIGH); 
   delayMicroseconds(500); 
   digitalWrite(4,LOW); 
   delayMicroseconds(500); 
}
}
else if (digitalRead(10)==HIGH )
{digitalWrite(3,LOW);
{ digitalWrite(4,HIGH); 
   delayMicroseconds(500); 
   digitalWrite(4,LOW); 
   delayMicroseconds(500); 
}
}
}

oraz i2c


const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
 return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
 Wire.beginTransmission(IMUAddress);
 Wire.write(registerAddress);
 Wire.write(data, length);
 uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
 if (rcode) {
   Serial.print(F("i2cWrite failed: "));
   Serial.println(rcode);
 }
 return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
 uint32_t timeOutTimer;
 Wire.beginTransmission(IMUAddress);
 Wire.write(registerAddress);
 uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
 if (rcode) {
   Serial.print(F("i2cRead failed: "));
   Serial.println(rcode);
   return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
 }
 Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
 for (uint8_t i = 0; i < nbytes; i++) {
   if (Wire.available())
     data[i] = Wire.read();
   else {
     timeOutTimer = micros();
     while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
     if (Wire.available())
       data[i] = Wire.read();
     else {
       Serial.println(F("i2cRead timeout"));
       return 5; // This error value is not already taken by endTransmission
     }
   }
 }
 return 0; // Success
}

Z góry dziękuję wszystkim za udzielenie mi pomocy. Pozdrawiam

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.