44 lines
1.2 KiB
Arduino
44 lines
1.2 KiB
Arduino
|
#include "DynamixelSerial1.h"
|
||
|
|
|
||
|
|
#define pin_dinamyxel 4
|
||
|
|
long int VEL = 100;
|
||
|
|
|
||
|
|
void setup(){
|
||
|
|
Serial.begin(9600);
|
||
|
|
Dynamixel.begin(1000000,pin_dinamyxel); // Inicialize the servo at 1Mbps and Pin Control 3
|
||
|
|
delay(1000);
|
||
|
|
for(int i=0; i<6; i++) Dynamixel.setEndless(i, OFF);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop(){
|
||
|
|
|
||
|
|
// for(int j=0; j<8; j++) Dynamixel.moveSpeed(j,500, VEL); //moveSpeed(ID, Position, Speed);
|
||
|
|
// delay(5000);
|
||
|
|
Serial.println("#######################################");
|
||
|
|
for (int i=0; i<6; i++)
|
||
|
|
{
|
||
|
|
if (Dynamixel.ping(i)!=-1)
|
||
|
|
{
|
||
|
|
Serial.print("Motor ");
|
||
|
|
Serial.print(i);
|
||
|
|
Serial.println(" ON");
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Serial.print("Motor ");
|
||
|
|
Serial.print(i);
|
||
|
|
Serial.println(" OFF");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for(int j=0; j<6; j++) Dynamixel.moveSpeed(j, 256, VEL); //moveSpeed(ID, Position, Speed);
|
||
|
|
delay(5000);
|
||
|
|
for(int j=0; j<6; j++) Dynamixel.moveSpeed(j, 768, VEL); //moveSpeed(ID, Position, Speed);
|
||
|
|
delay(5000);
|
||
|
|
|
||
|
|
// for(int i=256; i<768; i=i+10)
|
||
|
|
// {
|
||
|
|
// delay(1);
|
||
|
|
// for(int j=0; j<8; j++) Dynamixel.moveSpeed(j, i, VEL); //moveSpeed(ID, Position, Speed);
|
||
|
|
// }
|
||
|
|
}
|