Skocz do zawartości
S0wa

Robot samobalansujący

Pomocna odpowiedź

@S0wa, właśnie zaakceptowałem opis. Dziękuję za przedstawienie ciekawego projektu, zachęcam do prezentowania kolejnych DIY oraz aktywności na naszym forum 😉

Udostępnij ten post


Link to post
Share on other sites

Witam,

Ciekawy projekt, chciałbym spróbować skonstruować podobnego robota. Chętnie zobaczył bym kod źródłowy dla tego robota.

Udostępnij ten post


Link to post
Share on other sites
#include <Wire.h>

///////////////////////MPU Data/////////////////////////
long rawAccelX, rawAccelY, rawAccelZ; // raw data from accel
float accelX, accelY, accelZ; //calculated data from Accel

long rawGyroX, rawGyroY, rawGyroZ; //raw data from gyro
float gyroX, gyroY, gyroZ; //calculated data from Gyro 
///////////////////////MPU Data//////////////////////////

//////////////////////Time and angle/////////////////////////
float elapsedTime, timePrev, time;
float totalAngle; //Real, filtered angle from MPU
float desiredAngle=-2; //Desired angle of robot 
//////////////////////Time and angle/////////////////////////

//////////////////////PID/////////////////////////////////// 
float pid, error, prevError;
float pidP;
float pidI;
float pidD;

double kp=70; //<do ustawienia>>
double ki=1.2; //<do ustawienia>>
double kd=0.7; //<do ustawienia>>
//////////////////////PID///////////////////////////////////

//////////////////////PIDA//////////////////////////////////
int pidA, errorA, prevErrorA;
int desiredStepsA=0;   //
float pidPA;
float pidIA;
float pidDA;

double kpA=0.001; //<do ustawienia>>
double kiA=0.0000015; //<do ustawienia>>
double kdA=0.0015; //<do ustawienia>>
//////////////////////PIDA//////////////////////////////////


//////////////////////Motors////////////////////////////////
int in1=8; 
int in2=7;
int in3=5;
int in4=4;

int enA=9;
int enB=6;

int enc1=3;
int enc2=2;
int enc3=12;
int enc4=11;

int stepsA=0;
int stepsB=0;

int speedA;
int speedB;
//////////////////////Motors////////////////////////////////

/////////////////////Other/////////////////////////
float rad_to_deg = 180/3.141592654;
/////////////////////Other/////////////////////////


void setup() {
  pinMode(in1, INPUT);
  pinMode(in2, INPUT);
  pinMode(in3, INPUT);
  pinMode(in4, INPUT);
  pinMode(enc1, INPUT);
  pinMode(enc2, INPUT);
  pinMode(enc3, INPUT);
  pinMode(enc4, INPUT);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  Wire.begin();
  
  setupMPU(); //setups MPU

  attachInterrupt(digitalPinToInterrupt(enc1), steps1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(enc2), steps2, CHANGE);

  time = millis();
}

void loop() {
  timePrev = time;
  time = millis();
  elapsedTime = (time - timePrev) / 1000;
  
  readAccelData(); //reads data from accelerometer
  readGyroData(); //reads data from gyroscope
  calculateRealAngle(); //calculates real angle
  calculatePIDA();
  calculatePID(); //calculates PID value
  action();
}

void setupMPU() {
  Wire.beginTransmission(0b1101000); //Begins transmision with MPU
  Wire.write(0x6B);  //
  Wire.write(0b00000000); //Turns sleep mode off
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1B); //
  Wire.write(0x00000000); //Sets Gyro scale to +/- 250 deg/sec <do ustawienia>
  Wire.write(0x1C); //
  Wire.write(0x00000000); //Sets Accel scale to +/- 2g <do ustawienia>
  Wire.endTransmission();
}

void readAccelData() {
  Wire.beginTransmission(0b1101000);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Requests Accel registers
  while (Wire.available()<6);
  rawAccelX = Wire.read()<<8|Wire.read();
  rawAccelY = Wire.read()<<8|Wire.read();
  rawAccelZ = Wire.read()<<8|Wire.read();

    //Process Accel data to sth usefull
  accelX = rawAccelX / 16384.0; //<- depends on Accel scale
  accelY = rawAccelY / 16384.0; //<- depends on Accel scale
  accelZ = rawAccelZ / 16384.0; //<- depends on Accel scale
}

void readGyroData() {
  Wire.beginTransmission(0b1101000);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Requests Gyro registers
  while (Wire.available()<6);
  rawGyroX = Wire.read()<<8|Wire.read();
  rawGyroY = Wire.read()<<8|Wire.read();
  rawGyroZ = Wire.read()<<8|Wire.read();

    //Process Gyro data to sth usefull
  gyroX = rawGyroX / 131.0; //<- depends on Gyro scale
  gyroY = rawGyroY / 131.0; //<- depends on Gyro scale
  gyroZ = rawGyroZ / 131.0; //<- depends on Gyro scale
}

void calculateRealAngle() {
  float accelAngle = atan(accelY/sqrt(pow(accelX,2) + pow(accelZ,2)))*rad_to_deg; //Calculates Accel angle in X axis
  totalAngle = 0.98 *(totalAngle + gyroX * elapsedTime) + 0.02*accelAngle; //Real filtered angle in X axis
  
}

void calculatePID() {
  error = totalAngle - desiredAngle+pidA;
  pidP = kp*error;
  pidI = pidI+(ki*error);
  pidD = kd*((error-prevError)/elapsedTime);
  pid = pidP+pidI+pidD;
  prevError = error;
}

void action() {

  speedA = constrain(abs(pid), 0, 255);
  speedB = constrain(abs(pid), 0, 255);
  
  if (pid >0) {   
    moveForward();
  }
  else if (pid <0){ 
    moveBackward();
  }
  
}

void steps1(){
  if (digitalRead(enc1) == HIGH) {
    if (digitalRead(enc3) == LOW) {
      stepsA++;
    } else {
      stepsA--;
    }
  } else {
    if (digitalRead(enc3) == LOW) {
      stepsA--;
    } else {
      stepsA++;
    }
  }
}
void steps2(){
  if (digitalRead(enc2) == HIGH) {
    if (digitalRead(enc4) == LOW) {
      stepsB--;
    } else {
      stepsB++;
    }
  } else {
    if (digitalRead(enc4) == LOW) {
      stepsB++;
    } else {
      stepsB--;
    }
  }
}

void moveForward(){ //moves motors forward
  digitalWrite (in1, HIGH);
  digitalWrite (in2, LOW);
  digitalWrite (in3, HIGH);
  digitalWrite (in4, LOW);

  analogWrite(enA, speedA);
  analogWrite(enB, speedB);
}

void moveBackward(){ //moves motors backward
  digitalWrite (in1, LOW);
  digitalWrite (in2, HIGH);
  digitalWrite (in3, LOW);
  digitalWrite (in4, HIGH);

  analogWrite(enA, speedA);
  analogWrite(enB, speedB);
}

void calculatePIDA() {    //compensates position drift
  errorA = stepsA - desiredStepsA;
  pidPA = kpA*errorA;
  pidIA = pidIA+(kiA*errorA);
  pidDA = kdA*((errorA-prevErrorA)/elapsedTime);
  pidA = pidPA+pidIA+pidDA;
  prevErrorA = errorA;
}


Proszę bardzo. Z pewnością nie jest to kod, którym można się chwalić, ale mniej lub bardziej działa. Starałem się go komentować, ale na pewnym etapie prac wymknęło się to spod kontroli. Na wszystkie pytania postaram się odpowiedzieć 🙂 

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

Po przeskalowaniu tego projektu o 200%-300% widzę zastosowanie dla osób niepełnosprawnych poruszających się na wózku. Bądź osób element rehabilitacji czy wsparcia w problemie z chodzeniem, czy po amputacji nóg/nogi.

Świetny DIY.

Udostępnij ten post


Link to post
Share on other sites

@adusn86 Dzięki 😉 Projekt nie powstawał z myślą poprawy świata, bardziej służył mi nauce nowych rzeczy, a przy okazji załapał się jako projekt szkolny. Segwaye i hoverboardy działają na tej samej zasadzie, więc ktoś już to powiększył, ja tylko zmniejszyłem aby zmieścić się w budżecie 🙂. Są to jednak głównie zabawki (chociaż segwaye znalazły zastosowanie np. w branży ochroniarskiej), a mój robot bardziej obrazuje zasadę działania żyroskopu. W ten sam sposób sterowane są np. drony, tylko że w robocie mamy kontrolę jednej osi, a w dronach dwóch.  

