Paso 5: codificación
Este es el código de Arduino para controlar el robot con 1Sheeld
El avance y la cinemática inversa de nuestro Robot no se discute aquí, pero como una solución alternativa controla cada Servo con su propio ángulo para llevarlo a la posición requerida.
usted necesitará las siguientes bibliotecas instaladas en su computadora: Herkulex & 1Sheeld
#include <Wire.h><br>#include <Herkulex.h> #include <OneSheeld.h> #include <SoftwareSerial.h> //Software Serial Port #define RxD 52 //8 lenardo 52 mega #define TxD 7 #define SLAVE_ADDRESS 0x04 #define DEBUG_ENABLED 1
int ReceivedData , Data; int number = 0; float y;
const int Pin1 = 22; const int Pin2 = 23;
void setup() { pinMode(Pin1, OUTPUT); pinMode(Pin2, OUTPUT); delay(1000); pinMode(RxD, INPUT); pinMode(TxD, OUTPUT); Serial.begin(115200); // Open serial communications Serial.println("Begin"); Herkulex.beginSerial3(115200); //open serial port 1 Reboot(); Herkulex.initialize(); //initialize motors // initialize i2c as slave Wire.begin(SLAVE_ADDRESS); // define callbacks for i2c communication Wire.onReceive(receiveData); Wire.onRequest(sendData); OneSheeld.begin(); Herkulex.writeRegistryEEP(254,0x13,0x8E); delay(200); Herkulex.setLed(254,LED_PINK); }
void loop() { //Switch the Humanoid On/OFF /if(PushButton.isPressed()) { Reboot(); Intial(); Herkulex.setLed(254,LED_PINK); } else if(!PushButton.isPressed()) { Herkulex.torqueOFF(254); Herkulex.setLed(254,LED_GREEN2); Herkulex.end(); } if(ReceivedData == 2) //Giving a feedback about the detected Juics { delay(10000); TextToSpeech.say("your Juics is ready"); delay(100); ReceivedData = 255; }
if(ReceivedData == 3) //taking a Selfi { Camera.setFlash(ON); Camera.rearCapture(); delay(10000); Facebook.postLastPicture("Posted by Maelo"); delay(100); ReceivedData = 255; } //sensing the alarm ringtone of the mobile if(Mic.getValue() > 80) { delay(10000); WakeUp(); delay(1000); digitalWrite(Pin1, HIGH); digitalWrite(Pin2, LOW); delay(300); digitalWrite(Pin1, LOW); digitalWrite(Pin2, LOW); } if(LightSensor.getValue() < 20 && LightSensor.getValue() > 0) { sleep(); delay(1000); digitalWrite(Pin1, LOW); digitalWrite(Pin2, HIGH); delay(300); digitalWrite(Pin1, LOW); digitalWrite(Pin2, LOW); }
//Accelometer Control if (GamePad.isRedPressed()) { y=AccelerometerSensor.getY(); if ( y < 0) //Right Arm { rightArm(); } else if (y > 0) { leftArm(); } } //Dance and music else if (GamePad.isGreenPressed()) { MusicPlayer.play(); SimpleDance(); MusicPlayer.pause(); } else if (GamePad.isBluePressed()) { Intial(); } }
void rightArm() { Herkulex.moveOneAngle(1, 130, 1000, LED_BLUE); Herkulex.moveOneAngle(4, -10, 1000, LED_BLUE); delay(1000); } void leftArm() { Herkulex.moveOneAngle(1, 10, 1000, LED_BLUE); Herkulex.moveOneAngle(4, -130, 1000, LED_BLUE); delay(1000); }
// callback for received data void receiveData(int byteCount){
while(Wire.available()) { Data = Wire.read(); Serial.print("data received: "); Serial.println(Data); ReceivedData = Data; } }
// callback for sending data void sendData(){ number = ReceivedData; Wire.write(number); }
void Reboot() { Herkulex.reboot(0); //reboot first motor Herkulex.reboot(1); //reboot first motor Herkulex.reboot(2); //reboot first motor Herkulex.reboot(3); //reboot first motor Herkulex.reboot(4); //reboot first motor Herkulex.reboot(5); //reboot first motor Herkulex.reboot(6); //reboot first motor Herkulex.reboot(7); //reboot first motor Herkulex.reboot(8); //reboot first motor Herkulex.reboot(9); //reboot first motor Herkulex.reboot(10); //reboot first motor Herkulex.reboot(11); //reboot first motor Herkulex.reboot(12); //reboot first motor Herkulex.reboot(13); //reboot first motor Herkulex.reboot(14); //reboot first motor Herkulex.reboot(15); //reboot first motor Herkulex.reboot(16); //reboot first motor Herkulex.reboot(17); //reboot first motor Herkulex.reboot(18); //reboot first motor Herkulex.reboot(19); //reboot first motor delay(500); Herkulex.initialize(); //initialize motors }
void Intial() { Herkulex.moveOneAngle(0, 98.00, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 10.00, 1000, LED_BLUE); Herkulex.moveOneAngle(2, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(3, -98.00, 1000, LED_BLUE); Herkulex.moveOneAngle(4, -10.00, 1000, LED_BLUE); Herkulex.moveOneAngle(5, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(6, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(7, -5.00, 1000, LED_BLUE); Herkulex.moveOneAngle(8, 2.00, 1000, LED_BLUE); Herkulex.moveOneAngle(9, 4.00, 1000, LED_BLUE); Herkulex.moveOneAngle(10, -2.00, 1000, LED_BLUE); Herkulex.moveOneAngle(11, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(12, 8.00, 1000, LED_BLUE); Herkulex.moveOneAngle(13, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(14, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(15, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(16, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(17, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(18, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(19, 0.00, 1000, LED_BLUE); delay(1200); }
void Bye() { for(int i=0; i <3; i++) { Herkulex.moveOneAngle(0, 88, 700, LED_BLUE); Herkulex.moveOneAngle(1, 159, 700, LED_BLUE); Herkulex.moveOneAngle(2, 77, 700, LED_BLUE); Herkulex.moveOneAngle(3, -93, 700, LED_BLUE); Herkulex.moveOneAngle(4, -120, 700, LED_BLUE); Herkulex.moveOneAngle(5, -2, 700, LED_BLUE); delay(900); Herkulex.moveOneAngle(0, 76, 700, LED_GREEN); Herkulex.moveOneAngle(1, 92, 700, LED_GREEN); Herkulex.moveOneAngle(2, 3, 700, LED_GREEN); Herkulex.moveOneAngle(3, -76, 700, LED_GREEN); Herkulex.moveOneAngle(4, -140, 700, LED_GREEN); Herkulex.moveOneAngle(5, -83, 700, LED_GREEN); delay(900); } }
void SimpleDance() { Herkulex.moveOneAngle(3, -150, 1000, LED_BLUE); delay(950); Herkulex.moveOneAngle(3, -100, 1000, LED_BLUE); Herkulex.moveOneAngle(4, -60, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 60, 1000, LED_BLUE); delay(950); Herkulex.moveOneAngle(0, 110, 1000, LED_PINK); Herkulex.moveOneAngle(1, 25, 1000, LED_PINK); Herkulex.moveOneAngle(3, -150, 1000, LED_PINK); Herkulex.moveOneAngle(4, -25, 1000, LED_PINK); delay(1000); Herkulex.moveOneAngle(7, -0.97, 1000, LED_BLUE); Herkulex.moveOneAngle(8, 2.60, 1000, LED_BLUE); Herkulex.moveOneAngle(9, 4.88, 1000, LED_BLUE); Herkulex.moveOneAngle(12, 1.63, 1000, LED_BLUE); Herkulex.moveOneAngle(13, 0.00, 1000, LED_BLUE); Herkulex.moveOneAngle(14, -1.63, 1000, LED_BLUE); Herkulex.moveOneAngle(0, 150, 1000, LED_GREEN); Herkulex.moveOneAngle(3, -110, 1000, LED_BLUE); delay(1000); Herkulex.moveOneAngle(0, 110, 1000, LED_BLUE); Herkulex.moveOneAngle(3, -150, 1000, LED_GREEN); delay(1000); Herkulex.moveOneAngle(0, 150, 1000, LED_GREEN); Herkulex.moveOneAngle(3, -110, 1000, LED_BLUE); delay(1000); Herkulex.moveOneAngle(0, 110, 1000, LED_BLUE); Herkulex.moveOneAngle(3, -150, 1000, LED_GREEN); delay(1000); Herkulex.moveOneAngle(0, 160, 1000, LED_GREEN); Herkulex.moveOneAngle(3, -160, 1000, LED_GREEN); Herkulex.moveOneAngle(4, -70, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 0, 1000, LED_BLUE); delay(1000); Herkulex.moveOneAngle(4, 0, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 60, 1000, LED_BLUE); delay(1000); Herkulex.moveOneAngle(0, 98.70, 900, LED_BLUE); Herkulex.moveOneAngle(1, 10.00, 900, LED_BLUE); Herkulex.moveOneAngle(3, -98.70, 900, LED_BLUE); Herkulex.moveOneAngle(4, -10.00, 900, LED_BLUE); delay(800); Herkulex.moveOneAngle(0, 160, 900, LED_GREEN); Herkulex.moveOneAngle(3, -160, 900, LED_GREEN); Herkulex.moveOneAngle(4, -70, 900, LED_BLUE); Herkulex.moveOneAngle(1, 0, 900, LED_BLUE); delay(900); Herkulex.moveOneAngle(0, 89.05, 1500, LED_BLUE); Herkulex.moveOneAngle(1, 116.67, 1500, LED_BLUE); Herkulex.moveOneAngle(3, -92.30, 1500, LED_BLUE); Herkulex.moveOneAngle(4, -149.83, 1500, LED_BLUE); Herkulex.moveOneAngle(6, 12.02, 1500, LED_BLUE); Herkulex.moveOneAngle(10, 1.63, 1500, LED_BLUE); Herkulex.moveOneAngle(11, 30.55, 1500, LED_BLUE); Herkulex.moveOneAngle(15, 20.47, 1500, LED_BLUE); delay(1500); Herkulex.moveOneAngle(6, -7.00, 1000, LED_BLUE); Herkulex.moveOneAngle(10, -8.60, 1000, LED_BLUE); Herkulex.moveOneAngle(11, 8.00, 1000, LED_BLUE); Herkulex.moveOneAngle(15, 8, 1000, LED_BLUE); delay(900); Herkulex.moveOneAngle(1, 144.95, 1500, LED_BLUE); Herkulex.moveOneAngle(4, -123.17, 1500, LED_BLUE); Herkulex.moveOneAngle(6, -22.42, 1500, LED_BLUE); Herkulex.moveOneAngle(10, -19.17, 1500, LED_BLUE); Herkulex.moveOneAngle(11, -5.85, 1500, LED_BLUE); Herkulex.moveOneAngle(15, -1.63, 1500, LED_BLUE); delay(1500); Herkulex.moveOneAngle(6, -7.00, 1000, LED_BLUE); Herkulex.moveOneAngle(10, -8.60, 1000, LED_BLUE); Herkulex.moveOneAngle(11, 8.00, 1000, LED_BLUE); Herkulex.moveOneAngle(15, 8, 1000, LED_BLUE); delay(1500); Herkulex.moveOneAngle(0, 89.05, 1500, LED_BLUE); Herkulex.moveOneAngle(1, 116.67, 1500, LED_BLUE); Herkulex.moveOneAngle(3, -92.30, 1500, LED_BLUE); Herkulex.moveOneAngle(4, -149.83, 1500, LED_BLUE); Herkulex.moveOneAngle(6, 12.02, 1500, LED_BLUE); Herkulex.moveOneAngle(10, 1.63, 1500, LED_BLUE); Herkulex.moveOneAngle(11, 30.55, 1500, LED_BLUE); Herkulex.moveOneAngle(15, 20.47, 1500, LED_BLUE); delay(1500); Herkulex.moveOneAngle(6, -7.00, 1000, LED_BLUE); Herkulex.moveOneAngle(10, -8.60, 1000, LED_BLUE); Herkulex.moveOneAngle(11, 8.00, 1000, LED_BLUE); Herkulex.moveOneAngle(15, 8, 1000, LED_BLUE); delay(900); Herkulex.moveOneAngle(1, 144.95, 1500, LED_BLUE); Herkulex.moveOneAngle(4, -123.17, 1500, LED_BLUE); Herkulex.moveOneAngle(6, -22.42, 1500, LED_BLUE); Herkulex.moveOneAngle(10, -19.17, 1500, LED_BLUE); Herkulex.moveOneAngle(11, -5.85, 1500, LED_BLUE); Herkulex.moveOneAngle(15, -1.63, 1500, LED_BLUE); delay(1500); Herkulex.moveOneAngle(6, -7.00, 1000, LED_BLUE); Herkulex.moveOneAngle(10, -8.60, 1000, LED_BLUE); Herkulex.moveOneAngle(11, 8.00, 1000, LED_BLUE); Herkulex.moveOneAngle(15, 8, 1000, LED_BLUE); delay(1500); Intial(); }
void sleep() { Herkulex.moveOneAngle(0, 97.00, 2000, LED_BLUE); Herkulex.moveOneAngle(1, 9.00, 2000, LED_BLUE); Herkulex.moveOneAngle(2, -1.00, 2000, LED_BLUE); Herkulex.moveOneAngle(3, -97.00, 2000, LED_BLUE); Herkulex.moveOneAngle(4, -9.00, 2000, LED_BLUE); Herkulex.moveOneAngle(5, -1.00, 2000, LED_BLUE); Herkulex.moveOneAngle(6, 1.00, 2000, LED_BLUE); Herkulex.moveOneAngle(7, -37.00, 2000, LED_BLUE); Herkulex.moveOneAngle(8, -69.00, 2000, LED_BLUE); Herkulex.moveOneAngle(9, 43.00, 2000, LED_BLUE); Herkulex.moveOneAngle(10, 1.00, 2000, LED_BLUE); Herkulex.moveOneAngle(11, 1.00, 2000, LED_BLUE); Herkulex.moveOneAngle(12, 43.00, 2000, LED_BLUE); Herkulex.moveOneAngle(13, 69.00, 2000, LED_BLUE); Herkulex.moveOneAngle(14, -37.00, 2000, LED_BLUE); Herkulex.moveOneAngle(15, 1.00, 2000, LED_BLUE); }
void WakeUp() { Herkulex.moveOneAngle(6, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(7, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(8, 2.00, 1200, LED_BLUE); Herkulex.moveOneAngle(9, 4.00, 1200, LED_BLUE); Herkulex.moveOneAngle(10, -2.00, 1200, LED_BLUE); Herkulex.moveOneAngle(11, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(12, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(13, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(14, 0.00, 1200, LED_BLUE); Herkulex.moveOneAngle(15, 0.00, 1200, LED_BLUE); delay(1000); Herkulex.moveOneAngle(0, 160.00, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 30.00, 1200, LED_BLUE); Herkulex.moveOneAngle(2, -1.00, 1200, LED_BLUE); Herkulex.moveOneAngle(3, -143.00, 1200, LED_BLUE); Herkulex.moveOneAngle(4, -26.00, 1200, LED_BLUE); delay(1200); Herkulex.moveOneAngle(0, 160.00, 1000, LED_BLUE); Herkulex.moveOneAngle(1, 90.00, 1000, LED_BLUE); Herkulex.moveOneAngle(2, -1.00, 1000, LED_BLUE); Herkulex.moveOneAngle(3, -159.00, 1000, LED_BLUE); Herkulex.moveOneAngle(4, -64.00, 1000, LED_BLUE); delay(1200); Intial(); }