Stap 5: codering
Dit is de code van de Arduino voor het beheersen van de robot met behulp van 1Sheeld
De voorwaartse en Inverse kinematica van onze Robot is hier niet besproken, maar als een alternatieve oplossing we gecontroleerd elke Servo met zijn eigen hoek om hem naar de vereiste positie.
u moet de volgende bibliotheken op uw computer geïnstalleerd: 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(); }