Get best of the week

Kami menghidupkan kembali hexapod. Bagian kedua

Video hexapod yang bergerak


Dibandingkan dengan publikasi sebelumnya, dia pendahulunya ternyata lebih spektakuler, berkat sejumlah besar foto. Saya ingin mengisi kekosongan dalam masalah ini dan menyajikan kepada Anda beberapa video yang menangkap test drive kecil robot di sekitar apartemen.


Robot dikendalikan oleh operator dari telepon melalui Wi-Fi. Tetapi ketika menonton dua video pertama, Anda mungkin mendapatkan kesan yang salah bahwa hexapod memiliki dasar kecerdasan. Sayangnya, ini tidak begitu ...


Selanjutnya, robot dikeluarkan dari karpet lembut ke linoleum yang agak licin, yang menunjukkan hasil yang baik.


Implementasi gerakan hexapod


Seperti yang disebutkan dalam artikel sebelumnya , kelas MotionJob dan kelas turunan yang diwarisi dari kelas Motion bertanggung jawab atas implementasi gerakan robot .

Kelas MotionJob hanya memiliki satu metode eksekusi publik , yang meluncurkan gerakan yang diberikan untuk eksekusi.

int MotionJob :: execute (Motion * pMotion)
int MotionJob::execute(Motion* pMotion) {
  if(m_Status == NONE && pMotion != NULL) {
     
     m_pMotion = pMotion;
     m_MotionTime = 0;
     m_TotalTime = m_pMotion->totalTime();
     m_Status = RUNING;
  } else if(pMotion != NULL){
    
    if(m_pMotion->m_pNext != NULL) 
      delete m_pMotion->m_pNext;
      
    m_pMotion->m_pNext = pMotion;
     m_Status = STOPING;
  }
  return S_OK;
}


Logikanya cukup sederhana. Sebagai parameter, dibutuhkan pointer ke objek yang menggambarkan proses pergerakan. Jika pada saat ini tidak ada gerakan lain dalam eksekusi, maka gerakan yang ditunjukkan menjadi aktif, dan memasuki kondisi eksekusi ( RUNING ). Dalam kasus ketika sudah ada beberapa gerakan pada eksekusi, kami menerjemahkannya ke dalam stop state ( STOPING ), dan gerakan baru dicatat sebagai yang berikutnya di bidang m_pNext .

PENTING: Disetujui bahwa semua objek yang diwarisi dari Motion menyiratkan pembuatan di area memori dinamis (via baru ) dan dibebaskan oleh kelas MotionJob setelah gerakan selesai atau dibatalkan.

Kelas MotionJob , yang diwarisi dari AJob , mengimplementasikan semua keajaiban gerakan dalam metode OnRun yang tumpang tindih , yang disebut dengan frekuensi yang diberikan selama pengoperasian robot.

void MotionJob :: onRun ()
void MotionJob::onRun() {

  Vector3D points[FOOTS_COUNT];
  int pose[FOOTS_COUNT][3];
  //    ,  
  if(m_pMotion == NULL)
    return;
  //           ,
  //            .
  //      .
  int res;
  if(m_pMotion->getPose(min(m_MotionTime, m_pMotion->maxTime()), pose) == S_OK) {
     res = m_pGeksapod->_setPose(pose);
  } else if(m_pMotion->getPose(min(m_MotionTime, m_pMotion->maxTime()), points) == S_OK) {
     res = m_pGeksapod->_setPose(points);
  }
  //     
  m_MotionTime += MOTION_JOB_PERIOD;
  //     ,    ,
  //      :
  //   m_TotalTime -     
  if(m_TotalTime > 0) { 
    m_TotalTime -= MOTION_JOB_PERIOD;
    m_TotalTime = max(0,m_TotalTime); 
  }
  //      , 
  //      
  if(m_TotalTime == 0 && m_pMotion->isLooped()) {
     m_Status = STOPING;
  }
  //         
  //          .
  if( (m_MotionTime - m_pMotion->maxTime() >= MOTION_JOB_PERIOD && (!m_pMotion->isLooped() || m_Status == STOPING)) ) { //  
    //      
    Motion* pNextMotion = m_pMotion->m_pNext;
    m_pMotion->m_pNext = NULL;
    delete m_pMotion;
    m_pMotion = NULL;
    //   
    m_Status = NONE;
    execute(pNextMotion);
  } else if(m_MotionTime - m_pMotion->maxTime() >= MOTION_JOB_PERIOD) {
    //      
    //         
    m_MotionTime = 0; 
  }
}


