Project 6: Dance
Circuit Design
Based on the robot walks circuit, we are able to use the same electrical components and wiring method. Just change the code to make the frog robot dance.
Wiring
Stack the Nano ch340 onto the Nano shield;
Connect 4 servo motors to digital pin5,4,3,2 separately.
Code
#include <Servo.h>
#include <Oscillator.h>
#include <EEPROM.h>
#define N_SERVOS 4
//-- First step: Configure the pins where the servos are attached
/*
---------------
| O O |
|---------------|
YR 3==> | | <== YL 2
---------------
|| ||
|| ||
RR 5==> ----- ------ <== RL 4
|----- ------|
*/
#define EEPROM_TRIM false
//adjust the initial angles of servos
#define TRIM_RR (-5) //right02
#define TRIM_RL 5 //left02
#define TRIM_YR (-5) //right01
#define TRIM_YL 0 //left01
//pins of the servo
#define PIN_RR 5
#define PIN_RL 4
#define PIN_YR 3
#define PIN_YL 2
#define INTERVALTIME 10.0
Oscillator servo[N_SERVOS];
#include "SR04.h"
#define TRIG_PIN 6 //sending pins of the ultrasonic sensor
#define ECHO_PIN 7 //receiving pins of the ultrasonic sensor
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long a;
int i = 0;
int val = 0;
//each function declaration
void goingUp(int tempo);
void drunk (int tempo);
void noGravity(int tempo);
void kickLeft(int tempo);
void kickRight(int tempo);
void run(int steps, int T=500);
void walk(int steps, int T=1000);
void backyard(int steps, int T=3000);
void backyardSlow(int steps, int T=5000);
void turnLeft(int steps, int T=3000);
void turnRight(int steps, int T=3000);
void moonWalkLeft(int steps, int T=1000);
void moonWalkRight(int steps, int T=1000);
void crusaito(int steps, int T=1000);
void swing(int steps, int T=1000);
void upDown(int steps, int T=1000);
void flapping(int steps, int T=1000);
int t=495;
double pause=0;
//stop
void Stop()
{
for(int i=0;i<4;i++) servo[i].SetPosition(90);
}
//go back
void backyard(int steps, int T)
{
int A[4]= {15, 15, 30, 30};
int O[4] = {0, 0, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
//go forward
void walk(int steps, int T)
{
int A[4]= {15, 15, 30, 30};
int O[4] = {0, 0, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
void oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
for (int i=0; i<4; i++) {
servo[i].SetO(O[i]);
servo[i].SetA(A[i]);
servo[i].SetT(T);
servo[i].SetPh(phase_diff[i]);
}
double ref=millis();
for (double x=ref; x<T+ref; x=millis()){
for (int i=0; i<4; i++){
servo[i].refresh();
}
}
}
unsigned long final_time;
unsigned long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS];
int oldPosition[]={90,90,90,90};
void moveNServos(int time, int newPosition[]){
for(int i=0;i<N_SERVOS;i++) increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);
final_time = millis() + time;
iteration = 1;
while(millis() < final_time){ //Javi del futuro cambia esto
interval_time = millis()+INTERVALTIME;
oneTime=0;
while(millis()<interval_time){
if(oneTime<1){
for(int i=0;i<N_SERVOS;i++){
servo[i].SetPosition(oldPosition[i] + (iteration * increment[i]));
}
iteration++;
oneTime++;
}
}
}
for(int i=0;i<N_SERVOS;i++){
oldPosition[i] = newPosition[i];
}
}
//slide
void crusaito(int steps, int T){
int A[4]= {25, 25, 30, 30};
int O[4] = {- 15, 15, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
//wave
void drunk (int tempo){
pause=millis();
int move1[] = {60,70,90,90};
int move2[] = {110,120,90,90};
int move3[] = {60,70,90,90};
int move4[] = {110,120,90,90};
moveNServos(tempo*0.235,move1);
moveNServos(tempo*0.235,move2);
moveNServos(tempo*0.235,move3);
moveNServos(tempo*0.235,move4);
while(millis()<(pause+tempo));
}
//swing
void flapping(int steps, int T){
int A[4]= {25, 25, 0, 0};
int O[4] = {-15, 15, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
//tiptoe
void goingUp(int tempo){
pause=millis();
for(int i=0;i<4;i++) servo[i].SetPosition(90);
delay(tempo);
servo[0].SetPosition(80);
servo[1].SetPosition(100);
delay(tempo);
servo[0].SetPosition(70);
servo[1].SetPosition(110);
delay(tempo);
servo[0].SetPosition(60);
servo[1].SetPosition(120);
delay(tempo);
servo[0].SetPosition(50);
servo[1].SetPosition(130);
delay(tempo);
while(millis()<pause+8*t);
}
//jump
void jump()
{
int move5[4] = {70,110,80,100};
int move6[4] = {70,110,100,80};
int move7[4] = {90,90,80,100};
int move8[4] = {90,90,100,80};
pause=millis();
moveNServos(t*0.15,move5);
moveNServos(t*0.15,move6);
moveNServos(t*0.15,move7);
moveNServos(t*0.15,move8);
while(millis()<(pause+t));
}
//kick left
void kickLeft(int tempo){
for(int i=0;i<4;i++) servo[i].SetPosition(90);
delay(tempo);
servo[0].SetPosition(50); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo);
servo[0].SetPosition(80); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(30); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(80); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(30); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(80); //pie derecho
servo[1].SetPosition(70); //pie izquiero
delay(tempo);
}
//kick right
void kickRight(int tempo){
for(int i=0;i<4;i++) servo[i].SetPosition(90);
delay(tempo);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(130); //pie izquiero
delay(tempo);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(100); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(150); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(80); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(150); //pie izquiero
delay(tempo/4);
servo[0].SetPosition(110); //pie derecho
servo[1].SetPosition(100); //pie izquiero
delay(tempo);
}
//moon left
void moonLEFT(int steps, int T)
{
int A[4]= {25, 25, 0, 0};
int O[4] = {-15, 15, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
//moon right
void moonRight(int steps, int T)
{
int A[4]= {25, 25, 0, 0};
int O[4] = {-15, 15, 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
//weightlessness
void noGravity(int tempo){
int move1[4] = {120,140,90,90};
int move2[4] = {140,140,90,90};
int move3[4] = {120,140,90,90};
int move4[4] = {90,90,90,90};
for(int i=0;i<4;i++) servo[i].SetPosition(90);
for(int i=0;i<N_SERVOS;i++) oldPosition[i]=90;
moveNServos(tempo*2,move1);
moveNServos(tempo*2,move2);
delay(tempo*2);
moveNServos(tempo*2,move3);
moveNServos(tempo*2,move4);
}
//friction
void segunda1()
{
int move1[4] = {90,90,80,100};
int move2[4] = {90,90,100,80};
int move3[4] = {90,90,80,100};
int move4[4] = {90,90,100,80};
pause=millis();
moveNServos(t*0.15,move1);
moveNServos(t*0.15,move2);
moveNServos(t*0.15,move3);
moveNServos(t*0.15,move4);
while(millis()<(pause+t));
}
//swim
void swing(int steps, int T){
int A[4]= {15, 15, 8, 8};
int O[4] = {-A[0], A[1], 0, 0};
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(-90)};
for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}
void setup(){
Serial.begin(9600);
servo[0].attach(PIN_RR);
servo[1].attach(PIN_RL);
servo[2].attach(PIN_YR);
servo[3].attach(PIN_YL);
int trim;
if(EEPROM_TRIM){
for(int x=0;x<4;x++){
trim=EEPROM.read(x);
if(trim>128)trim=trim-256;
Serial.print("TRIM ");
Serial.print(x);
Serial.print(" en ");
Serial.println(trim);
servo[x].SetTrim(trim);
}
}
else{
servo[0].SetTrim(TRIM_RR);
servo[1].SetTrim(TRIM_RL);
servo[2].SetTrim(TRIM_YR);
servo[3].SetTrim(TRIM_YL);
}
for(int i=0;i<4;i++) servo[i].SetPosition(90);
}
void loop(){
goingUp(2*t); //tiptoe in 2*t
for (int i = 0; i <= 2; i = i + (1)) { //set the variable i to save times of rubbing, from 0 to 2, step is 1
segunda1(); // robots rub on the floor
}
for (int i = 0; i <= 2; i = i + (1)) { //set the variable i to save times of jumping, step is 1
jump(); // robots jump
}
walk(3,2*t); //go forward for 3 steps in 2*t
backyard(3,2*t); //go forward for 3 steps in 2*t
crusaito(1,5*t); //slide for 1 step in 5*t
crusaito(1,1*t); //slide for 1 step in 1*t
crusaito(1,3*t); //slide for 1 step in 3*t
for (int i = 0; i <= 2; i = i + (1)) { //set the variable i to save times of swinging, from 0 to 2, step is 1
drunk(4*t); //move in 4*t
}
moonLEFT(5,2*t); //left moon walking for five steps in 2*t
noGravity(2*t); //no gravity in 2*t
swing(5,2*t); //swinging for five steps in 2*t
moonRight(5,2*t); //right moon walking for five steps in 2*t
Stop(); //stop
flapping(5,2*t); //flapping for five steps in 2*t
Stop(); //stop
kickLeft(t); //kick left
for (int i = 0; i <= 2; i = i + (1)) { //set the variable i to save times of jumping, step is 1
jump(); //jump
}
Stop(); //stop
kickRight(t); //kick right
for (int i = 0; i <= 2; i = i + (1)) { //set the variable i to save times of jumping, step is 1
jump(); //jump
}
Stop(); //stop
}
Note: should upload the code success first, then plug in Bluetooth module. Otherwise, code upload fails.
Result
Done uploading the code, press down the Power_Switch on Nano Shield.
The Otto frog robot will dance. You will see unbelievable dancing movements.
