Skocz do zawartości

Problem z odwrotną kinematyką w hexapodzie


Worldi

Pomocna odpowiedź

// ============= LIBRARIES =============
// Libraries for communication and servo control
// The following libraries are included for I2C communication and servo control
#include <Wire.h>                     // I2C communication library
#include <Adafruit_PWMServoDriver.h>  // PCA9685 control library
#include <math.h>                     // Math library for mathematical operations

// ============= SERVO CONSTANTS =============
// Constants used to set servo angles
// The following constants define the minimum and maximum pulse lengths for the servos
#define SERVOMIN 150             // Minimum pulse length (out of 4096 possible)
#define SERVOMAX 600             // Maximum pulse length (out of 4096 possible)
#define RAD_TO_DEG (180 / M_PI)  // Radian to degree conversion factor
#define SERVO_ZERO 90            // Servo zero position (middle of the range)

// ============= ROBOT LEG NUMBERS =============
// Definitions of robot leg numbers
// The following constants define the numbers for each robot leg
#define prawa_przednia 0  // Right front leg number
#define prawa_tylna 1     // Right rear leg number
#define lewa_tylna 2      // Left rear leg number
#define lewa_przednia 3   // Left front leg number

// ============= CONTROLLER INITIALIZATION =============
// Initialization of the servo control object
// The following line initializes the servo control object
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// ============= ROBOT DIMENSIONS =============
// Definitions of robot leg segment lengths
// The following constants define the lengths of the robot leg segments
const float coxaLength = 30.6;    // Length of the first segment (hip)
const float femurLength = 57.0;   // Length of the second segment (thigh)
const float tibiaLength = 103.5;  // Length of the third segment (shank)
const float xOffset = 57.0;
const float yOffset = -103.5;

// ============= PIN CONFIGURATION =============
// Array storing pin numbers for each robot leg
// The following array stores the pin numbers for each robot leg
int legs[4][3] = {
  { 0, 1, 2 },   // Pins for the right front leg
  { 3, 4, 5 },   // Pins for the right rear leg
  { 6, 7, 8 },   // Pins for the left rear leg
  { 9, 10, 11 }  // Pins for the left front leg
};

// ============= INVERSE KINEMATICS =============
// Function to calculate servo angles based on X, Y, Z position
// The following function calculates the servo angles based on the X, Y, Z position
void calculateIK(double x, double y, double z, float &theta1, float &theta2, float &theta3) {
  // Calculate the length of the leg
  float legLength = sqrt((x * x) + (z * z));
  // Calculate the length of the hip-femur segment
  float HF = sqrt(((legLength - coxaLength) * (legLength - coxaLength)) + (y * y));
  // Calculate the angle of the hip-femur segment
  float K1 = atan2((legLength - coxaLength), y) * RAD_TO_DEG;
  // Calculate the angle of the femur-tibia segment
  float K2 = acos(((tibiaLength * tibiaLength) - (femurLength * femurLength) - (HF * HF)) / (-2 * femurLength * HF)) * RAD_TO_DEG;
  // Calculate the angle of the tibia segment
  float M1 = acos(((HF * HF) - (tibiaLength * tibiaLength) - (femurLength * femurLength)) / (-2 * femurLength * tibiaLength)) * RAD_TO_DEG;  // Angle for the shank segment

  // Calculate the servo angles
  theta2 = 90 - (K1 + K2);  // Angle calculation for the thigh
  theta3 = 90 - M1;         // Angle calculation for the shank
  theta1 = atan2(z, x) * RAD_TO_DEG;

  // Adjust the servo angles
  theta1 += 90;
  theta2 += 90;
  theta3 += 90;
}

// ============= SERVO CONTROL =============
// Function to set servo angle
// The following function sets the servo angle
void setServoAngle(uint8_t num, float angle) {
  // Constrain the angle to the valid range
  angle = constrain(angle, 0, 180);
  // Map the angle to the pulse length
  uint16_t pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX);  // Angle mapping to pulse length
  // Set the pulse on the appropriate pin
  pwm.setPWM(num, 0, pulse);                                // Setting the pulse on the appropriate pin
}

// Function to set robot leg position
// The following function sets the robot leg position
void setLegPosition(int leg, float x, float y, float z) {
  // Calculate the servo angles
  float theta1, theta2, theta3;                  // Angles for the leg segments
  calculateIK(x, y, z, theta1, theta2, theta3);  // Angle calculation
  // Set the servo angles
  setServoAngle(legs[leg][0], theta1);           // Setting the angle for the hip segment
  delay(200);                                    // Added delay between movements of individual servos
  setServoAngle(legs[leg][1], theta2);           // Setting the angle for the thigh segment
  delay(200);
  setServoAngle(legs[leg][2], theta3);  // Setting the angle for the shank segment
  delay(200);
}

// ============= INITIALIZATION =============
// Function to initialize robot settings
// The following function initializes the robot settings
void setup() {
  // Start serial communication
  Serial.begin(9600);  // Starting serial communication
  // Initialize the PWM controller
  pwm.begin();         // PWM controller initialization
  // Set the PWM frequency
  pwm.setPWMFreq(50);  // Setting the PWM frequency

  // Initial position - slower initialization
  for (int i = 0; i < 12; i++) {
    // Set the servos to the zero position
    setServoAngle(i, SERVO_ZERO);  // Setting the servos to the zero position
    // Add a delay during initialization
    delay(300);                    // Increased delay during initialization
  }
}

// ============= MAIN LOOP =============
void loop() {
  // Set the leg position
  setLegPosition(lewa_przednia, 0 + (xOffset), 0 + (yOffset), 0);
}

Wydaje mi się, że obliczenia są dobre, ale noga hexapoda (póki co nie ma 6 nóg, ale będzie mieć) nie jest w stanie zrobić ruchu wzdłuż jednej współrzędnej, a czasem robi nieoczekiwane losowe ruchy. Mam zmieniony ukł. współrzędnych:  x - podłóżna, y - pionowa, z- poprzeczna.

image.thumb.png.a43d18a140a395d7dc14eb455d3cee6d.png

Edytowano przez Worldi
  • Lubię! 1
Link do komentarza
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ę »
×
×
  • 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.