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 | //HSR Serial Communication
//Commands and Memory Locations
#include<project.h>
//Commands
#define _HSR_ComStart 0x80 //Communication start byte
#define _HSR_ReadEPPROM 0xE1 // Read a specific location from EEPROM
#define _HSR_WriteEPPROM 0xE2 // Write a specific location from EEPROM
#define _HSR_ReadMemory 0xE3 // Read a specific location from memory
#define _HSR_WriteMemory 0xE4 // Write a specific location from memory
#define _HSR_ReadPosition 0xE5 // Read current servo position
#define _HSR_SetPosition 0xE6 // Set target position
#define _HSR_ReadVerID 0xE7 // Read servo version and ID
#define _HSR_ReadPWM 0xE8 // Read pulsewidth and Voltage
#define _HSR_SetSpeed 0xE9 // Set servo speed and read position
#define _HSR_SelectControl 0xEA // Select control parameter set
#define _HSR_SetGoStop 0xEB // Set go/stop
#define _HSR_Release 0xEF // Release
//Servo Related Variables
uint8 _ServoID; // Servo ID
uint8 _ServoVersion; // Servo version
uint8 _ServoPosH; // Servo position high
uint8 _ServoPosL; // Servo position low
/*uint8 _ServoMinPosH; // Minimum servo position - High Byte
uint8 _ServoMinPosL; // Minimum servo position - Low Byte
uint16 _ServoMinPos = 550; // Minimum Servo Position
uint8 _ServoMaxPosH; // Maximum servo position - High Byte
uint8 _ServoMaxPosL; // Maximum servo position - Low Byte
uint16 _ServoMaxPos = 2350; // Maximum Servo Position
uint8 _ServoMidPosH; // Center servo position - High Byte
uint8 _ServoMidPosL; // Center servo position - Low Byte
uint16 _ServoMidPos = 1500; // Center Servo Position
uint8 _ServoSpeed; // Servo speed
uint8 _temp=0; // */
char _RxData[3]; // Incoming data from servo
char _RxLastData[2]; // Last Received Complete Data
uint8 _RxCounter=0; // Counter for incoming data
short _RxComplete=0; // Incomming data is ready
uint8 _CheckSum; // It is used for error checking
uint8 _ServoPW; // Pulsewidth of PWM signal
uint8 _ServoVoltage; // Voltage of PWM signal
uint16 _Start_time;
uint16 _Reset_interval= URB_HEARTBEAT_INTERVAL/60;
//Important EPPROM Locations
#define _HSR_EPPROM_P1H 0x00 // High byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_P1L 0x01 // Low byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_DZ1 0x02 // Dead zone for parameter set 1
#define _HSR_EPPROM_P1D 0x03 // Derivative gain for parameter set 1
#define _HSR_EPPROM_P2H 0x1F // High byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_P2L 0x20 // Low byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_DZ2 0x21 // Dead zone for parameter set 1
#define _HSR_EPPROM_P2D 0x22 // Derivative gain for parameter set 1
#define _HSR_EPPROM_P3H 0x24 // High byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_P3L 0x25 // Low byte of Proportional gain for parameter set 1
#define _HSR_EPPROM_DZ3 0x26 // Dead zone for parameter set 1
#define _HSR_EPPROM_P3D 0x27 // Derivative gain for parameter set 1
#define _HSR_EPPROM_Speed 0x06 // EEPROM address for speed
#define _HSR_EPPROM_ID 0x29 // Servo ID
#define _HSR_EPPROM_MinPosH 0x09 // High byte of minimum servo position
#define _HSR_EPPROM_MinPosL 0x0A // Low byte of minimum servo positon
#define _HSR_EPPROM_MaxPosH 0x0B // High byte of maximum servo position
#define _HSR_EPPROM_MaxPosL 0x0C // Low byte of maximum servo positon
#define _HSR_EPPROM_MidPosH 0x11 // High byte of center servo position
#define _HSR_EPPROM_MidPosL 0x12 // Low byte of center servo positon
//Important Memory Locations
//Some of them are loaded from EPPROM at power-on
#define _HSR_MEM_CurTargetPosL 0x06 // Low byte of current target set position
#define _HSR_MEM_CurTargetPosH 0x07 // High byte of current target set position
#define _HSR_MEM_TargetPosH 0xA5 // High byte of target position
#define _HSR_MEM_TargetPosL 0xA6 // Low byte of target position
#define _HSR_MEM_ActualPosH 0xA7 // High byte of actual position
#define _HSR_MEM_ActualPosL 0xA8 // Low byte of actual position
#define _HSR_MEM_PosErrorH 0xA9 // High byte of position error (Target Position - Actual Position)
#define _HSR_MEM_PosErrorL 0xAA // Low byte of position error (Target Position - Actual Position)
#define _HSR_MEM_GoStop 0xC9 // Go/Stop
#define _HSR_MEM_P1H 0x80 // High byte of Proportional gain for parameter set 1
#define _HSR_MEM_P1L 0x81 // Low byte of Proportional gain for parameter set 1
#define _HSR_MEM_DZ1 0x82 // Dead zone for parameter set 1
#define _HSR_MEM_P1D 0x83 // Derivative gain for parameter set 1
#define _HSR_MEM_Speed 0x86 // Memory address for speed
#define _HSR_MEM_CurSpeed 0xC3 // Current Speed setting and sometimes updated from HSR_MEM_Speed
#define _HSR_MEM_MinPosH 0x89 // High byte of minimum servo position
#define _HSR_MEM_MinPosL 0x8A // Low byte of minimum servo positon
#define _HSR_MEM_MaxPosH 0x8B // High byte of maximum servo position
#define _HSR_MEM_MaxPosL 0x8C // Low byte of maximum servo positon
#define _HSR_MEM_MidPosH 0x91 // High byte of center servo position
#define _HSR_MEM_MidPosL 0x92 // Low byte of center servo positon
// Function Prototypes
void HSR_SerialInitialization(void); // Initial Serial Configuration for HSR communication
void HSR_Release(void); // Release Servo
void HSR_Release_v2(void); // Release Servo
uint16 HSR_ReadPosition(void); // Read Servo Position
void HSR_ReadPosition_v2(void); // Read Servo Position
short IsRxDataReady(void); // Is incoming data ready
void WaitRxDataReady(void); // Wait until incoming data is ready.
uint8 HSR_ReadEPPROM(uint8 addr); //Read a Specific EPPROM location
void HSR_ReadEPPROM_v2(uint8 addr); //Read a Specific EPPROM location
void HSR_WriteEPPROM(uint8 addr, uint8 data); //Write one byte data to the EPPROM
void HSR_WriteEPPROM_v2(uint8 addr, uint8 data); //Write one byte data to the EPPROM
uint8 HSR_ReadMemory(uint8 addr); //Read a specific memory location
void HSR_ReadMemory_v2(uint8 addr); //Read a specific memory location
void HSR_WriteMemory(uint8 addr, uint8 data); //Write Memory
void HSR_WriteMemory_v2(uint8 addr, uint8 data); //Write Memory
void HSR_SetPosition(uint8 PosH, uint8 PosL); //Set target position
void HSR_SetPosition(uint8 ServoID, uint8 PosH, uint8 PosL); // Set target position of a specific servo. Overloaded version
void HSR_SetInitialPosition(uint8 PosH, uint8 PosL); // Smooth Startup
void HSR_ReadVerID(void); // Read servo version and id
void HSR_ReadVerID_v2(void); // Read servo version and id
void HSR_ReadPWM(void); // Read pulsewidth and voltage
void HSR_ReadPWM_v2(void); // Read pulsewidth and voltage
void HSR_SetSpeed(uint8 ServoID, uint8 ServoSpeed);// Set the peed of a single servo using its ID and read its current position
void HSR_SetSpeed_v2(uint8 ServoID, uint8 ServoSpeed);// Set the peed of a single servo using its ID and read its current position
void HSR_SelectControl(uint8 ControlSet); // Select control parameter set
void HSR_SelectControl_v2(uint8 ControlSet); // Select control parameter set
void HSR_SetGoStop(uint8 GoStop); // Set Go/Stop
void HSR_SetGoStop_v2(uint8 GoStop); // Set Go/Stop
void HSR_SetAction(sint16 TargetAngle, uint16 ActionTime); //Set Action
void HSR_SetAction_v2(sint16 TargetAngle, uint16 ActionTime); //Set Action
uint32 HSR_SetAction_v3(sint16 TargetAngle, uint16 ActionTime);//Set Action and estimaate time of motion
|