From 8266143dcda67f9d02948c1891affadaa8d654c4 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Fri, 6 Sep 2024 16:51:50 +0200 Subject: [PATCH] add servo functions --- prosthesis_controller.ino | 11 +- servo.cpp | 221 +++++++++++++++++++++++++++++++++++++- servo.h | 37 +++++++ vacuum.cpp | 2 +- vacuum.h | 2 +- 5 files changed, 268 insertions(+), 5 deletions(-) diff --git a/prosthesis_controller.ino b/prosthesis_controller.ino index 82b685b..a549c46 100644 --- a/prosthesis_controller.ino +++ b/prosthesis_controller.ino @@ -11,6 +11,8 @@ void setup() { pinMode(PIN_INPUTARM1,INPUT); + + initServos(); } @@ -27,14 +29,19 @@ void loop() { loopVacuum(loopmillis); + loopServos(loopmillis); + //Print Debug Information static unsigned long last_print=0; if (loopmillis - last_print >100) { last_print=loopmillis; - printVacuumValues(); + //printVacuumValues(); + //Serial.print(" Vac="); Serial.print(vac); + + printServoDebug(); + - Serial.print(" Vac="); Serial.print(vac); Serial.println(); Serial.println(); } diff --git a/servo.cpp b/servo.cpp index 6e71670..4aee0f3 100644 --- a/servo.cpp +++ b/servo.cpp @@ -1,2 +1,221 @@ #include "Arduino.h" -#include "servo.h" \ No newline at end of file +#include "servo.h" + +using namespace DYNAMIXEL; + +#define SERVO_COUNT 3 +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].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].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].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;i0 && 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) { + servos[0].angle=sin(millis/1000.0/1.0)*20; + servos[1].angle=cos(millis/1000.0/1.0)*20; + servos[2].angle=sin(millis/1000.0/1.0)*20; + + static unsigned long last_servo_update=0; + if (millis-last_servo_update>SERVO_UPDATE_INTERVAL){ + last_servo_update=millis; + for (uint8_t i=0;i "); Serial.print(servos[i].offset_angle+servos[i].angle); + 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!"); + } +} \ No newline at end of file diff --git a/servo.h b/servo.h index 45d86a3..fdd4689 100644 --- a/servo.h +++ b/servo.h @@ -2,4 +2,41 @@ #define SERVO_H_ +#include +#include "actuator.h" //needed to be able to access ControlTableItem:: + +#if defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + //#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DXL_SERIAL Serial1 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + //const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 28; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#endif + +#define DXL_SERIAL Serial1 + +#define SERVO_UPDATE_INTERVAL 1000/50 + + +struct dxl_servo { + int id; + //int initial_speed; + float offset_angle; + float angle; + float min_angle; + float max_angle; + int orientation; + uint8_t mode=OP_CURRENT_BASED_POSITION; + float current; + uint16_t modelnumber; +}; + +void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid); +void scanDynamixel(); +void changeID(uint8_t present_id, uint8_t new_id); +void initServos(); +void loopServos(unsigned long millis); +void printServoDebug(); + + #endif \ No newline at end of file diff --git a/vacuum.cpp b/vacuum.cpp index 05e2b87..002e5d3 100644 --- a/vacuum.cpp +++ b/vacuum.cpp @@ -45,7 +45,7 @@ void initVacuum() { void loopVacuum(unsigned long millis) { static unsigned long last_pressurereading = 0; - if (millis - last_pressurereading > 10) + if (millis - last_pressurereading > VACUUM_UPDATE_INTERVAL) { last_pressurereading=millis; vacuum1 = readHX710B(PIN_PRESSURE1_OUT) / 1000000.0; diff --git a/vacuum.h b/vacuum.h index 143a079..3d8f697 100644 --- a/vacuum.h +++ b/vacuum.h @@ -6,7 +6,7 @@ - +#define VACUUM_UPDATE_INTERVAL 1000/10