Udostępnij ten post


Link to post
Share on other sites

Dokładnie, Segwaye znalazły zastosowanie, tylko w dalszym ciągu uważam że ceny tej technologi - żyroskopu w mobilnych pojazdach (bez uwzględnienia kosztów ogniw) są przeszacowane. Powinni "klepać" protezy nóg nogi na potęgę. 

 

Udostępnij ten post


Link to post
Share on other sites

z ciekawości: po co w protezie nogi żyroskop? poważnie pytam...

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

@adusn86 Wszystko co ma efekt "wow" kosztuje. Żyroskop do mojego robota kosztował z 5 zł, ale podejrzewam że w Segwayach są dokładniejsze. Cały mój robot nie jest zbyt dokładny co widać po tym jak się trzęsie, choć to pewnie głównie zasługa silników. Są to drogie pojazdy, ale jak się doda wszystko razem to koszt części niski nie jest, a że ich główne zastosowanie jest w specjalistycznej branży to i marżę można dodać sobie większą.

@ethanak Zgaduję że dzięki temu taka noga będzie się sama naturalnie zginać w kolanie, tak jak to robi ludzka noga. Nogi mam swoje i nigdy nie miałem do czynienia z protezami, ale tak zgaduję. Drugą opcją która przychodzi mi do głowy to balansowanie tej nogi. Zauważ że jak stoisz, pomimo że się nie ruszasz to mięśnie w nodze cały czas pracują aby utrzymać cię w pionie, a szczególnie palce u stopy. Z tego co słyszałem, osobom które straciły palce u nóg ciężej jest utrzymać stabilność. Bardzo dobrze to widać na robocie, który pomimo że ma za zadanie stać prosto, cały czas musi korygować swoją pozycję i silniki cały czas pracują, w przeciwnym wypadku od razu się wywala. Osoby z protezą nogi często chodzą o kulach, chociaż teoretycznie ilość kończyn jest taka sama. Sztuczna noga nie zapewnia jednak tego balansu który zapewnia prawdziwa. 

To wszystko moje domysł, poczekajmy aż Adusn się wypowie 🙂 

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

To wszystko zależy od protezy.

Jeśli masz protezę poniżej kolana to po prostu się musisz przyzwyczaić do stopy.

Jeśli masz nad kolanem (i jakiś napęd tego kolana) to ważniejszy jest kąt zgięcia (zresztą mózg nie dostaje danych o bezwzględnym położeniu kończyny, ale właśnie o kątach).

To że ludzie z protezami chodzą o kulach to raczej normalne - żadna proteza nie zapewni dokładnego wyczucia położenia, a kula pomaga w wielu przypadkach w utrzymaniu równowagi (ewentualnie po porostu w tym, żeby nie polecieć na ryj jak się potkniesz). A jeśli masz sztywną protezę powyżej kolana, to i tak kula jest potrzebna choćby po to, aby zejść ze schodów bo sztywna proteza tego nie umożliwia.

Sam mam taką trzecią nogę (cukrzyca, dna i tak dalej) i potrzebuję jej wtedy kiedy tracę równowagę... a przecież obie nogi mam swoje...

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

@ethanak To ciekawe co piszesz. Trzeciej nogi potrzebujesz kiedy tracisz kontrolę nad swoimi nogami, jak zgaduję. W takiej sytuacji nie ma to większego znaczenia czy to prawdziwa noga, czy sztuczna, jeżeli kontrola nad nią jest ograniczona. Stanie prosto też nie jest takie proste jak się wydaje. Wymaga ciągłej korekcji swojej pozycji, co nasz organizm opanował bardzo dobrze, ponieważ punkt styku z podłożem jest relatywnie mały w porównaniu do wysokości ciała. Jeżeli postawimy coś podobnych rozmiarów do człowiek o takiej samej powierzchni styku, będzie to łatwo przewrócić, a wystarczy że nie będzie stało prosto i już się wywali. Człowiek może o wiele lepiej utrzymać stabilność, nawet w trudnych warunkach, kosztem ciągłej korekcji swojej pozycji. Jeżeli możliwość tej korekcji z jakiegoś powodu traci to się wywala. 

