259 lines
No EOL
7.7 KiB
C++
259 lines
No EOL
7.7 KiB
C++
#include "Arduino.h"
|
|
#include "servo.h"
|
|
|
|
using namespace DYNAMIXEL;
|
|
|
|
const uint8_t DXL_ID = 1;
|
|
const float DXL_PROTOCOL_VERSION = 2.0;
|
|
|
|
|
|
dxl_servo servos[SERVO_COUNT];
|
|
|
|
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
|
|
|
|
void initServos()
|
|
{
|
|
//Main Arm first Servo. Model 1020
|
|
servos[0].id=11;
|
|
servos[0].mode=OP_CURRENT_BASED_POSITION;
|
|
servos[0].current=10.0;
|
|
servos[0].offset_angle=180;
|
|
servos[0].min_angle=180-90;
|
|
servos[0].max_angle=180+90;
|
|
servos[0].orientation=1;
|
|
servos[0].modelnumber=1020;
|
|
|
|
//Main Arm second Servo. Model 1080
|
|
servos[1].id=12;
|
|
servos[1].mode=OP_POSITION;
|
|
servos[1].offset_angle=180;
|
|
servos[1].min_angle=180-90;
|
|
servos[1].max_angle=180+90;
|
|
servos[1].orientation=1;
|
|
servos[1].modelnumber=1080;
|
|
|
|
//Main Arm third Servo. Model 1080
|
|
servos[2].id=13;
|
|
servos[2].mode=OP_POSITION;
|
|
servos[2].offset_angle=180;
|
|
servos[2].min_angle=180-90;
|
|
servos[2].max_angle=180+90;
|
|
servos[2].orientation=1;
|
|
servos[2].modelnumber=1080;
|
|
|
|
|
|
for(uint8_t j=0;j<10;j++) { //Startup Wait
|
|
Serial.print(j+1);Serial.println("/10");
|
|
delay(200);
|
|
}
|
|
/*changeID(1,21);
|
|
scanDynamixel(57600,DXL_PROTOCOL_VERSION,64);
|
|
|
|
while(1){
|
|
delay(100);
|
|
}*/
|
|
|
|
dxl.begin(57600);
|
|
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
|
|
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
|
|
|
|
uint8_t servoErrors=0;
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
|
|
// Get DYNAMIXEL information
|
|
if (!dxl.ping(servos[i].id)){
|
|
Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" does not respond ");
|
|
Serial.println();
|
|
delay(2000);
|
|
servoErrors++;
|
|
}else{
|
|
uint16_t servomodel=dxl.getModelNumber(servos[i].id);
|
|
if (servos[i].modelnumber!=0 && servomodel!=servos[i].modelnumber) { //if modelnumber given, check if it matches
|
|
Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" with id "); Serial.print(servos[i].id); Serial.print(" with Model "); Serial.print(servomodel); Serial.print(" does not match "); Serial.print(servos[i].modelnumber);
|
|
Serial.println();
|
|
servoErrors++;
|
|
delay(2000);
|
|
}
|
|
|
|
// Turn off torque when configuring items in EEPROM area
|
|
dxl.torqueOff(servos[i].id);
|
|
|
|
dxl.setOperatingMode(servos[i].id, servos[i].mode);
|
|
if (servos[i].current>0 && servos[i].mode==OP_CURRENT_BASED_POSITION) {
|
|
dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
|
|
}
|
|
|
|
dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, 30); //was 30
|
|
dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
|
|
|
|
dxl.torqueOn(servos[i].id);
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
if (servoErrors>0) {
|
|
scanDynamixel(57600,DXL_PROTOCOL_VERSION,DXL_BROADCAST_ID);
|
|
while(1){
|
|
delay(100);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void loopServos(unsigned long millis) {
|
|
|
|
static uint8_t servoReadI=0; //only one servo per function call
|
|
static unsigned long last_readservo_update=0;
|
|
|
|
static uint8_t servoUpdateI=0; //only one servo per function call
|
|
static unsigned long last_servo_update=0;
|
|
|
|
//Sequentially set servo angles
|
|
if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT && millis-last_readservo_update>SERVO_MIN_COMMAND_TIME){
|
|
last_servo_update=millis;
|
|
|
|
dxl.setGoalPosition(servos[servoUpdateI].id, constrain((servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation) , servos[servoUpdateI].min_angle,servos[servoUpdateI].max_angle), UNIT_DEGREE);
|
|
servoUpdateI++;
|
|
servoUpdateI%=SERVO_COUNT;
|
|
}
|
|
|
|
//Sequentially read servo status
|
|
if (millis-last_readservo_update>SERVO_READUPDATE_INTERVAL/SERVO_COUNT && millis-last_servo_update>SERVO_MIN_COMMAND_TIME){
|
|
last_readservo_update=millis;
|
|
|
|
servos[servoReadI].actual_angle = dxl.getPresentPosition(servos[servoReadI].id, UNIT_DEGREE);
|
|
|
|
servoReadI++;
|
|
servoReadI%=SERVO_COUNT;
|
|
}
|
|
|
|
}
|
|
|
|
float getAverageServoPositionerror()
|
|
{
|
|
float angleerror=0;
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
|
|
if (current_error<0){
|
|
current_error*=-1;
|
|
}
|
|
angleerror+=current_error;
|
|
}
|
|
angleerror/=SERVO_COUNT;
|
|
return angleerror;
|
|
}
|
|
|
|
|
|
void printServoDebug() {
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
//Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
|
|
Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].actual_angle); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
|
|
Serial.println();
|
|
}
|
|
Serial.print("erroravg:"); Serial.print(getAverageServoPositionerror());
|
|
Serial.println();
|
|
}
|
|
|
|
|
|
void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid) {
|
|
|
|
int8_t found_dynamixel = 0;
|
|
dxl.setPortProtocolVersion((float)protocol);
|
|
dxl.begin(baud);
|
|
for(int id = 0; id < maxid; id++) { //highest ID is DXL_BROADCAST_ID
|
|
//iterate until all ID in each buadrate is scanned.
|
|
if(dxl.ping(id)) {
|
|
DEBUG_SERIAL.print("ID : ");
|
|
DEBUG_SERIAL.print(id);
|
|
DEBUG_SERIAL.print(", Model Number: ");
|
|
DEBUG_SERIAL.println(dxl.getModelNumber(id));
|
|
found_dynamixel++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void scanDynamixel()
|
|
{
|
|
#define MAX_BAUD 5
|
|
const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000};
|
|
|
|
|
|
int8_t index = 0;
|
|
int8_t found_dynamixel = 0;
|
|
|
|
|
|
for(int8_t protocol = 1; protocol < 3; protocol++) {
|
|
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
|
|
dxl.setPortProtocolVersion((float)protocol);
|
|
DEBUG_SERIAL.print("SCAN PROTOCOL ");
|
|
DEBUG_SERIAL.println(protocol);
|
|
|
|
for(index = 0; index < MAX_BAUD; index++) {
|
|
// Set Port baudrate.
|
|
DEBUG_SERIAL.print("SCAN BAUDRATE ");
|
|
DEBUG_SERIAL.println(buad[index]);
|
|
dxl.begin(buad[index]);
|
|
for(int id = 0; id < DXL_BROADCAST_ID; id++) {
|
|
//iterate until all ID in each buadrate is scanned.
|
|
if(dxl.ping(id)) {
|
|
DEBUG_SERIAL.print("ID : ");
|
|
DEBUG_SERIAL.print(id);
|
|
DEBUG_SERIAL.print(", Model Number: ");
|
|
DEBUG_SERIAL.println(dxl.getModelNumber(id));
|
|
found_dynamixel++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
DEBUG_SERIAL.print("Total ");
|
|
DEBUG_SERIAL.print(found_dynamixel);
|
|
DEBUG_SERIAL.println(" DYNAMIXEL(s) found!");
|
|
}
|
|
|
|
|
|
|
|
void changeID(uint8_t present_id, uint8_t new_id) {
|
|
// put your setup code here, to run once:
|
|
|
|
// For Uno, Nano, Mini, and Mega, 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);
|
|
|
|
DEBUG_SERIAL.print("PROTOCOL ");
|
|
DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
|
|
DEBUG_SERIAL.print(", ID ");
|
|
DEBUG_SERIAL.print(present_id);
|
|
DEBUG_SERIAL.print(": ");
|
|
if(dxl.ping(present_id) == true) {
|
|
DEBUG_SERIAL.print("ping succeeded!");
|
|
DEBUG_SERIAL.print(", Model Number: ");
|
|
DEBUG_SERIAL.println(dxl.getModelNumber(present_id));
|
|
|
|
// Turn off torque when configuring items in EEPROM area
|
|
dxl.torqueOff(present_id);
|
|
|
|
// set a new ID for DYNAMIXEL. Do not use ID 200
|
|
if(dxl.setID(present_id, new_id) == true){
|
|
present_id = new_id;
|
|
DEBUG_SERIAL.print("ID has been successfully changed to ");
|
|
DEBUG_SERIAL.println(new_id);
|
|
}else{
|
|
DEBUG_SERIAL.print("Failed to change ID to ");
|
|
DEBUG_SERIAL.println(new_id);
|
|
}
|
|
}
|
|
else{
|
|
DEBUG_SERIAL.println("ping failed!");
|
|
}
|
|
} |