59 lines
1.5 KiB
Arduino
59 lines
1.5 KiB
Arduino
|
#include "DynamixelSerial.h"
|
||
|
|
|
||
|
|
//TLC5940NT pin definitions
|
||
|
|
#define VPRG 2
|
||
|
|
#define SIN 11
|
||
|
|
#define SCLK 13
|
||
|
|
#define XLAT 4
|
||
|
|
#define BLANK 5
|
||
|
|
#define DCPRG 6
|
||
|
|
#define GSCLK 7
|
||
|
|
uint8_t dataPin = 8;
|
||
|
|
uint8_t latchPin = 9;
|
||
|
|
uint8_t clockPin = 10;
|
||
|
|
|
||
|
|
#define pin_dinamyxel 3
|
||
|
|
long int VEL = 100;
|
||
|
|
|
||
|
|
void setup(){
|
||
|
|
pinMode(VPRG, OUTPUT);
|
||
|
|
pinMode(SIN, OUTPUT);
|
||
|
|
pinMode(SCLK, OUTPUT);
|
||
|
|
pinMode(XLAT, OUTPUT);
|
||
|
|
pinMode(BLANK, OUTPUT);
|
||
|
|
pinMode(DCPRG, OUTPUT);
|
||
|
|
pinMode(GSCLK, OUTPUT);
|
||
|
|
pinMode(MISO, INPUT);
|
||
|
|
pinMode(SS,OUTPUT);
|
||
|
|
pinMode(dataPin, OUTPUT);
|
||
|
|
pinMode(latchPin, OUTPUT);
|
||
|
|
pinMode(clockPin, OUTPUT);
|
||
|
|
digitalWrite(SS,HIGH); //disable device
|
||
|
|
digitalWrite(SIN, LOW);
|
||
|
|
digitalWrite(SCLK, LOW);
|
||
|
|
digitalWrite(XLAT, LOW);
|
||
|
|
digitalWrite(VPRG, LOW);
|
||
|
|
digitalWrite(BLANK, HIGH);
|
||
|
|
digitalWrite(GSCLK, HIGH);
|
||
|
|
digitalWrite(DCPRG, LOW); // USE EEPROM DC register if LOW
|
||
|
|
Dynamixel.begin(1000000,pin_dinamyxel); // Inicialize the servo at 1Mbps and Pin Control 3
|
||
|
|
delay(1000);
|
||
|
|
for(int i=0; i<8; 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);
|
||
|
|
|
||
|
|
for(int j=0; j<8; j++) Dynamixel.moveSpeed(j, 256, VEL); //moveSpeed(ID, Position, Speed);
|
||
|
|
delay(5000);
|
||
|
|
for(int j=0; j<8; 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);
|
||
|
|
// }
|
||
|
|
}
|