Sužinokite atvirkštinę kinematiką su roboto rankos pynimu

Pažvelkime į mūsų grandinę ir pridėkime naudingą komponentą, kuris leis mums greitai ir lengvai valdyti mūsų projekto tikslo koordinatės vertę: vairasvirtę! Norėdami padidinti ir sumažinti mūsų kintamojo vertę, mes galime naudoti vairasvirtės ašį aukštyn žemyn, o tai pakelia mūsų robotą aukštyn ir žemyn, ir naudoti kairės dešinės vairasvirtės ašį, kad padidintume ir sumažintumėte mūsų X kintamojo vertę, judindami robotą iš kairės į dešinę.

Prijunkite teigiamą vairasvirtės įtampos kaiščią prie 5 V kaiščio ant Arduino per duonos plokštės galios bėgelius. Prijunkite GND kaištį prie „GND“ duonos lentos bėgio, o vairasvirtės X ir Y kaiščius – prie A0 ir A1 kaiščių ant Arduino.

Dabar, kai mes sujungėme visus komponentus, kurių mums reikia, paskutinis veiksmas bus mūsų atvirkštinės kinematikos lygčių konvertavimas į kodą ir pridedant keletą eilučių, kurių mums reikia norint perskaityti mūsų vairasvirtės vertes! Laimei, „Arduinos“ puikiai moka matematikos ir turi funkcijas kiekvienai matematinėms operacijoms, kurias mums reikės atlikti norėdami atlikti skaičiavimus, integruotus į programavimo kalbą.

#include "Servo.h"
Servo servoOne;
Servo servoTwo;
//Setting up our IK variables
double x = 30.0;
double y = 120.0;
double linkOne = 100.0;
double linkTwo = 100.0;
double alphaOne;
double alphaTwo;
double alphaFinal;
double betaOne;
double betaTwo;
double betaFinal;
double c;
double d = 60.0;
double e;
int xval;
int yval;

void setup() {
  Serial.begin(9600);
  servoOne.attach(3);
  servoTwo.attach(5);
}

void loop()
{
  //IK Calculations
  yval = analogRead(A0);
  xval = analogRead(A1);
  c = sqrt((x*x)+(y*y));
  e = sqrt(((d-x)*(d-x))+(y*y));
  alphaOne = atan(y/x) * (180/PI);
  alphaTwo = acos(((linkTwo*linkTwo)-(c*c)-(linkOne*linkOne))
            /(-2*linkOne*c)) * (180/PI);
  betaOne = atan(y/(d-x)) * (180/PI);
  betaTwo = acos(((linkTwo*linkTwo)-(e*e)-(linkOne*linkOne))
            /(-2*linkOne*e)) * (180/PI);
  if(x < 0){ alphaFinal = 180 + ((alphaOne) + (alphaTwo)); betaFinal = 180 - ((betaOne) + (betaTwo)); } else if(x > d){
      alphaFinal = ((alphaOne) + (alphaTwo));
      betaFinal = -1 * ((betaOne) + (betaTwo));
    }
  else{
      alphaFinal = ((alphaOne) + (alphaTwo));
      betaFinal = 180 - ((betaOne) + (betaTwo));
    }

  //Printing the results! 
  Serial.print("alpha One: ");
  Serial.print(alphaOne);
  Serial.print(" beta One:");
  Serial.print(betaOne);
  Serial.print(" alpha Two: ");
  Serial.print(alphaTwo);
  Serial.print(" beta Two:");
  Serial.print(betaTwo);
  Serial.print(" Alpha: ");
  Serial.print(alphaFinal);
  Serial.print(" Beta: ");
  Serial.println(betaFinal);
  servoOne.write(alphaFinal);
  servoTwo.write(betaFinal);

/*
Depending on your customization, it may be a good idea to add some limits here! If you see your arms acting squirrely, add a statement  such as “if(xVal > 800 && xVal < 200)” */ if(xval > 800){
    x-=5;
  }
  if(xval < 300){ x+=5; } if(yval > 800){
    y-=5;
  }
  if(yval < 300){
    y+=5;
  }
}

Suskirstykime kodą, kurį ką tik išsiuntėme savo „Arduino“. Vienas iš pirmųjų dalykų, kuriuos mūsų kodas atlieka x ir y koordinatės. Šiuo atveju buvo pasirinktos koordinatės (30, 120), kad robotas prasidėtų tinklelio centre. Šios vertės gali būti pakeistos, kad rašiklis būtų pradėtas bet kurioje vietoje, kur norite tinklo! Toliau galime sukurti likusius kintamuosius, kurių mums reikia mūsų skaičiavimams. Kiekvienas lygčių žingsnis kode turi savo kintamąjį.

Kintamieji Linkone ir Linktwo Atspindėkite mūsų sistemos nuorodų ilgį, kuris abu yra 100 mm. Toliau, kintamieji alfaonasAr AlphatwoAr alfafinalysAr BetaoneAr Betatwoir Betafinalinė Atspindi visus mūsų povandeninius ir galutinio kampo kintamuosius. Galiausiai c ir e Kintamieji yra mūsų teorinės trikampio linijos ir d žymi atstumą tarp mūsų dviejų pavarų velenų.

Dabar, kai turime visus reikalingus kintamuosius, galime pradėti konvertuoti savo atvirkštinės-kinematikos lygtis į kodą. Nors šios kodo eilutės gali atrodyti daug sudėtingesnės nei kai kurios ankstesnės mūsų kodo eilutės, jei peržvelgiate jas žingsnis po žingsnio, pamatysite, kad imame kiekvieną mūsų sukurtą veiksmą ir paverčiame jį kodu, kurį supranta mūsų arduino.

Kai turėsime visų savo kintamųjų reikšmes ir baigėme pagrindinius skaičiavimus, viskas, kas liko, yra sujungti mūsų povandeninio kintamuosius į galutinio kampo kintamuosius, kurie bus išsiųsti mūsų pavaros. Kaip galite prisiminti iš ankstesnių skaičiavimų, kai kurie mūsų galutinio kampo skaičiavimai priklauso nuo mūsų vertės x kintamasis. Laimei, visi sąlyginių teiginių rinkiniai yra viskas, ko mums reikia, kad įsitikintume, jog mūsų kampai yra teisingai apskaičiuojami, nesvarbu x ir y Pasirinktos vertės.