add servo functions
This commit is contained in:
parent
0301479357
commit
8266143dcd
5 changed files with 268 additions and 5 deletions
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
219
servo.cpp
219
servo.cpp
|
|
@ -1,2 +1,221 @@
|
|||
#include "Arduino.h"
|
||||
#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;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) {
|
||||
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<SERVO_COUNT;i++) {
|
||||
dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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.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!");
|
||||
}
|
||||
}
|
||||
37
servo.h
37
servo.h
|
|
@ -2,4 +2,41 @@
|
|||
#define SERVO_H_
|
||||
|
||||
|
||||
#include <Dynamixel2Arduino.h>
|
||||
#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
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
2
vacuum.h
2
vacuum.h
|
|
@ -6,7 +6,7 @@
|
|||
|
||||
|
||||
|
||||
|
||||
#define VACUUM_UPDATE_INTERVAL 1000/10
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue