54 lines
No EOL
1.8 KiB
C
54 lines
No EOL
1.8 KiB
C
#ifndef SERVO_H_
|
|
#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
|
|
#define SERVO_READUPDATE_INTERVAL 1000/5
|
|
#define SERVO_MIN_COMMAND_TIME 1 //should be less than any of the other servo intervals/SERVO_COUNT
|
|
|
|
|
|
#define SERVO_COUNT 3
|
|
|
|
|
|
|
|
|
|
struct dxl_servo {
|
|
uint8_t id;
|
|
//int initial_speed;
|
|
int32_t velocity;
|
|
float offset_angle; //constant offset. set this to match angle=0 to a straight arm
|
|
float angle; //commanded angle
|
|
float actual_angle; //angle read from servo
|
|
float min_angle; //soft angle limit
|
|
float max_angle; //soft angle limit
|
|
int orientation; //1.0 = forward. -1.0 = backwards
|
|
uint8_t mode=OP_CURRENT_BASED_POSITION; // OP_POSITION, OP_CURRENT_BASED_POSITION
|
|
float current; //when mode=OP_CURRENT_BASED_POSITION, set current for servo
|
|
uint16_t modelnumber; //Servo Model number for connection check at startup. use 0 to deactivate
|
|
};
|
|
|
|
|
|
|
|
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);
|
|
float getAverageServoPositionerror();
|
|
void printServoDebug();
|
|
|
|
|
|
#endif |