Sébastien Posada Ramirez
Lib herkulex utilisée pour la carte du phare
fonctions_herkulex.h@4:8ed862a631eb, 2019-05-16 (annotated)
- Committer:
- Artiom
- Date:
- Thu May 16 10:54:05 2019 +0000
- Revision:
- 4:8ed862a631eb
- Parent:
- 3:5cc8c7dfd3b7
- Child:
- 5:3de53c9af201
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| AdrienSalle | 0:32fd5a08c430 | 1 | #include "mbed.h" |
| AdrienSalle | 0:32fd5a08c430 | 2 | |
| AdrienSalle | 0:32fd5a08c430 | 3 | //#define ID 0xFD //=253 |
| AdrienSalle | 0:32fd5a08c430 | 4 | |
| AdrienSalle | 0:32fd5a08c430 | 5 | // Herkulex ROM Register |
| AdrienSalle | 0:32fd5a08c430 | 6 | #define ROM_MODEL_NO1 0 |
| AdrienSalle | 0:32fd5a08c430 | 7 | #define ROM_MODEL_NO2 1 |
| AdrienSalle | 0:32fd5a08c430 | 8 | #define ROM_VERSION1 2 |
| AdrienSalle | 0:32fd5a08c430 | 9 | #define ROM_VERSION2 3 |
| AdrienSalle | 0:32fd5a08c430 | 10 | #define ROM_BAUD_RATE 4 |
| AdrienSalle | 0:32fd5a08c430 | 11 | #define ROM_RESERVED5 5 |
| AdrienSalle | 0:32fd5a08c430 | 12 | #define ROM_ID 6 |
| AdrienSalle | 0:32fd5a08c430 | 13 | #define ROM_ACK_POLICY 7 |
| AdrienSalle | 0:32fd5a08c430 | 14 | #define ROM_ALARM_LED_POLICY 8 |
| AdrienSalle | 0:32fd5a08c430 | 15 | #define ROM_TORQUE_POLICY 9 |
| AdrienSalle | 0:32fd5a08c430 | 16 | #define ROM_RESERVED10 10 |
| AdrienSalle | 0:32fd5a08c430 | 17 | #define ROM_MAX_TEMPERATURE 11 |
| AdrienSalle | 0:32fd5a08c430 | 18 | #define ROM_MIN_VOLTAGE 12 |
| AdrienSalle | 0:32fd5a08c430 | 19 | #define ROM_MAX_VOLTAGE 13 |
| AdrienSalle | 0:32fd5a08c430 | 20 | #define ROM_ACCELERATION_RATIO 14 |
| AdrienSalle | 0:32fd5a08c430 | 21 | #define ROM_MAX_ACCELERATION_TIME 15 |
| AdrienSalle | 0:32fd5a08c430 | 22 | #define ROM_DEAD_ZONE 16 |
| AdrienSalle | 0:32fd5a08c430 | 23 | #define ROM_SATURATOR_OFFSET 17 |
| AdrienSalle | 0:32fd5a08c430 | 24 | #define ROM_SATURATOR_SLOPE 18 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 25 | #define ROM_PWM_OFFSET 20 |
| AdrienSalle | 0:32fd5a08c430 | 26 | #define ROM_MIN_PWM 21 |
| AdrienSalle | 0:32fd5a08c430 | 27 | #define ROM_MAX_PWM 22 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 28 | #define ROM_OVERLOAD_PWM_THRESHOLD 24 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 29 | #define ROM_MIN_POSITION 26 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 30 | #define ROM_MAX_POSITION 28 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 31 | #define ROM_POSITION_KP 30 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 32 | #define ROM_POSITION_KD 32 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 33 | #define ROM_POSITION_KI 34 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 34 | #define ROM_POSITION_FEEDFORWARD_1ST_GAIN 36 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 35 | #define ROM_POSITION FEEDFORWARD_2ND_GAIN 38 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 36 | #define ROM_RESERVED40 40 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 37 | #define ROM_RESERVED42 42 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 38 | #define ROM_LED_BLINK_PERIOD 44 |
| AdrienSalle | 0:32fd5a08c430 | 39 | #define ROM_ADC_FAULT_CHECK_PERIOD 45 |
| AdrienSalle | 0:32fd5a08c430 | 40 | #define ROM_PACKET_GARBAGE_CHECK_PERIOD 46 |
| AdrienSalle | 0:32fd5a08c430 | 41 | #define ROM_STOP_DETECTION_PERIOD 47 |
| AdrienSalle | 0:32fd5a08c430 | 42 | #define ROM_OVERLOAD_DETECTION_PERIOD 48 |
| AdrienSalle | 0:32fd5a08c430 | 43 | #define ROM_STOP_THRESHOLD 49 |
| AdrienSalle | 0:32fd5a08c430 | 44 | #define ROM_INPOSITION_MARGIN 50 |
| AdrienSalle | 0:32fd5a08c430 | 45 | #define ROM_RESERVED51 51 |
| AdrienSalle | 0:32fd5a08c430 | 46 | #define ROM_RESERVED52 52 |
| AdrienSalle | 0:32fd5a08c430 | 47 | #define ROM_CALIBRATION_DIFFERENCE 53 |
| AdrienSalle | 0:32fd5a08c430 | 48 | |
| AdrienSalle | 0:32fd5a08c430 | 49 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 50 | // Herkulex RAM Register |
| AdrienSalle | 0:32fd5a08c430 | 51 | #define RAM_ID 0 |
| AdrienSalle | 0:32fd5a08c430 | 52 | #define RAM_ACK_POLICY 1 |
| AdrienSalle | 0:32fd5a08c430 | 53 | #define RAM_ALARM_LED_POLICY 2 |
| AdrienSalle | 0:32fd5a08c430 | 54 | #define RAM_TORQUE_POLICY 3 |
| AdrienSalle | 0:32fd5a08c430 | 55 | #define RAM_RESERVED4 4 |
| AdrienSalle | 0:32fd5a08c430 | 56 | #define RAM_MAX_TEMPERATURE 5 |
| AdrienSalle | 0:32fd5a08c430 | 57 | #define RAM_MIN_VOLTAGE 6 |
| AdrienSalle | 0:32fd5a08c430 | 58 | #define RAM_MAX_VOLTAGE 7 |
| AdrienSalle | 0:32fd5a08c430 | 59 | #define RAM_ACCELERATION_RATIO 8 |
| AdrienSalle | 0:32fd5a08c430 | 60 | #define RAM_MAX_ACCELERATION 9 |
| AdrienSalle | 0:32fd5a08c430 | 61 | #define RAM_DEAD_ZONE 10 |
| AdrienSalle | 0:32fd5a08c430 | 62 | #define RAM_SATURATOR_OFFSET 11 |
| AdrienSalle | 0:32fd5a08c430 | 63 | #define RAM_SATURATOR_SLOPE 12 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 64 | #define RAM_PWM_OFFSET 14 |
| AdrienSalle | 0:32fd5a08c430 | 65 | #define RAM_MIN_PWM 15 |
| AdrienSalle | 0:32fd5a08c430 | 66 | #define RAM_MAX_PWM 16 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 67 | #define RAM_OVERLOAD_PWM_THRESHOLD 18 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 68 | #define RAM_MIN_POSITION 20 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 69 | #define RAM_MAX_POSITION 22 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 70 | #define RAM_POSITION_KP 24 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 71 | #define RAM_POSITION_KD 26 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 72 | #define RAM_POSITION_KI 28 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 73 | #define RAM_POSITION_FEEDFORWARD_1ST_GAIN 30 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 74 | #define RAM_POSITION_FEEDFORWARD 2ND GAIN 32 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 75 | #define RAM_RESERVED34 34 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 76 | #define RAM_RESERVED36 36 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 77 | #define RAM_LED_BLINK_PERIOD 38 |
| AdrienSalle | 0:32fd5a08c430 | 78 | #define RAM_ADC_FAULT_DETECTION_PERIOD 39 |
| AdrienSalle | 0:32fd5a08c430 | 79 | #define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40 |
| AdrienSalle | 0:32fd5a08c430 | 80 | #define RAM_STOP_DETECTION_PERIOD 41 |
| AdrienSalle | 0:32fd5a08c430 | 81 | #define RAM_OVERLOAD_DETECTION_PERIOD 42 |
| AdrienSalle | 0:32fd5a08c430 | 82 | #define RAM_STOP_THRESHOLD 43 |
| AdrienSalle | 0:32fd5a08c430 | 83 | #define RAM_INPOSITION_MARGIN 44 |
| AdrienSalle | 0:32fd5a08c430 | 84 | #define RAM_RESERVED45 45 |
| AdrienSalle | 0:32fd5a08c430 | 85 | #define RAM_RESERVED46 46 |
| AdrienSalle | 0:32fd5a08c430 | 86 | #define RAM_CALIBRATION_DIFFERENCE 47 |
| AdrienSalle | 0:32fd5a08c430 | 87 | #define RAM_STATUS_ERROR 48 |
| AdrienSalle | 0:32fd5a08c430 | 88 | #define RAM_STATUS_DETAIL 49 |
| AdrienSalle | 0:32fd5a08c430 | 89 | #define RAM_RESERVED50 50 |
| AdrienSalle | 0:32fd5a08c430 | 90 | #define RAM_RESERVED51 51 |
| AdrienSalle | 0:32fd5a08c430 | 91 | #define RAM_TORQUE_CONTROL 52 |
| AdrienSalle | 0:32fd5a08c430 | 92 | #define RAM_LED_CONTROL 53 |
| AdrienSalle | 0:32fd5a08c430 | 93 | #define RAM_VOLTAGE 54 |
| AdrienSalle | 0:32fd5a08c430 | 94 | #define RAM_TEMPERATURE 55 |
| AdrienSalle | 0:32fd5a08c430 | 95 | #define RAM_CURRENT_CONTROL_MODE 56 |
| AdrienSalle | 0:32fd5a08c430 | 96 | #define RAM_TICK 57 |
| AdrienSalle | 0:32fd5a08c430 | 97 | #define RAM_CALIBRATED_POSITION 58 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 98 | #define RAM_ABSOLUTE_POSITION 60 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 99 | #define RAM_DIFFERENTIAL_POSITION 62 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 100 | #define RAM_PWM 64 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 101 | #define RAM_RESERVED66 66 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 102 | #define RAM_ABSOLUTE_GOAL_POSITION 68 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 103 | #define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION 70 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 104 | #define RAM_DESIRED_VELOCITY 72 // 2Byte |
| AdrienSalle | 0:32fd5a08c430 | 105 | |
| AdrienSalle | 0:32fd5a08c430 | 106 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 107 | // Request Packet [To Servo Module] |
| AdrienSalle | 0:32fd5a08c430 | 108 | #define CMD_ROM_WRITE 0x01 // Write Length number of values to EEP Register Address |
| AdrienSalle | 0:32fd5a08c430 | 109 | #define CMD_ROM_READ 0x02 // Request Length number of values from EEP Register Address |
| AdrienSalle | 0:32fd5a08c430 | 110 | #define CMD_RAM_WRITE 0x03 // Write Length number of values to RAM Register Address |
| AdrienSalle | 0:32fd5a08c430 | 111 | #define CMD_RAM_READ 0x04 // Request Lenght number of values from RAM Register Address |
| AdrienSalle | 0:32fd5a08c430 | 112 | #define CMD_I_JOG 0x05 // Able to send JOG command to maximum 43 servos (operate timing of individual Servo) |
| AdrienSalle | 0:32fd5a08c430 | 113 | #define CMD_S_JOG 0x06 // Able to send JOG command to maximum 53 servos (operate simultaneously at same time) |
| AdrienSalle | 0:32fd5a08c430 | 114 | #define CMD_STAT 0x07 // Status Error, Status Detail request |
| AdrienSalle | 0:32fd5a08c430 | 115 | #define CMD_ROLLBACK 0x08 // Change all EEP Regsters to Factory Default value |
| AdrienSalle | 0:32fd5a08c430 | 116 | #define CMD_REBOOT 0x09 // Request Reboot |
| AdrienSalle | 0:32fd5a08c430 | 117 | |
| AdrienSalle | 0:32fd5a08c430 | 118 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 119 | // ACK Packet [To Controller(ACK)] |
| AdrienSalle | 0:32fd5a08c430 | 120 | #define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40 |
| AdrienSalle | 0:32fd5a08c430 | 121 | #define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 122 | #define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 123 | #define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 124 | #define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 125 | #define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 126 | #define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 127 | #define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 128 | #define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 129 | #define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK) |
| AdrienSalle | 0:32fd5a08c430 | 130 | |
| AdrienSalle | 0:32fd5a08c430 | 131 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 132 | // Status Error |
| AdrienSalle | 0:32fd5a08c430 | 133 | #define STATUS_OK = 0x00; |
| AdrienSalle | 0:32fd5a08c430 | 134 | #define ERROR_EXCEED_INPUT_VOLTAGE = 0x01; |
| AdrienSalle | 0:32fd5a08c430 | 135 | #define ERROR_EXCEED_POT_LIMIT = 0x02; |
| AdrienSalle | 0:32fd5a08c430 | 136 | #define ERROR_EXCEED_TEMPERATURE_LIMIT = 0x04; |
| AdrienSalle | 0:32fd5a08c430 | 137 | #define ERROR_INVALID_PACKET = 0x08; |
| AdrienSalle | 0:32fd5a08c430 | 138 | #define ERROR_OVERLOAD = 0x10; |
| AdrienSalle | 0:32fd5a08c430 | 139 | #define ERROR_DRIVER_FAULT = 0x20; |
| AdrienSalle | 0:32fd5a08c430 | 140 | #define ERROR_EEP_REG_DISTORT = 0x40; |
| AdrienSalle | 0:32fd5a08c430 | 141 | |
| AdrienSalle | 0:32fd5a08c430 | 142 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 143 | // Status Detail |
| AdrienSalle | 0:32fd5a08c430 | 144 | #define MOVING_FLAG = 0x01; |
| AdrienSalle | 0:32fd5a08c430 | 145 | #define INPOSITION_FLAG = 0x02; |
| AdrienSalle | 0:32fd5a08c430 | 146 | #define CHECKSUM_ERROR = 0x04; // Invalid packet`s detailed information |
| AdrienSalle | 0:32fd5a08c430 | 147 | #define UNKNOWN_COMMAND = 0x08; // Invalid packet`s detailed information |
| AdrienSalle | 0:32fd5a08c430 | 148 | #define EXCEED_REG_RANGE = 0x10; // Invalid packet`s detailed information |
| AdrienSalle | 0:32fd5a08c430 | 149 | #define GARBAGE_DETECTED = 0x20; // Invalid packet`s detailed information |
| AdrienSalle | 0:32fd5a08c430 | 150 | #define MOTOR_ON_FLAG = 0x40; |
| AdrienSalle | 0:32fd5a08c430 | 151 | |
| AdrienSalle | 0:32fd5a08c430 | 152 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 153 | // Header |
| AdrienSalle | 0:32fd5a08c430 | 154 | #define HEADER 0xFF |
| AdrienSalle | 0:32fd5a08c430 | 155 | |
| AdrienSalle | 0:32fd5a08c430 | 156 | // Size |
| AdrienSalle | 0:32fd5a08c430 | 157 | #define MIN_PACKET_SIZE 7 |
| AdrienSalle | 0:32fd5a08c430 | 158 | #define MIN_ACK_PACKET_SIZE 9 |
| AdrienSalle | 0:32fd5a08c430 | 159 | #define WRITE_PACKET_SIZE 13 |
| AdrienSalle | 0:32fd5a08c430 | 160 | #define MAX_PACKET_SIZE 223 |
| AdrienSalle | 0:32fd5a08c430 | 161 | #define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE) |
| AdrienSalle | 0:32fd5a08c430 | 162 | |
| AdrienSalle | 0:32fd5a08c430 | 163 | // ID |
| AdrienSalle | 0:32fd5a08c430 | 164 | #define MAX_PID 0xFD |
| AdrienSalle | 0:32fd5a08c430 | 165 | #define DEFAULT_ID 0xFD |
| AdrienSalle | 0:32fd5a08c430 | 166 | #define MAX_ID 0xFD |
| AdrienSalle | 0:32fd5a08c430 | 167 | #define BROADCAST_ID 0xFE |
| AdrienSalle | 0:32fd5a08c430 | 168 | |
| AdrienSalle | 0:32fd5a08c430 | 169 | // Checksum |
| AdrienSalle | 0:32fd5a08c430 | 170 | #define CHKSUM_MASK 0xFE |
| AdrienSalle | 0:32fd5a08c430 | 171 | |
| AdrienSalle | 0:32fd5a08c430 | 172 | // Torque CMD |
| AdrienSalle | 0:32fd5a08c430 | 173 | #define TORQUE_FREE 0x00 |
| AdrienSalle | 0:32fd5a08c430 | 174 | #define BREAK_ON 0x40 |
| AdrienSalle | 0:32fd5a08c430 | 175 | #define TORQUE_ON 0x60 |
| AdrienSalle | 0:32fd5a08c430 | 176 | |
| AdrienSalle | 0:32fd5a08c430 | 177 | // Register Size |
| AdrienSalle | 0:32fd5a08c430 | 178 | #define BYTE1 1 |
| AdrienSalle | 0:32fd5a08c430 | 179 | #define BYTE2 2 |
| AdrienSalle | 0:32fd5a08c430 | 180 | |
| AdrienSalle | 0:32fd5a08c430 | 181 | // Jog Set CMD |
| AdrienSalle | 0:32fd5a08c430 | 182 | #define STOP 0x01 |
| AdrienSalle | 0:32fd5a08c430 | 183 | #define POS_MODE 0x00 |
| AdrienSalle | 0:32fd5a08c430 | 184 | #define TURN_MODE 0x02 |
| AdrienSalle | 0:32fd5a08c430 | 185 | #define GLED_ON 0x04 |
| AdrienSalle | 0:32fd5a08c430 | 186 | #define BLED_ON 0x08 |
| AdrienSalle | 0:32fd5a08c430 | 187 | #define RLED_ON 0x10 |
| AdrienSalle | 0:32fd5a08c430 | 188 | |
| AdrienSalle | 0:32fd5a08c430 | 189 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 190 | //------------------------------------------a choisir--------------------------- |
| AdrienSalle | 0:32fd5a08c430 | 191 | #define nombre_servomoteur 3 // |
| AdrienSalle | 0:32fd5a08c430 | 192 | //-----------------------servo petit robot-------------------------------------- |
| Artiom | 1:94a29f20ca18 | 193 | //avant |
| Artiom | 1:94a29f20ca18 | 194 | |
| Artiom | 1:94a29f20ca18 | 195 | #define AV_EP_D 11 |
| Artiom | 1:94a29f20ca18 | 196 | #define AV_poigne_D 13 |
| Artiom | 1:94a29f20ca18 | 197 | |
| Artiom | 1:94a29f20ca18 | 198 | #define AV_EP_C 15 |
| Artiom | 1:94a29f20ca18 | 199 | #define AV_poigne_C 16 |
| Artiom | 1:94a29f20ca18 | 200 | |
| Artiom | 1:94a29f20ca18 | 201 | #define AV_EP_G 12 |
| Artiom | 1:94a29f20ca18 | 202 | #define AV_poigne_G 14 |
| Artiom | 2:49913cb93c64 | 203 | #define AV_sol 20 |
| AdrienSalle | 0:32fd5a08c430 | 204 | |
| Artiom | 1:94a29f20ca18 | 205 | //arriere |
| Artiom | 1:94a29f20ca18 | 206 | |
| Artiom | 1:94a29f20ca18 | 207 | #define AR_EP_D 21 |
| Artiom | 1:94a29f20ca18 | 208 | #define AR_poigne_D 23 |
| AdrienSalle | 0:32fd5a08c430 | 209 | |
| Artiom | 1:94a29f20ca18 | 210 | #define AR_EP_C 25 |
| Artiom | 1:94a29f20ca18 | 211 | #define AR_poigne_C 26 |
| Artiom | 1:94a29f20ca18 | 212 | |
| Artiom | 1:94a29f20ca18 | 213 | #define AR_EP_G 22 |
| Artiom | 1:94a29f20ca18 | 214 | #define AR_poigne_G 24 |
| Artiom | 1:94a29f20ca18 | 215 | |
| Artiom | 2:49913cb93c64 | 216 | #define AR_sol 10 |
| Artiom | 1:94a29f20ca18 | 217 | |
| Artiom | 4:8ed862a631eb | 218 | //-----------------------servo gros robot-------------------------------------- |
| Artiom | 4:8ed862a631eb | 219 | //serial 1 |
| Artiom | 4:8ed862a631eb | 220 | #define roue_G 4 |
| Artiom | 4:8ed862a631eb | 221 | #define stockage_G 3 |
| Artiom | 4:8ed862a631eb | 222 | //serial 2 |
| Artiom | 4:8ed862a631eb | 223 | #define doigt 2 |
| Artiom | 4:8ed862a631eb | 224 | //serial 3 |
| Artiom | 4:8ed862a631eb | 225 | #define stockage_D 1 |
| Artiom | 4:8ed862a631eb | 226 | #define roue_D 5 |
| Artiom | 4:8ed862a631eb | 227 | |
| AdrienSalle | 0:32fd5a08c430 | 228 | |
| AdrienSalle | 0:32fd5a08c430 | 229 | //------------------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 230 | |
| AdrienSalle | 0:32fd5a08c430 | 231 | /** Create an Herkulex servo object connected to the serial pins and baudrate |
| AdrienSalle | 0:32fd5a08c430 | 232 | * |
| AdrienSalle | 0:32fd5a08c430 | 233 | * @param tx Transmit pin. |
| AdrienSalle | 0:32fd5a08c430 | 234 | * @param rx Receive pin. |
| AdrienSalle | 0:32fd5a08c430 | 235 | * @param baudRate The serial tx/rx speed. |
| AdrienSalle | 0:32fd5a08c430 | 236 | */ |
| AdrienSalle | 0:32fd5a08c430 | 237 | // void Herkulex(PinName tx, PinName rx, uint32_t baudRate); |
| AdrienSalle | 0:32fd5a08c430 | 238 | |
| AdrienSalle | 0:32fd5a08c430 | 239 | /** Destroy an Herkulex servo object |
| AdrienSalle | 0:32fd5a08c430 | 240 | */ |
| AdrienSalle | 0:32fd5a08c430 | 241 | //void N_Herkulex(); |
| AdrienSalle | 0:32fd5a08c430 | 242 | |
| AdrienSalle | 0:32fd5a08c430 | 243 | /** Transmit packet datas |
| AdrienSalle | 0:32fd5a08c430 | 244 | * |
| AdrienSalle | 0:32fd5a08c430 | 245 | * @param packetSize The packet size. |
| AdrienSalle | 0:32fd5a08c430 | 246 | * @param data The transmit packet data array. |
| AdrienSalle | 0:32fd5a08c430 | 247 | */ |
| AdrienSalle | 0:32fd5a08c430 | 248 | void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 249 | |
| AdrienSalle | 0:32fd5a08c430 | 250 | /** Receive packet datas |
| AdrienSalle | 0:32fd5a08c430 | 251 | * |
| AdrienSalle | 0:32fd5a08c430 | 252 | * @param packetSize The packet size. |
| AdrienSalle | 0:32fd5a08c430 | 253 | * @param data The receive packet data array. |
| AdrienSalle | 0:32fd5a08c430 | 254 | */ |
| AdrienSalle | 0:32fd5a08c430 | 255 | void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 256 | |
| AdrienSalle | 0:32fd5a08c430 | 257 | /** Clear error status |
| AdrienSalle | 0:32fd5a08c430 | 258 | * |
| AdrienSalle | 0:32fd5a08c430 | 259 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 260 | */ |
| AdrienSalle | 0:32fd5a08c430 | 261 | void clear(uint8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 262 | |
| AdrienSalle | 0:32fd5a08c430 | 263 | /** Set torque setting |
| AdrienSalle | 0:32fd5a08c430 | 264 | * |
| AdrienSalle | 0:32fd5a08c430 | 265 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 266 | * @param cmdTorque The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60) |
| AdrienSalle | 0:32fd5a08c430 | 267 | */ |
| AdrienSalle | 0:32fd5a08c430 | 268 | void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 269 | //------------------------------Déplacements------------------------------------- |
| AdrienSalle | 0:32fd5a08c430 | 270 | /** Position Control |
| AdrienSalle | 0:32fd5a08c430 | 271 | * |
| AdrienSalle | 0:32fd5a08c430 | 272 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 273 | * @param position The goal position of herkulex servo. |
| AdrienSalle | 0:32fd5a08c430 | 274 | * @param playtime Time to target position. |
| AdrienSalle | 0:32fd5a08c430 | 275 | * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) |
| AdrienSalle | 0:32fd5a08c430 | 276 | */ |
| AdrienSalle | 0:32fd5a08c430 | 277 | void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 278 | |
| AdrienSalle | 0:32fd5a08c430 | 279 | /** Velocity Control |
| AdrienSalle | 0:32fd5a08c430 | 280 | * |
| AdrienSalle | 0:32fd5a08c430 | 281 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 282 | * @param speed The goal position of herkulex servo. |
| AdrienSalle | 0:32fd5a08c430 | 283 | * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) |
| AdrienSalle | 0:32fd5a08c430 | 284 | */ |
| AdrienSalle | 0:32fd5a08c430 | 285 | void velocityControl(uint8_t id, int16_t speed,uint8_t setLED, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 286 | /** |
| AdrienSalle | 0:32fd5a08c430 | 287 | * |
| AdrienSalle | 0:32fd5a08c430 | 288 | *même paramètres que positionControl(); |
| AdrienSalle | 0:32fd5a08c430 | 289 | *permet de donner des instructions à deux servomoteurs simultanéments |
| AdrienSalle | 0:32fd5a08c430 | 290 | */ |
| AdrienSalle | 0:32fd5a08c430 | 291 | void positionControl_Mul_ensemble(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t setLED2, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 292 | /** |
| AdrienSalle | 0:32fd5a08c430 | 293 | * |
| AdrienSalle | 0:32fd5a08c430 | 294 | *même paramètres que positionControl_Mul_ensemble(); |
| AdrienSalle | 0:32fd5a08c430 | 295 | *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent |
| AdrienSalle | 0:32fd5a08c430 | 296 | */ |
| AdrienSalle | 0:32fd5a08c430 | 297 | void positionControl_Mul_playtime_different(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t playtime2, uint8_t setLED2, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 298 | /** |
| AdrienSalle | 0:32fd5a08c430 | 299 | * |
| AdrienSalle | 0:32fd5a08c430 | 300 | * |
| AdrienSalle | 0:32fd5a08c430 | 301 | *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent |
| AdrienSalle | 0:32fd5a08c430 | 302 | */ |
| AdrienSalle | 0:32fd5a08c430 | 303 | void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos, uint8_t numero_serial); // uint16_t position, uint8_t setLED, uint8_t id |
| AdrienSalle | 0:32fd5a08c430 | 304 | /** |
| AdrienSalle | 0:32fd5a08c430 | 305 | * |
| AdrienSalle | 0:32fd5a08c430 | 306 | *même paramètres que positionControl_Mul_ensemble(); |
| AdrienSalle | 0:32fd5a08c430 | 307 | *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent |
| AdrienSalle | 0:32fd5a08c430 | 308 | */ |
| AdrienSalle | 0:32fd5a08c430 | 309 | void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos, uint8_t numero_serial); // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime |
| AdrienSalle | 0:32fd5a08c430 | 310 | |
| AdrienSalle | 0:32fd5a08c430 | 311 | |
| AdrienSalle | 0:32fd5a08c430 | 312 | //-------------------------------------Demandes d'informations |
| AdrienSalle | 0:32fd5a08c430 | 313 | /** Get Status |
| AdrienSalle | 0:32fd5a08c430 | 314 | * |
| AdrienSalle | 0:32fd5a08c430 | 315 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 316 | * @return -1 is getStatus failed. other is servo`s status error value. |
| AdrienSalle | 0:32fd5a08c430 | 317 | */ |
| AdrienSalle | 0:32fd5a08c430 | 318 | int8_t getStatus(uint8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 319 | |
| AdrienSalle | 0:32fd5a08c430 | 320 | /** Get Position |
| AdrienSalle | 0:32fd5a08c430 | 321 | * |
| AdrienSalle | 0:32fd5a08c430 | 322 | * @param id The herkulex servo ID. |
| AdrienSalle | 0:32fd5a08c430 | 323 | * @return -1 is getPos failed. other is servo's current position. |
| AdrienSalle | 0:32fd5a08c430 | 324 | */ |
| AdrienSalle | 0:32fd5a08c430 | 325 | int16_t getPos(uint8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 326 | |
| AdrienSalle | 0:32fd5a08c430 | 327 | //void recevoir_irq(void); |
| AdrienSalle | 0:32fd5a08c430 | 328 | |
| AdrienSalle | 0:32fd5a08c430 | 329 | void receive_Sv(void); |
| AdrienSalle | 0:32fd5a08c430 | 330 | |
| AdrienSalle | 0:32fd5a08c430 | 331 | int8_t Get_Torque(int8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 332 | |
| AdrienSalle | 0:32fd5a08c430 | 333 | int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 334 | |
| AdrienSalle | 0:32fd5a08c430 | 335 | int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 336 | |
| AdrienSalle | 0:32fd5a08c430 | 337 | void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 338 | |
| AdrienSalle | 0:32fd5a08c430 | 339 | |
| AdrienSalle | 0:32fd5a08c430 | 340 | int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial); |
| AdrienSalle | 0:32fd5a08c430 | 341 | |
| AdrienSalle | 0:32fd5a08c430 | 342 | //--------------------------------------------Fonctions d'interruptions et de vérifications------------------------------------------------------------------ |
| AdrienSalle | 0:32fd5a08c430 | 343 | |
| AdrienSalle | 0:32fd5a08c430 | 344 | void servo_interrupt_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 345 | |
| AdrienSalle | 0:32fd5a08c430 | 346 | void Interrupt1_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 347 | |
| AdrienSalle | 0:32fd5a08c430 | 348 | void Interrupt2_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 349 | |
| AdrienSalle | 0:32fd5a08c430 | 350 | void Interrupt3_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 351 | |
| AdrienSalle | 0:32fd5a08c430 | 352 | void Interrupt4_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 353 | |
| AdrienSalle | 0:32fd5a08c430 | 354 | void Interrupt5_en(void); |
| AdrienSalle | 0:32fd5a08c430 | 355 | |
| AdrienSalle | 0:32fd5a08c430 | 356 | void automate_serial1(void); |
| AdrienSalle | 0:32fd5a08c430 | 357 | |
| AdrienSalle | 0:32fd5a08c430 | 358 | void automate_serial2(void); |
| AdrienSalle | 0:32fd5a08c430 | 359 | |
| AdrienSalle | 0:32fd5a08c430 | 360 | void automate_serial3(void); |
| AdrienSalle | 0:32fd5a08c430 | 361 | |
| AdrienSalle | 0:32fd5a08c430 | 362 | void automate_serial4(void); |
| AdrienSalle | 0:32fd5a08c430 | 363 | |
| AdrienSalle | 0:32fd5a08c430 | 364 | void automate_serial5(void); |
| AdrienSalle | 0:32fd5a08c430 | 365 | |
| AdrienSalle | 0:32fd5a08c430 | 366 | void Pompe_init(PinName Pompe_pin); |
| AdrienSalle | 0:32fd5a08c430 | 367 | |
| AdrienSalle | 0:32fd5a08c430 | 368 | void Pompe_essai(PinName Pompe_pin); |
| AdrienSalle | 0:32fd5a08c430 | 369 | |
| AdrienSalle | 0:32fd5a08c430 | 370 | //----------------Définition des ports série en variables externes--------------------------- |
| AdrienSalle | 0:32fd5a08c430 | 371 | /* |
| AdrienSalle | 0:32fd5a08c430 | 372 | * permet d'utiliser les ports série dans le main.ci |
| AdrienSalle | 0:32fd5a08c430 | 373 | */ |
| AdrienSalle | 0:32fd5a08c430 | 374 | extern RawSerial pc; |
| AdrienSalle | 0:32fd5a08c430 | 375 | extern RawSerial serial5; |
| AdrienSalle | 0:32fd5a08c430 | 376 | extern RawSerial serial4; |
| AdrienSalle | 0:32fd5a08c430 | 377 | extern RawSerial serial3; |
| AdrienSalle | 0:32fd5a08c430 | 378 | extern RawSerial serial2; |
| AdrienSalle | 0:32fd5a08c430 | 379 | extern RawSerial serial1; |
| AdrienSalle | 0:32fd5a08c430 | 380 | //extern RawSerial serial1; |
| AdrienSalle | 0:32fd5a08c430 | 381 | //#define Serial1 serial4 |
| AdrienSalle | 0:32fd5a08c430 | 382 | |
| AdrienSalle | 0:32fd5a08c430 | 383 | //---------------Fonctions gros robot------------------------------------------- |
| AdrienSalle | 0:32fd5a08c430 | 384 | //-------------------------------------Utiliser la roue de sortie--------------- |
| AdrienSalle | 0:32fd5a08c430 | 385 | /** Get Status |
| AdrienSalle | 0:32fd5a08c430 | 386 | * |
| AdrienSalle | 0:32fd5a08c430 | 387 | * @ tour ==> nombre de fois où la roue descend |
| AdrienSalle | 0:32fd5a08c430 | 388 | */ |
| Artiom | 1:94a29f20ca18 | 389 | //void roueSortie(tour); |
| Artiom | 1:94a29f20ca18 | 390 | |
| Artiom | 1:94a29f20ca18 | 391 | void deverouillage_torque (void); |
| Artiom | 3:5cc8c7dfd3b7 | 392 | void deverouillage_torque_avant (void); |
| Artiom | 3:5cc8c7dfd3b7 | 393 | void deverouillage_torque_arriere (void); |
| Artiom | 3:5cc8c7dfd3b7 | 394 | void deverouillage_torque_sol (void); |