Robotukas valdomas 4 servo motorais – kodas

Elektronikos komponentai: Arduino uno bei Adafruit PWM servo driver

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

uint8_t movement[32][4] = {
  {150, 20, 150, 20},
  {90, 90, 90, 90},
  {20, 150, 20, 150},
  {90, 90, 90, 90}
};

uint8_t total = sizeof(movement) / sizeof(movement[0]);


Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);


#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 500 // Analog servos run at ~50 Hz updates

uint8_t split = 10;
uint8_t cs = 0;

void setup() {
  Serial.begin(115200);
  Serial.println(total);

  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

}

void loop() {
for (int i = 0; i < total ; i++) {
    uint8_t *c = movement[i];
    uint8_t *n = movement[(i + 1) % total];
    Serial.print("Next ");
    for (int k = 0; k < 4; k++) {
      Serial.print(n[k]);
      Serial.print(" ");
    }
    Serial.println();
    int dir[4] = {1, 1, 1, 1};
    for (int a = 0; a < 4; a++) {
      if (n[a] > c[a]) dir[a] = (n[a] - c[a]) / split; else dir[a] = -(c[a] - n[a]) / split;
    }
    int tmp[4];
    for (int k = 0; k < 4; k++) {
      tmp[k] = c[k];
    }
    for (int a = 0; a < split; a++) {
      for (int k = 0; k < 4; k++) {
        
        tmp[k] += dir[k];
        Serial.print(tmp[k]);
        Serial.print(" ");
        setServoPos(k, tmp[k]);
      }
      Serial.println();
      delay(10);
    }
  }
}

void setServoPos(uint8_t n, double pulse) {
   pwm.setPWM(n, 0, map(pulse, 0, 180, 0, 4096));
}