Kode di atas mengandung cukup komentar tentang kerjanya, oleh karena itu, sebagai tambahan, saya perhatikan hanya beberapa poin kunci:
  • Motion (Vector3D points[FOOTS_COUNT]), (int pose[FOOTS_COUNT][3]). , . getPose , getPose. _setPose, .
  • Gerakan bisa teratur atau siklus. Gerakan normal setelah mencapai akhir periode waktu yang diberikan untuk pelaksanaan satu siklus gerakan (dikembalikan oleh metode Motion :: maxTime () ) akan segera diselesaikan.
    Gerakan siklik akan diselesaikan hanya setelah penghentian paksa yang terjadi ketika metode eksekusi dipanggil (dapat dipanggil dengan penunjuk nol), atau ketika titik positif dalam waktu diberikan untuk pelaksanaan gerakan (dikembalikan oleh metode Motion :: totalTime () ).

Kelas-kelas berikut didefinisikan untuk menggambarkan berbagai opsi gerak:

kelas TransPoseMotion
class TransPoseMotion: public Motion {
  Vector3D m_Pose0[FOOTS_COUNT];  //  
  Vector3D m_Offset[FOOTS_COUNT];  //  
public:  
  TransPoseMotion(Vector3D begPose[FOOTS_COUNT], Vector3D endPose[FOOTS_COUNT],
        long maxTime, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

kelas TransAnglesMotion
class TransAnglesMotion: public Motion {
  char m_Pose0[FOOTS_COUNT][3];
  char m_Offset[FOOTS_COUNT][3];
public:  
  TransAnglesMotion(int begPose[FOOTS_COUNT][3], int endPose[FOOTS_COUNT][3],
        long maxTime, Motion* pNext = NULL);
  int getPose(long actionTime, int pose[FOOTS_COUNT][3]);
};

kelas LinearMotion
class LinearMotion: public Motion {
  //     ( ) 
  Vector3D m_Pose0[FOOTS_COUNT];
  Vector3D m_Offset; //     
  int m_Heigth;          //   
public:  
  LinearMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed, 
        long maxTime, long totalTime = -1, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

RotateMotion kelas
class RotateMotion: public Motion {
  //     ( ) 
  Vector3D m_Pose0[FOOTS_COUNT]; 
  Vector3D m_Angles; //     
  int m_Heigth;           //   
public:  
  RotateMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D rotor, 
        long maxTime, long totalTime = -1, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

Kelas TransPoseMotion


Kelas TransPoseMotion memungkinkan Anda untuk memindahkan anggota badan dari satu posisi ke posisi lain. Saat membuat objek, koordinat titik dan periode waktu di mana gerakan harus dilakukan dipindahkan ke konstruktor kelas.

TransPoseMotion :: TransPoseMotion (...)
TransPoseMotion::TransPoseMotion(Vector3D begPose[FOOTS_COUNT],
  Vector3D endPose[FOOTS_COUNT], long maxTime, Motion* pNext) : Motion(maxTime, -1, pNext) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    m_Pose0[i] = begPose[i];
    m_Offset[i] = endPose[i] - begPose[i];
  }
}


Mengetahui posisi awal dan vektor perpindahan untuk setiap ekstremitas, kita dapat dengan mudah mendapatkan posisi baru cakram dalam metode getPose dalam interval waktu dari 0 hingga maxTime dengan melakukan perhitungan linear sederhana.

int TransPoseMotion :: getPose (...)
int TransPoseMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  for(int i = 0; i < FOOTS_COUNT; i++)
    linearFunc(1.0, m_Offset[i], m_Pose0[i], 1.0*time/m_MaxTime, points[i]);
}


Komputasi linear ditugaskan ke fungsi bantu linearFunc, yang merupakan persamaan linear biasa dengan parameter yang ditentukan melalui argumen fungsi.

membatalkan linearFunc (...)
void linearFunc(float a, Vector3D offset, Vector3D p0, float k,  Vector3D& res) {
  res = a*offset*k + p0;
}


Kelas TransAnglesMotion


Logika kelas ini mirip dengan yang sebelumnya. Satu-satunya perbedaan adalah bahwa dalam kelas TransAnglesMotion, posisi anggota hexapod diatur oleh himpunan sudut di mana sambungannya harus ditekuk, dan bukan oleh koordinat spasial dari ujung cakar robot.

TransAnglesMotion :: TransAnglesMotion (...)
TransAnglesMotion::TransAnglesMotion(int begPose[FOOTS_COUNT][3],
  int endPose[FOOTS_COUNT][3], long maxTime, Motion* pNext) : Motion(maxTime, -1, pNext) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    for(int j = 0; j < 3; j++) {
      m_Pose0[i][j] = begPose[i][j];
      m_Offset[i][j] = endPose[i][j] - begPose[i][j];
    }
  }
}