Tym samym podejrzewam że rozbudowanie protezy to jakiś system utrzymywania pozycji oparty na różnych czujnikach może umożliwić poruszanie się na takich protezach bez kul. Mówię tu oczywiście o protezach powyżej kolana ze zginanym kolanem i kostką. Wydaje mi się że już istnieją takie protezy, ale podejrzewam że są strasznie drogie.

Udostępnij ten post


Link to post
Share on other sites
Dnia 30.03.2019 o 21:28, S0wa napisał:

@ethanak To ciekawe co piszesz. Trzeciej nogi potrzebujesz kiedy tracisz kontrolę nad swoimi nogami, jak zgaduję. W takiej sytuacji nie ma to większego znaczenia czy to prawdziwa noga, czy sztuczna, jeżeli kontrola nad nią jest ograniczona. Stanie prosto też nie jest takie proste jak się wydaje. Wymaga ciągłej korekcji swojej pozycji, co nasz organizm opanował bardzo dobrze, ponieważ punkt styku z podłożem jest relatywnie mały w porównaniu do wysokości ciała. Jeżeli postawimy coś podobnych rozmiarów do człowiek o takiej samej powierzchni styku, będzie to łatwo przewrócić, a wystarczy że nie będzie stało prosto i już się wywali. Człowiek może o wiele lepiej utrzymać stabilność, nawet w trudnych warunkach, kosztem ciągłej korekcji swojej pozycji. Jeżeli możliwość tej korekcji z jakiegoś powodu traci to się wywala. 

Tym samym podejrzewam że rozbudowanie protezy to jakiś system utrzymywania pozycji oparty na różnych czujnikach może umożliwić poruszanie się na takich protezach bez kul. Mówię tu oczywiście o protezach powyżej kolana ze zginanym kolanem i kostką. Wydaje mi się że już istnieją takie protezy, ale podejrzewam że są strasznie drogie.

https://www.nibib.nih.gov/news-events/newsroom/powered-robotic-legs-leaping-toward-future

 

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

Przepraszam że wcześniej nie odpowiedziałem, ale po prostu gdzieś mi post zginął 😞

@S0waPisałeś o trudnościach w utrzymaniu równowagi w przypadku amputacji palców. To akurat jest dość proste - bez palców można spokojnie chodzić, palce po prostu działają jak tensometr. Zauważ: jeśli zbudujesz maszynę której zadaniem będzie stanie prosto i wyposażysz ją w tensometr mierzący siłę nacisku przed punktem podparcia (taką "piętą") będzie w stanie utrzymać równowagę odpowiednio balansując ciałem i kontrolując siłę naciskającą na ów tensometr. Za duża - przechylamy się za mocno do przodu; za mała - do tyłu. I dalej nie ma w nodze żadnych żyroskopów, akcelerometrów i tym podobnych - coś takiego masz w uchu, ale od nogi to trochę daleko 🙂 Zresztą - przy nierównym terenie, gdzie stopa ma stanąć nie prostopadle do wektora grawitacji, ale równolegle do przechylonego podłoża, żadne żyroskopy nie pomogą. Poza tym przyspieszenia działające na nogę w czasie chodzenia (nie wspominając o biegu) uczynią wskazania owego żyroskopu bezwartościowymi.

Natomiast przy wózku akcelerometr/żyroskop są konieczne; wózek ma utrzymać pozycję poziomą siedziska niezależnie od podwozia, inaczej po prostu się przewróci (zauważ: środek ciężkości wózka położony jest stosunkowo wysoko). Przyspieszenia są niewielkie, prędkości też. Tu jest większe pole do popisu - również dla konstrukcji balansujących. Zaprojektuj porządny wózek jeżdżący po schodach i pochylniach czy wjeżdżający skosem na krawężnik, to Cię inwalidzi ozłocą 🙂

 

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites

Bądź aktywny - zaloguj się lub utwórz konto!

Tylko zarejestrowani użytkownicy mogą komentować zawartość tej strony

Utwórz konto w ~20 sekund!

Zarejestruj nowe konto, to proste!

Zarejestruj się »

Zaloguj się

Posiadasz własne konto? Użyj go!

Zaloguj się »

×