Humanoid Robot tonande kontrolleras med Arduino Mega, raspberry Pi och 1Sheeld (5 / 5 steg)
Steg 5: kodning
Detta är den Arduino kod för att styra roboten med hjälp av 1Sheeld
Framåt och omvänd kinematik av vår Robot diskuteras inte här men som en alternativ lösning vi kontrollerade varje Servo med sin egen vinkel för att få honom till Position krävs.
du behöver följande bibliotek installerat i din dator: Herkulex & 1Sheeld
#include < Wire.h >< br > #include < Herkulex.h >
#include < OneSheeld.h >
#include < SoftwareSerial.h > //Software seriell Port
#define RxD 52 //8 lenardo 52 mega
#define TxD 7
#define SLAVE_ADDRESS 0x04
#define DEBUG_ENABLED 1
int ReceivedData, Data;
int antal = 0;
float y;
CONST int Pin1 = 22.
CONST int Pin2 = 23.
void setup()
{
pinMode (Pin1, OUTPUT);
pinMode (PIN2-, OUTPUT);
Delay(1000);
pinMode (RxD, indata);
pinMode (TxD, OUTPUT);
Serial.BEGIN(115200); Öppna seriell kommunikation
Serial.println("BEGIN");
Herkulex.beginSerial3(115200); Öppna seriell port 1
Reboot();
Herkulex.Initialize(); initiera motorer
initiera i2c som slav
Wire.BEGIN(SLAVE_ADDRESS);
definiera callbacks för i2c kommunikation
Wire.onReceive(receiveData);
Wire.onRequest(sendData);
OneSheeld.begin();
Herkulex.writeRegistryEEP(254,0x13,0x8E);
Delay(200);
Herkulex.setLed(254,LED_PINK);
}
void loop()
{
Slå Humanoid på/av
/IF(PushButton.isPressed())
{
Reboot();
Intial();
Herkulex.setLed(254,LED_PINK);
}
annat om (!. PushButton.isPressed())
{
Herkulex.torqueOFF(254);
Herkulex.setLed(254,LED_GREEN2);
Herkulex.End();
}
IF(ReceivedData == 2) //Giving en feedback om de upptäckta Juics
{
Delay(10000);
TextToSpeech.say ("din Juics är redo");
Delay(100);
ReceivedData = 255;
}
IF(ReceivedData == 3) //taking en Selfi
{
Camera.setFlash(ON);
Camera.rearCapture();
Delay(10000);
Facebook.postLastPicture ("Inlagd av Maelo");
Delay(100);
ReceivedData = 255;
}
Sensing larm ringsignal för mobilen
IF(MIC.getValue() > 80)
{
Delay(10000);
WakeUp();
Delay(1000);
digitalWrite (Pin1, hög);
digitalWrite (Pin2, låg);
Delay(300);
digitalWrite (Pin1, låg);
digitalWrite (Pin2, låg);
}
IF(LightSensor.getValue() < 20 & & LightSensor.getValue() > 0)
{
Sleep();
Delay(1000);
digitalWrite (Pin1, låg);
digitalWrite (Pin2, hög);
Delay(300);
digitalWrite (Pin1, låg);
digitalWrite (Pin2, låg);
}
Accelometer kontroll
om (GamePad.isRedPressed())
{
y=AccelerometerSensor.getY();
om (y < 0) //Right Arm
{
rightArm();
}
annars om (y > 0)
{
leftArm();
}
}
Dans och musik
annars om (GamePad.isGreenPressed())
{
MusicPlayer.play();
SimpleDance();
MusicPlayer.pause();
}
annars om (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);
}
motringning för mottagna data
void receiveData (int byteCount) {
While(Wire.available()) {
Data = Wire.read();
Serial.Print ("mottagna data:");
Serial.println(data);
ReceivedData = Data;
}
}
motringning för att skicka data
void sendData() {
antal = ReceivedData;
Wire.write(Number);
}
void Reboot()
{
Herkulex.Reboot(0); omstart första motor
Herkulex.Reboot(1); omstart första motor
Herkulex.Reboot(2); omstart första motor
Herkulex.Reboot(3); omstart första motor
Herkulex.Reboot(4); omstart första motor
Herkulex.Reboot(5); omstart första motor
Herkulex.Reboot(6); omstart första motor
Herkulex.Reboot(7); omstart första motor
Herkulex.Reboot(8); omstart första motor
Herkulex.Reboot(9); omstart första motor
Herkulex.Reboot(10); omstart första motor
Herkulex.Reboot(11); omstart första motor
Herkulex.Reboot(12); omstart första motor
Herkulex.Reboot(13); omstart första motor
Herkulex.Reboot(14); omstart första motor
Herkulex.Reboot(15); omstart första motor
Herkulex.Reboot(16); omstart första motor
Herkulex.Reboot(17); omstart första motor
Herkulex.Reboot(18); omstart första motor
Herkulex.Reboot(19); omstart första motor
Delay(500);
Herkulex.Initialize(); initiera motorer
}
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()
{
för (int jag = 0; jag < 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 antal, 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();
}