Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
This sample code is for OTTO DIY Robot using Maker Pi RP2040.
/* 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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment