This sample code is for OTTO DIY Robot using Maker Pi RP2040.

OttoMakerPiRP2040.ino

/* Otto DIY Robot
  by Suad Anwar

  This sample code is for Otto DIY Robot using Maker Pi RP2040 board.

  https://my.cytron.io/p-maker-pi-rp2040-simplifying-robotics-with-raspberry-pi-rp2040
*/

#include <Servo.h>
#define PIN_YL 12 //servo[0]  left leg
#define PIN_YR 13 //servo[1]  right leg
#define PIN_RL 14 //servo[2]  left foot
#define PIN_RR 15 //servo[3]  right foot
#define PIN_Trigger  8  //TRIGGER pin (8)
#define PIN_Echo     9  //ECHO pin (9)
#define PIN_Buzzer  22 //BUZZER pin (22)

int pos = 90;
int pos2 = 90;
int pos4 = 90;

Servo myservoYL;  // create servo object to control a servo
Servo myservoYR;
Servo myservoRL;
Servo myservoRR;


void setup() {
  myservoYL.attach(PIN_YL);  // attaches the servo on GIO2 to the servo object
  myservoYR.attach(PIN_YR);
  myservoRL.attach(PIN_RL);
  myservoRR.attach(PIN_RR);

  myservoYL.write(90);
  myservoYR.write(90);
  myservoRL.write(90);
  myservoRR.write(90);

  Serial.begin(9600);
}

void loop() {

  stopdance();
  delay(5000);
  dance4();
  dance4();
  dance4();
  dance4();
  stopdance();
  dance2();
  dance2();
  dance2();
  dance2();
  stopdance();
  dance3();
  dance3();
  dance3();
  dance3();
  stopdance();
  dance1();
  dance1();
  dance1();
  dance1();
  stopdance();
  dance3();
  dance3();
  dance3();
  dance3();
  stopdance();
  dance5();
  dance5();
  dance5();
  dance5();
  stopdance();


}

void dance1() { // angkat tumit kiri
  for (pos = 90; pos <= 180; pos += 10) {
    myservoRL.write(pos);
    delay(20);
  } delay(200);
  for (pos2 = 0; pos2 <= 90; pos2 += 10) {
    // in steps of 1 degree
    myservoRR.write(pos2);
    delay(20);
  }
  for (pos = 180; pos >= 90; pos -= 10) {
    myservoRL.write(pos);
    delay(20);
  }
  for (pos2 = 90; pos2 >= 0; pos2 -= 10) {
    myservoRR.write(pos2);
    delay(20);
  }
}

void dance2() { //angkat tumit kanan

  for (pos2 = 0; pos2 <= 90; pos2 += 10) {
    myservoRR.write(pos2);
    delay(20);
  }
  for (pos = 90; pos <= 180; pos += 10) {
    myservoRL.write(pos);
    delay(20);
  }
  for (pos2 = 90; pos2 >= 0; pos2 -= 10) {
    myservoRR.write(pos2);
    delay(20);
  } delay(200);
  for (pos = 180; pos >= 90; pos -= 10) {
    myservoRL.write(pos);
    delay(20);
  }
}
void dance3() { //dua kaki pusing
  for (pos = 30; pos <= 140; pos += 10) {
    myservoYL.write(pos);
    myservoYR.write(pos);
    delay(20);
  } delay(200);
  for (pos = 140; pos >= 30; pos -= 10) {
    myservoYL.write(pos);
    myservoYR.write(pos);
    delay(20);
  } delay(200);
}
void dance4() { //hentak kaki
  for (pos2 = 50; pos2 <= 100; pos2 += 10) {
    myservoRL.write(pos2);
    myservoRR.write(70);
    delay(20);
  } delay(300);
  for (pos2 = 100; pos2 >= 50; pos2 -= 10) {
    myservoRL.write(pos2);
    myservoRR.write(70);
    delay(20);
  }
}
void dance5() {
  for (pos = 120; pos <= 180; pos += 5) {
    myservoRL.write(pos);
    myservoRR.write(180 - pos);
    delay(50);
  } delay(500);
  for (pos = 180; pos >= 120; pos -= 5) {
    myservoRL.write(pos);
    myservoRR.write(180 - pos);
    delay(50);
  } delay(200);
}
void stopdance() {


  int posRL = myservoRL.read();
  Serial.print("posRL: "); Serial.println(posRL);
  if (posRL > 90) {
    for (pos = posRL; pos >= 90; pos -= 5) {
      myservoRL.write(pos); delay(20);
    }
  } else {
    for (pos = posRL; pos <= 90; pos += 5) {
      myservoRL.write(pos); delay(20);
    }
  }

  int posRR = myservoRR.read();
  Serial.print("posRR: "); Serial.println(posRR);
  if (posRR > 90) {
    for (pos = posRR; pos >= 90; pos -= 5) {
      myservoRR.write(pos); delay(20);
      Serial.print("Looping posRR: "); Serial.println(posRR);
    }
  } else {
    for (pos = posRR; pos <= 90; pos += 5) {
      myservoRR.write(pos); delay(20);
      Serial.print("Looping posRR: "); Serial.println(posRR);
    }
  }

  int posYL = myservoYL.read();
  Serial.print("posYL: "); Serial.println(posYL);
  if (posYL > 90) {
    for (pos = posYL; pos >= 90; pos -= 5) {
      myservoYL.write(pos); delay(20);
    }
  } else {
    for (pos = posYL; pos <= 90; pos += 5) {
      myservoYL.write(pos); delay(20);
    }
  }

  int posYR = myservoYR.read();
  Serial.print("posYR: "); Serial.println(posYR);
  if (posYR > 90) {
    for (pos = posYR; pos >= 90; pos -= 5) {
      myservoYR.write(pos); delay(20);
    }
  } else {
    for (pos = posYR; pos <= 90; pos += 5) {
      myservoYR.write(pos); delay(20);
    }
  }

  //  Serial.print(myservoYL.read());
  //  Serial.print(myservoYR.read());
  //  Serial.print(myservoRL.read());
  //  Serial.println(myservoRR.read());

  myservoYL.write(90);
  myservoYR.write(90);
  myservoRL.write(90);
  myservoRR.write(90);
  delay(500);
}