int TransAnglesMotion :: getPose (...)
int TransAnglesMotion::getPose(long time, int pose[FOOTS_COUNT][3]) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    for(int j = 0; j < 3; j++) 
      pose[i][j] = m_Pose0[i][j] + (int)(1.0*m_Offset[i][j]*time/m_MaxTime);
  }
  return S_OK;
}


Kelas LinearMotion


Menerapkan gerakan langsung membutuhkan sedikit lebih banyak kode dan perhatian Anda. Desain hexapod memiliki jumlah anggota tubuh yang optimal, memungkinkan Anda untuk selalu memiliki tiga titik dukungan saat bergerak, sehingga proyeksi pusat massa robot selalu berada di dalam segitiga yang dibentuk oleh mereka. Ini memastikan posisi robot yang stabil saat bergerak. Oleh karena itu, gerakan bujursangkar robot dapat dianggap sebagai satu set langkah yang identik. Dalam setiap langkah tersebut, dua fase dapat dibedakan. Pada fase pertama, satu serangkai kaki menyentuh permukaan dan ujungnya bergerak sejajar dengan permukaan dalam arah yang berlawanan dengan arah gerakan. Dalam hal ini, triad kedua cakar, tanpa menyentuh permukaan, bergerak di sepanjang jalur melengkung ke arah pergerakan robot.Pada titik waktu tertentu, mereka mencapai posisi ekstrem mereka, dan triad kedua mulai bersentuhan dengan permukaan. Pada fase kedua, proses gerak diulang, hanya kali ini triad kedua menggerakkan robot, dan yang pertama kembali ke titik awal.



Animasi ini ditemukan ketika sedang mempersiapkan sebuah artikel di Internet di halaman ini , tetapi setelah memeriksanya, saya yakin bahwa sumber aslinya adalah habr , dan penulis publikasi itu adalah kata kunci yang disegani .

Ketika menyadari gerakan dalam garis lurus, kita perlu mengatur ketinggian kaki dari permukaan ( heigth ), posisi anggota badan robot di awal (dan di akhir) dari gerakan ( pose0 ), vektor kecepatan ( speed ), waktu yang dibutuhkan untuk menyelesaikan satu langkah ( maxTime ), dan berapa lama robot harus melakukan gerakan ( totalTime ). Jika Anda menentukan nilai negatif untuk totalTime, gerakan akan berlanjut tanpa batas waktu sampai muatan robot habis atau gerakan selesai secara paksa.

LinearMotion :: LinearMotion (...)
LinearMotion::LinearMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed,
  long maxTime, long totalTime, Motion* pNext = NULL) 
    : Motion(maxTime, true, totalTime, pNext) {
  copyPose(m_Pose0, pose0);
  m_Offset = 1.0*speed*maxTime/1000;
  m_Heigth = heigth;
}

Terlepas dari kenyataan bahwa metode getPose berisi jumlah kode yang mengesankan, itu cukup sederhana.

