130 lines
3.8 KiB
C++
130 lines
3.8 KiB
C++
|
|
#include <Dynamixel2Arduino.h>
|
|
#define DXL_SERIAL Serial1
|
|
#define DEBUG_SERIAL Serial
|
|
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
|
|
|
|
const uint8_t DXL_ID = 1;
|
|
const float DXL_PROTOCOL_VERSION = 1.0;
|
|
|
|
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
|
|
|
|
//This namespace is required to use Control table item names
|
|
using namespace ControlTableItem;
|
|
|
|
long zero_offset = 0.0;
|
|
|
|
void setup() {
|
|
// put your setup code here, to run once:
|
|
|
|
// Use UART port of DYNAMIXEL Shield to debug.
|
|
DEBUG_SERIAL.begin(115200);
|
|
while(!DEBUG_SERIAL);
|
|
|
|
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
|
|
dxl.begin(57600);
|
|
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
|
|
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
|
|
// Get DYNAMIXEL information
|
|
dxl.ping(DXL_ID);
|
|
|
|
// Turn off torque when configuring items in EEPROM area
|
|
dxl.torqueOff(DXL_ID);
|
|
dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
|
|
dxl.writeControlTableItem(RESOLUTION_DIVIDER, DXL_ID, 1);
|
|
dxl.torqueOn(DXL_ID);
|
|
|
|
// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
|
|
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0);
|
|
delay(500);
|
|
|
|
|
|
// long current_position = dxl.getPresentPosition(DXL_ID);
|
|
// zero_offset = current_position;
|
|
// Serial.print("Zero_offset: ");
|
|
// Serial.println(zero_offset);
|
|
// delay(100);
|
|
|
|
// Serial.print("Going to: ");
|
|
// Serial.println(zero_offset);
|
|
// dxl.setGoalPosition(DXL_ID, zero_offset); delay(2000);
|
|
|
|
// current_position = dxl.getPresentPosition(DXL_ID);
|
|
// Serial.print("now at: ");
|
|
// Serial.println(current_position);
|
|
}
|
|
|
|
|
|
|
|
void loop() {
|
|
|
|
// while(Serial.available() == 0) { }
|
|
// String serial_string;
|
|
// char char_input[6]; //char array buffer
|
|
// int int_input[4]; //char array buffer
|
|
// serial_string = Serial.readString();
|
|
// // serial_string.toCharArray(char_input, 6);
|
|
// long output = serial_string.toInt();
|
|
|
|
// Serial.print("char array: ");
|
|
// Serial.print(char_input[0]);
|
|
// Serial.print(char_input[1]);
|
|
// Serial.print(char_input[2]);
|
|
// Serial.print(char_input[3]);
|
|
// Serial.print(char_input[4]);
|
|
// Serial.println(char_input[5]);
|
|
|
|
// int_input[0] = char_input[0].toInt();
|
|
// int_input[1] = char_input[0].toInt();
|
|
// int_input[2] = char_input[0].toInt();
|
|
// int_input[3] = char_input[0].toInt();
|
|
|
|
// Serial.print("atoi char array: ");
|
|
// Serial.print(atoi(char_input[0]));
|
|
// Serial.print(atoi(char_input[1]));
|
|
// Serial.print(atoi(char_input[2]));
|
|
// Serial.print(atoi(char_input[3]));
|
|
// Serial.print(atoi(char_input[4]));
|
|
// Serial.println(atoi(char_input[5]));
|
|
// // delay(10000000);
|
|
|
|
// output = output + (char_input[1] - "0") * 1000;
|
|
// output = output + (char_input[2] - "0") * 100;
|
|
// output = output + (char_input[3] - "0") * 10;
|
|
// output = output + (char_input[4] - "0") * 1;
|
|
// if (char_input[0] == "-") {output = output * -1;}
|
|
|
|
// Serial.print(" Input recieved: ");
|
|
// Serial.println(output);
|
|
long current_position = dxl.getPresentPosition(DXL_ID);
|
|
|
|
// Serial.print(" Current: ");
|
|
// Serial.print(current_position);
|
|
// Serial.print(", target: ");
|
|
// Serial.print(output);
|
|
// Serial.println(", see you in 100ms");
|
|
|
|
// dxl.setGoalPosition(DXL_ID, output);
|
|
// delay(1000);
|
|
|
|
// current_position = dxl.getPresentPosition(DXL_ID);
|
|
|
|
// Serial.print("We moved, Now at: ");
|
|
// Serial.println(current_position);
|
|
// Serial.println();
|
|
|
|
|
|
// put your main code here, to run repeatedly:
|
|
|
|
// Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value.
|
|
// Set Goal Position in RAW value
|
|
|
|
Serial.print("pos: ");
|
|
Serial.println(current_position);
|
|
delay(500);
|
|
// int i_present_position = 0;
|
|
// i_present_position = dxl.getPresentPosition(DXL_ID);
|
|
|
|
|
|
}
|