Étape 5: codage
C’est le code de l’Arduino pour contrôler le robot à l’aide de 1Sheeld
L’avant et la cinématique Inverse de notre Robot n’est pas question ici, mais comme une Solution Alternative, nous avons contrôlé chaque Servo avec son propre angle pour l’amener à la Position requise.
les bibliothèques suivantes installées sur votre ordinateur, vous aurez besoin : 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(); }