int LinearMotion :: getPose (...)
int LinearMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  double k = 1.0*time/m_MaxTime;

  if(k < 0.25) {  //   
    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*cos(M_PI*k*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;

  } else if(k < 0.5) { //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*sin(M_PI*(k - 0.25)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;

  } else if(k < 0.75) { //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
    
    int h = (int)m_Heigth*cos(M_PI*(k - 0.5)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;
    
  } else {  //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], 1 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], 1 - k,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 1 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], 1 - k,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], 1 - k,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], 1 - k,
      points[RIGTH_BACK_FOOT_IDX]);
    
    int h = (int)m_Heigth*sin(M_PI*(k - 0.75)*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;
  }

  return S_OK;
}


Kami membagi seluruh langkah menjadi empat bagian:

  1. Pada kuartal pertama, triad anggota badan pertama (depan kiri, kiri belakang dan tengah kanan) melakukan gerakan dorong, yaitu, mereka bergerak di sepanjang permukaan dengan arah yang berlawanan dengan gerakan. Triad kedua cakar (depan kanan, kanan belakang dan tengah kiri) bergerak ke arah gerakan, sedangkan ketinggian kenaikannya bervariasi dari nilai maksimum tinggi kaki yang ditentukan (pada awal kuartal) hingga nol (pada akhir kuartal) sebagai fungsi sinus.
  2. Di kuartal kedua, arah triad berubah. Sekarang triad kedua cakar menggerakkan robot, dan yang pertama bergerak ke arah yang sama dengan robot. Dalam hal ini, ketinggian cakar triad pertama berubah dalam sinusoid dari nol ke nilai maksimum.
  3. Pada kuartal ketiga, arah pergerakan kaki dipertahankan, dan ujung-ujung triad pertama turun, mencapai permukaan pada akhir kuartal ketiga.
  4. Pada kuartal terakhir, triad pertama menjadi penopang, dan yang kedua mengubah arah dan bergerak ke atas di sepanjang sinusoid, mencapai ketinggian maksimum pada akhir kuartal.

RotateMotion Class


Setelah memahami logika kelas LinearMotion , kita dapat dengan mudah mengetahui implementasi kelas RotateMotion .

RotateMotion :: RotateMotion (...)
RotateMotion::RotateMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed,
  long maxTime, long totalTime = -1, Motion* pNext = NULL) 
    : Motion(maxTime, true, totalTime, pNext) {
  copyPose(m_Pose0, pose0);
  m_Angles = 0.001*maxTime*speed*M_PI/180;
  m_Heigth = heigth;
}


Konstruktor kedua kelas hampir identik dalam komposisi argumen yang disahkan. Hanya dalam kasus pembalikan kecepatan adalah vektor kecepatan sudut menentukan sumbu dan kecepatan rotasi robot.

Dalam metode getPose dari kelas RotateMotion , logika kerja juga dibagi menjadi empat tahap. Namun, untuk menghitung posisi cakar, bukan fungsi linear linearFunc , fungsi radial rotFunc yang digunakan .

int RotateMotion :: getPose (...)
int RotateMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  double k = 1.0*time/m_MaxTime;
  if(k < 0.25) {  //   

    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], -k,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], -k,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], -k,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*cos(M_PI*k*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;

  } else if(k < 0.5) {  //   
    
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], 0.5 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], 0.5 - k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 0.5 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*sin(M_PI*(k - 0.25)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;

  } else if(k < 0.75) {  //   
  
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], 0.5 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], 0.5 - k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 0.5 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*cos(M_PI*(k - 0.5)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;
  } else { //   
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 1,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], k - 1,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 1,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], 1 - k,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], 1 - k,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], 1 - k,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*sin(M_PI*(k - 0.75)*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;
  }
  return S_OK;
}


Fungsi radial memungkinkan Anda menghitung posisi suatu titik di ruang angkasa setelah rotasi di sekitar pusat robot, yang bertepatan dengan titik asal.

membatalkan rotFunc (...)
void rotFunc(float a, Vector3D angles, Vector3D p0, float k, Vector3D& res) {
  Matrix3D m = rotMatrix2(a*angles*k);
  res = m*p0;
}


Di dalam fungsinya, sebuah alat matematika terlibat, yang memungkinkan kita untuk bekerja dengan besaran vektor dan matriks.

Itu saja untuk sekarang. Kami telah menganalisis bagian penting dari kode program yang bertanggung jawab untuk mengimplementasikan pergerakan robot. Jika Anda memiliki pertanyaan atau komentar, saya akan dengan senang hati menjawabnya di komentar. Saya akan mencoba mencurahkan artikel berikut untuk sisa kode, kode sumber yang akan tersedia untuk diunduh.
Terimakasih atas perhatiannya!

Bersambung…

More articles:


All Articles