-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmotorMX430.h
150 lines (128 loc) · 6.18 KB
/
motorMX430.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
#ifndef MOTORMX430_H
#define MOTORMX430_H
#if defined(__linux__) || defined(__APPLE__)
#include <fcntl.h>
#include <termios.h>
#define STDIN_FILENO 0
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h> //used for delay
#include "dynamixel_sdk.h" // Uses Dynamixel SDK library
//Control table address for MX430-W350 (ref. LabRobotArmOfficial.py)
#define ADDR_PRO_MODEL 0
#define ADDR_PRO_OPERATING_MODE 11
#define ADDR_PRO_CURRENT_LIMIT 38
#define ADDR_PRO_ACCELERATION_LIMIT 40
#define ADDR_PRO_VELOCITY_LIMIT 44
#define ADDR_PRO_TORQUE_ENABLE 64
#define ADDR_PRO_POSITION_D_GAIN 80
#define ADDR_PRO_POSITION_I_GAIN 82
#define ADDR_PRO_POSITION_P_GAIN 84
#define ADDR_PRO_FEEDFORWARD_2nd_GAIN 88
#define ADDR_PRO_FEEDFORWARD_1st_GAIN 90
#define ADDR_PRO_GOAL_CURRENT 102
#define ADDR_PRO_GOAL_VELOCITY 104
#define ADDR_PRO_PROFILE_ACCELERATION 108
#define ADDR_PRO_PROFILE_VELOCITY 112
#define ADDR_PRO_GOAL_POSITION 116
#define ADDR_PRO_MOVING 122
#define ADDR_PRO_MOVING_STATUS 123
#define ADDR_PRO_PRESENT_CURRENT 126
#define ADDR_PRO_PRESENT_POSITION 132
//Protocol Version
#define PROTOCOL_VERSION 2.0
//Default setting
#define BAUDRATE 57600
#define DEVICENAME "/dev/ttyUSB0"
#define TORQUE_ENABLE 1
#define TORQUE_DISABLE 0
#define DXL_MINIMUN_POSITION_VALUE 0.0
#define DXL_MAXIMUN_POSITION_VALUE 4095.0
#define DXL_MOVING_STATUD_THRESHOLD 10
//Motor's limits
#define VELOCITY_LIMIT 500 //ref: LabRobotArmOfficial.py
#define ACCELERATION_LIMIT 32767
#define CURRENT_LIMIT 1193 //mA
#define MIN_MOTOR_ANGLE 0 //Use for mapping
#define MAX_MOTOR_ANGLE 4095 //Use for mapping
class MotorXM430
{
private:
//Motors
uint8_t m_ID;
uint32_t m_model_number;
//Control
uint8_t m_mode;
int8_t m_operating_mode;
int m_current_limit;
int m_goal_current;
//Status
uint32_t m_present_position;
//Communication instance and variables
dynamixel::PacketHandler* packetHandler;
dynamixel::PortHandler* portHandler;
int dxl_comm_result;
uint8_t dxl_error;
public:
//Constructor:
MotorXM430(int ID, int operating_mode, int current_limit, int goal_current);
//Functions:
//GetID: return the motors ID. Input: Void. Output : int ID. Example: MotorXM430 motor; printf("ID: %d\n",motor.GetID());
int GetID();
//GetModelNumber(): return the model of the motor. (ex: model numer 1020 is the MX430). This function is used to read the current of the motors.
//As the first function call in the construstor, we used this function to check if the motor is turn ON or without failure. In case of failure, this function will terminate the program.
uint16_t GetModelNumber();
//IsMoving(): return the moving status of the motor.Used for motor synchronisation when several motors are moving.
uint8_t IsMoving();
//MovingStatus: printf the moving status of the motors. Used for debugging.
void MovingStatus();
//ReadCurrent(): return the signed current of the motor in motorUnit.
int16_t ReadCurrent();
//MAP: return the mapped angle value into the other unit. Used for converting angle degree to angle motors.]
//Inputs: angle is the value we want to convert. in_min/max: the minimal/maximal angle value in the 1fst unit. out_min/max: the minimal/maximal angle value in the 2nd unit.
//Output: the mapped angle.
//Example: MAP(m_present_position, MIN_MOTOR_ANGLE, MAX_MOTOR_ANGLE, 0.0, 360.0)
float MAP(uint32_t angle, long in_min, long in_max, long out_min, long out_max);
//TorqueON - TorqueOFF: (dis-)activate the torque inside the motor. Without torque the motor will not move.
void TorqueON();
void TorqueOFF();
//ReadAngle: return the current angle of the motor in degree
float ReadAngle();
//Goto: set the goal position in the motor. The function do not currently check the validity of the input
//Input: wanted position in degree float
void Goto(float position);
//SetOperatingMode: set the operating mode in the motor (Availiable mode: 0 (Current), 1 (Velocity), 3 (Position), 4 (Extended position), 5 (current-based position), 16 (PWM))
//Input: wanted mode uint8_t
void SetOperatingMode(uint8_t mode);
//PrintOperationMode: printf in the console the operationmode. Used for debugging.
void PrintOperatingMode();
//SetPID: set the different parameters of the internal motor's PID controler.
//Input the P / I / D value between 0-16383 uint16_t
void SetPID(uint16_t P_gain, uint16_t I_gain, uint16_t D_gain);
//PrintPID: printf the gain value of the motor's PID in the consol. Used for debugging.
void PrintPID();
//SetFFGain: set the different parameters of the internal motor's FeedForward controler.
//Input the FF1 FF2 value between 0-16383 uint16_t
void SetFFGain(uint16_t FF1Gain, uint16_t FF2Gain);
//PrintFFGain: printf the gain value of the motor's FeedForward control in the consol. Used for debugging.
void PrintFFGain();
//SetProfile: set the acceleration and velocity profile of the motor. Used to tunned the motor behaviour and for motor synchronisation when several motors are moving.
//Input: wanted velocity (RPM) and acceleration (Rev/min2) uint32_t. The function check if the wanted value are in the motor's limits ( XM430: VELOCITY_LIMIT 500 / ACCELERATION_LIMIT 32767)
void SetProfile(uint32_t Velocity, uint32_t Acceleration);
//PrintProfile: printf the Velocity and Acceleration value in the consol. Used for debugging.
void PrintProfile();
//SetCurrentLimit: set the maximun current (torque) output of the motor. Used for current-based position control (gripper)
//Input is set in the define section CURRENT_LIMIT 1193
void SetCurrentLimit();
//PrintCurrentLimit: printf the value of the current limit (mA) in the console. Used for debugging.
void PrintCurrentLimit();
//SetGoalCurrent: set the goal current. Used for current-based position control
//Input: wanted goal current mA uint16_t. The function check if the wanted goalcurrent is not exceeding the currentlimit
void SetGoalCurrent(uint16_t GoalCurrent);
//PrintGoalcurrent: printf the value of the goalcurrent (mA) in the console. Used for debugging.
void PrintGoalCurrent();
};
#endif