-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAxelFlow.h
256 lines (235 loc) · 9.75 KB
/
AxelFlow.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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
/*
* TODO RM_Dynamixel_AX-12A.h
*
* Created on: Apr 20, 2024
* Author: srikar
*/
#ifndef AXELFLOW_H
#define AXELFLOW_H
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "AxelFlow_Debug.h"
#include "AxelFlow_Serial.h"
//#include <stm32f0xx_hal_uart.h>
//#include "stm32f0xx_hal.h"
//#########################################################################
//################ define - Dynamixel Hex code table ######################
// EEPROM AREA
#define EEPROM_MODEL_NUMBER_L 0x00
#define EEPROM_MODEL_NUMBER_H 0x01
#define EEPROM_VERSION 0x02
#define EEPROM_ID 0x03
#define EEPROM_BAUD_RATE 0x04
#define EEPROM_RETURN_DELAY_TIME 0x05
#define EEPROM_CW_ANGLE_LIMIT_L 0x06
#define EEPROM_CW_ANGLE_LIMIT_H 0x07
#define EEPROM_CCW_ANGLE_LIMIT_L 0x08
#define EEPROM_CCW_ANGLE_LIMIT_H 0x09
#define EEPROM_LIMIT_TEMPERATURE 0x0B
#define EEPROM_LOW_LIMIT_VOLTAGE 0x0C
#define EEPROM_HIGH_LIMIT_VOLTAGE 0x0D
#define EEPROM_MAX_TORQUE_L 0x0E
#define EEPROM_MAX_TORQUE_H 0x0F
#define EEPROM_RETURN_LEVEL 0x10
#define EEPROM_ALARM_LED 0x11
#define EEPROM_ALARM_SHUTDOWN 0x12
// RAM AREA
#define RAM_TORQUE_ENABLE 0x18
#define RAM_LED 0x19
#define RAM_PROPORTIONAL_GAIN 0x1C
#define RAM_INTERGRAL_GAIN 0x1B
#define RAM_DERIVATIVE_GAIN 0x1A
#define RAM_GOAL_POSITION_L 0x1E
#define RAM_GOAL_POSITION_H 0x1F
#define RAM_GOAL_SPEED_L 0x20
#define RAM_GOAL_SPEED_H 0x21
#define RAM_TORQUE_LIMIT_L 0x22
#define RAM_TORQUE_LIMIT_H 0x23
#define RAM_PRESENT_POSITION_L 0x24
#define RAM_PRESENT_POSITION_H 0x25
#define RAM_PRESENT_SPEED_L 0x26
#define RAM_PRESENT_SPEED_H 0x27
#define RAM_PRESENT_LOAD_L 0x28
#define RAM_PRESENT_LOAD_H 0x29
#define RAM_PRESENT_VOLTAGE 0x2A
#define RAM_PRESENT_TEMPERATURE 0x2B
#define RAM_REGISTER 0x2C
#define RAM_MOVING 0x2E
#define RAM_LOCK 0x2F
#define RAM_PUNCH_L 0x30
#define RAM_PUNCH_H 0x31
//#########################################################################
//################ Instruction commands Set ###############################
#define COMMAND_PING 0x01
#define COMMAND_READ_DATA 0x02
#define COMMAND_WRITE_DATA 0x03
#define COMMAND_REG_WRITE_DATA 0x04
#define COMMAND_ACTION 0x05
#define COMMAND_RESET 0x06
#define COMMAND_REBOOT 0x08
#define COMMAND_SYNC_WRITE 0x83
//#########################################################################
//################ Instruction packet lengths #############################
// Packet length is number of parameters (N) + 2
#define READ_ONE_BYTE_LENGTH 0x01
#define READ_TWO_BYTE_LENGTH 0x02
#define RESET_LENGTH 0x02
#define PING_LENGTH 0x02
#define ACTION_LENGTH 0x02
#define SET_ID_LENGTH 0x04
#define SET_BD_LENGTH 0x04
#define SET_RETURN_LEVEL_LENGTH 0x04
#define READ_TEMP_LENGTH 0x04
#define READ_POS_LENGTH 0x04
#define READ_LOAD_LENGTH 0x04
#define READ_SPEED_LENGTH 0x04
#define READ_VOLT_LENGTH 0x04
#define READ_REGISTER_LENGTH 0x04
#define READ_MOVING_LENGTH 0x04
#define READ_LOCK_LENGTH 0x04
#define LED_LENGTH 0x04
#define SET_HOLDING_TORQUE_LENGTH 0x04
#define SET_MAX_TORQUE_LENGTH 0x05
#define SET_ALARM_LENGTH 0x04
#define READ_LOAD_LENGTH 0x04
#define SET_RETURN_LENGTH 0x04
#define WHEEL_LENGTH 0x05
#define SERVO_GOAL_LENGTH 0x07
#define SET_MODE_LENGTH 0x07
#define SET_PUNCH_LENGTH 0x04
#define SET_PID_LENGTH 0x06
#define SET_TEMP_LENGTH 0x04
#define SET_VOLT_LENGTH 0x05
#define SYNC_LOAD_LENGTH 0x0D
#define SYNC_DATA_LENGTH 0x02
//#########################################################################
//############################ Specials ###################################
#define PORT0 0x00
#define PORT1 0x01
#define PORT2 0x02
#define PORT3 0x03
#define OFF 0x00
#define ON 0x01
#define SERVO 0x01
#define WHEEL 0x00
#define LEFT 0x00
#define RIGHT 0x01
#define NONE 0x00
#define READ 0x01
#define ALL 0x02
#define BROADCAST_ID 0xFE
#define ID_MIN 0x00
#define ID_MAX 0xFD
#define MAX_JOINT_SPEED 0x00
#define CLOCKWISE_SWITCH 1
typedef struct
{
uint8_t id;
UART_HandleTypeDef huartx;
bool jointMode;
} Servo;
typedef enum
{
Input_Voltage_Error = 0x00,
Angle_Limit_Error,
Overheating_Error,
Range_Error,
Checksum_Error,
Overload_Error,
Instruction_Error,
} Status_Error;
typedef enum
{
BAUD_9600 = 0xCF, // Margin of Error = -0.160%
BAUD_19200 = 0x67, // Margin of Error = -0.160%
BAUD_57600 = 0x22, // Margin of Error = 0.794%
BAUD_115200 = 0x10, // Margin of Error = -2.124%
BAUD_200000 = 0x09, // Margin of Error = 0.000%
BAUD_250000 = 0x07, // Margin of Error = 0.000%
BAUD_400000 = 0x04, // Margin of Error = 0.000%
BAUD_500000 = 0x03, // Margin of Error = 0.000%
BAUD_DEFAULT = 0x01 // Margin of Error = 0.000% (default)
} BaudRate;
//TODO: CLean this up
unsigned int setStatusPaketReturnDelay(unsigned char, unsigned char);
unsigned int setID(unsigned char, unsigned char);
unsigned int setBaudRate(unsigned char, long);
unsigned int setHoldingTorque(unsigned char, bool);
unsigned int setAlarmShutdown(unsigned char, unsigned char);
unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
unsigned int setPID(unsigned char, unsigned char, unsigned char, unsigned char);
unsigned int setTemp(unsigned char, unsigned char);
unsigned int setVoltage(unsigned char, unsigned char, unsigned char);
unsigned int wheel(unsigned char, bool, unsigned int);
void wheelSync(unsigned char, bool, unsigned int, unsigned char, bool,
unsigned int, unsigned char, bool, unsigned int);
unsigned int wheelPreload(unsigned char, bool, unsigned int);
unsigned int readTemperature(unsigned char);
unsigned int readVoltage(unsigned char);
unsigned int readLoad(unsigned char);
unsigned int readSpeed(unsigned char);
unsigned int checkRegister(unsigned char);
unsigned int checkMovement(unsigned char);
unsigned int checkLock(unsigned char);
void transmitInstructionPacket(void);
unsigned int readStatusPacket(void);
Servo AxelFlow_servo_init(uint8_t id, UART_HandleTypeDef *huartx,
bool jointMode);
bool ping(Servo servo);
uint8_t getFirmwareVersion(Servo servo);
void print_status(Status_Packet packet, bool just_Errors);
bool checkChecksum(Status_Packet packet);
uint8_t* degreesToData(float degrees);
Status_Packet setCCWLimit(float max_angle, Servo servo);
uint16_t getCCWLimit(Servo servo);
uint16_t getCWLimit(Servo servo);
Status_Packet setCWLimit(float max_angle, Servo servo);
void pack_packet(void);
Status_Packet setMaxTorque(float torque, Servo servo); // torque 0 to 100%
Status_Packet setRunningTorque(float torque, Servo servo); // torque 0 to 100%
Status_Packet setPosition(float angle, Servo servo);
Status_Packet controlLED(bool state, Servo servo);
float dataToDegrees(uint8_t *Param);
Status_Packet getPosition(Servo servo);
Status_Packet changeServoID(uint8_t future_ID, Servo servo);
Status_Packet changeBaudRate(uint32_t new_Baud_Rate, Servo servo);
Status_Packet forceSetPosition(uint16_t angle, Servo servo1);
float getPositionAngle(Servo servo);
BaudRate getBaudRate(int new_Baud_Rate);
uint32_t readBaudRate(Servo servo);
uint8_t readID(Servo servo);
uint8_t scanID(Servo servo);
uint32_t scanBaudRate(Servo servo);
uint16_t getResponseDelayTime(Servo servo);
Status_Packet setResponseDelayTime(uint16_t response_time, Servo servo);//in micro Seconds
uint8_t getLEDStatus(Servo servo);
uint8_t getTemperatureLimit(Servo servo);
Status_Packet setTemperatureLimit(float max_temp, Servo servo);
Status_Packet setMinVoltageLimit(float min_voltage, Servo servo);
uint8_t getMinVoltageLimit(Servo servo);
Status_Packet setMaxVoltageLimit(float max_voltage, Servo servo);
uint8_t getMaxVoltageLimit(Servo servo);
float getMaxTorque(Servo servo);
uint8_t getStatusReturnLevel(Servo servo);
Status_Packet setStatusReturnLevel(uint8_t status, Servo servo);
uint8_t getAlarmLEDStatus(Servo servo);
Status_Packet setAlarmLEDStatus(uint8_t status, Servo servo);
Status_Packet setShutdownStatus(uint8_t status, Servo servo);
uint8_t getShutdownStatus(Servo servo);
uint8_t getTorqueEnableStatus(Servo servo);
Status_Packet setTorqueEnableStatus(bool enable, Servo servo);
Status_Packet setSpeed(float speed, Servo servo);
void setWheelMode(Servo servo);
float getPresentLoad(Servo servo);
float getPresentVoltage(Servo servo);
float getPresentTemperature(Servo servo);
float getSpeedRPM(Servo servo);
uint8_t getRegistered(Servo servo);
uint8_t getMoving(Servo servo);
uint8_t getLockStatus(Servo servo);
float getPunch(Servo servo);
Status_Packet setPunch(float punch, Servo servo);
#endif /* AXELFLOW_H */