Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of mbed_tes_cpt by
AX12/AX12.cpp@5:7e1c328c5d50, 2017-05-25 (annotated)
- Committer:
- matthieuvignon
- Date:
- Thu May 25 06:35:22 2017 +0000
- Revision:
- 5:7e1c328c5d50
- Parent:
- 3:43843ab8af47
Version Gatien Nuit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 1:f3f702086a30 | 1 | /* mbed AX-12+ Servo Library |
ClementBreteau | 1:f3f702086a30 | 2 | * |
ClementBreteau | 1:f3f702086a30 | 3 | * Copyright (c) 2010, cstyles (http://mbed.org) |
ClementBreteau | 1:f3f702086a30 | 4 | * |
ClementBreteau | 1:f3f702086a30 | 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
ClementBreteau | 1:f3f702086a30 | 6 | * of this software and associated documentation files (the "Software"), to deal |
ClementBreteau | 1:f3f702086a30 | 7 | * in the Software without restriction, including without limitation the rights |
ClementBreteau | 1:f3f702086a30 | 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
ClementBreteau | 1:f3f702086a30 | 9 | * copies of the Software, and to permit persons to whom the Software is |
ClementBreteau | 1:f3f702086a30 | 10 | * furnished to do so, subject to the following conditions: |
ClementBreteau | 1:f3f702086a30 | 11 | * |
ClementBreteau | 1:f3f702086a30 | 12 | * The above copyright notice and this permission notice shall be included in |
ClementBreteau | 1:f3f702086a30 | 13 | * all copies or substantial portions of the Software. |
ClementBreteau | 1:f3f702086a30 | 14 | * |
ClementBreteau | 1:f3f702086a30 | 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
ClementBreteau | 1:f3f702086a30 | 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
ClementBreteau | 1:f3f702086a30 | 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
ClementBreteau | 1:f3f702086a30 | 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
ClementBreteau | 1:f3f702086a30 | 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
ClementBreteau | 1:f3f702086a30 | 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
ClementBreteau | 1:f3f702086a30 | 21 | * THE SOFTWARE. |
ClementBreteau | 1:f3f702086a30 | 22 | */ |
ClementBreteau | 1:f3f702086a30 | 23 | |
ClementBreteau | 1:f3f702086a30 | 24 | #include "mbed.h" |
ClementBreteau | 1:f3f702086a30 | 25 | #include "AX12.h" |
ClementBreteau | 1:f3f702086a30 | 26 | |
ClementBreteau | 1:f3f702086a30 | 27 | #define MAX_TIMEOUT 500 |
ClementBreteau | 1:f3f702086a30 | 28 | |
ClementBreteau | 1:f3f702086a30 | 29 | extern Timer t; |
ClementBreteau | 1:f3f702086a30 | 30 | |
ClementBreteau | 1:f3f702086a30 | 31 | typedef struct |
ClementBreteau | 1:f3f702086a30 | 32 | { |
ClementBreteau | 1:f3f702086a30 | 33 | unsigned short Model_Number; |
ClementBreteau | 1:f3f702086a30 | 34 | unsigned char Firmware; |
ClementBreteau | 1:f3f702086a30 | 35 | unsigned char ID; |
ClementBreteau | 1:f3f702086a30 | 36 | unsigned char Baud_Rate; |
ClementBreteau | 1:f3f702086a30 | 37 | unsigned char Return_Delay_Time; |
ClementBreteau | 1:f3f702086a30 | 38 | unsigned short CW_Angle_Limit; |
ClementBreteau | 1:f3f702086a30 | 39 | unsigned short CCW_Angle_Limit; |
ClementBreteau | 1:f3f702086a30 | 40 | unsigned char Reserved1; |
ClementBreteau | 1:f3f702086a30 | 41 | unsigned char Highest_Limit_Temperature; |
ClementBreteau | 1:f3f702086a30 | 42 | unsigned char Lowest_Limit_voltage; |
ClementBreteau | 1:f3f702086a30 | 43 | unsigned char Highest_Limit_voltage; |
ClementBreteau | 1:f3f702086a30 | 44 | unsigned short Max_Torque; |
ClementBreteau | 1:f3f702086a30 | 45 | unsigned char Status_Return_Level; |
ClementBreteau | 1:f3f702086a30 | 46 | unsigned char Alarm_LED; |
ClementBreteau | 1:f3f702086a30 | 47 | unsigned char Alarm_Shutdown; |
ClementBreteau | 1:f3f702086a30 | 48 | unsigned char Reserved2; |
ClementBreteau | 1:f3f702086a30 | 49 | unsigned short Down_Calibration; |
ClementBreteau | 1:f3f702086a30 | 50 | unsigned short Up_Calibration; |
ClementBreteau | 1:f3f702086a30 | 51 | unsigned char Torque_Enable; |
ClementBreteau | 1:f3f702086a30 | 52 | unsigned char LED; |
ClementBreteau | 1:f3f702086a30 | 53 | unsigned char CW_Compliance_Margin; |
ClementBreteau | 1:f3f702086a30 | 54 | unsigned char CCW_Compliance_Margin; |
ClementBreteau | 1:f3f702086a30 | 55 | unsigned char CW_Compliance_Slope; |
ClementBreteau | 1:f3f702086a30 | 56 | unsigned char CCW_Compliance_Slope; |
ClementBreteau | 1:f3f702086a30 | 57 | unsigned short Goal_Position; |
ClementBreteau | 1:f3f702086a30 | 58 | unsigned short Moving_Speed; |
ClementBreteau | 1:f3f702086a30 | 59 | unsigned short Torque_Limit; |
ClementBreteau | 1:f3f702086a30 | 60 | unsigned short Present_Position; |
ClementBreteau | 1:f3f702086a30 | 61 | unsigned short Present_Speed; |
ClementBreteau | 1:f3f702086a30 | 62 | unsigned short Present_Load; |
ClementBreteau | 1:f3f702086a30 | 63 | unsigned char Present_Voltage; |
ClementBreteau | 1:f3f702086a30 | 64 | unsigned char Present_Temperature; |
ClementBreteau | 1:f3f702086a30 | 65 | unsigned char Registered_Instruction; |
ClementBreteau | 1:f3f702086a30 | 66 | unsigned char Reserved3; |
ClementBreteau | 1:f3f702086a30 | 67 | unsigned char Moving; |
ClementBreteau | 1:f3f702086a30 | 68 | unsigned char Lock; |
ClementBreteau | 1:f3f702086a30 | 69 | unsigned short Punch; |
ClementBreteau | 1:f3f702086a30 | 70 | } T_AX12; |
ClementBreteau | 1:f3f702086a30 | 71 | |
ClementBreteau | 1:f3f702086a30 | 72 | |
ClementBreteau | 1:f3f702086a30 | 73 | AX12::AX12(PinName tx, PinName rx, int ID, int baud) |
ClementBreteau | 1:f3f702086a30 | 74 | : _ax12(tx,rx) |
ClementBreteau | 1:f3f702086a30 | 75 | { |
ClementBreteau | 1:f3f702086a30 | 76 | _baud = baud; |
ClementBreteau | 1:f3f702086a30 | 77 | _ID = ID; |
ClementBreteau | 1:f3f702086a30 | 78 | _ax12.baud(_baud); |
ClementBreteau | 1:f3f702086a30 | 79 | |
ClementBreteau | 1:f3f702086a30 | 80 | |
ClementBreteau | 1:f3f702086a30 | 81 | } |
ClementBreteau | 1:f3f702086a30 | 82 | |
ClementBreteau | 1:f3f702086a30 | 83 | int AX12::Set_Secure_Goal(int degres) |
ClementBreteau | 1:f3f702086a30 | 84 | { |
ClementBreteau | 1:f3f702086a30 | 85 | int error = 0; |
ClementBreteau | 1:f3f702086a30 | 86 | int position = 0; |
ClementBreteau | 1:f3f702086a30 | 87 | int difference = 0; |
ClementBreteau | 1:f3f702086a30 | 88 | int timeout_secure = 0; |
ClementBreteau | 1:f3f702086a30 | 89 | int autorisation = 0; |
ClementBreteau | 1:f3f702086a30 | 90 | |
ClementBreteau | 1:f3f702086a30 | 91 | position = Get_Position(); |
ClementBreteau | 1:f3f702086a30 | 92 | error = Set_Goal(degres); |
ClementBreteau | 1:f3f702086a30 | 93 | |
ClementBreteau | 1:f3f702086a30 | 94 | while ((autorisation == 0) && (timeout_secure < 100) ) |
ClementBreteau | 1:f3f702086a30 | 95 | { |
ClementBreteau | 1:f3f702086a30 | 96 | position = Get_Position(); |
ClementBreteau | 1:f3f702086a30 | 97 | //printf("position : %d", position ); |
ClementBreteau | 1:f3f702086a30 | 98 | error = Set_Goal(degres); |
ClementBreteau | 1:f3f702086a30 | 99 | //printf("degres : %d", degres); |
ClementBreteau | 1:f3f702086a30 | 100 | difference = degres - position; |
ClementBreteau | 1:f3f702086a30 | 101 | //printf ("difference : %d", difference ); |
ClementBreteau | 1:f3f702086a30 | 102 | if (((difference < 2) && (difference > (-2) ))) |
ClementBreteau | 1:f3f702086a30 | 103 | autorisation = 1; |
ClementBreteau | 1:f3f702086a30 | 104 | |
ClementBreteau | 1:f3f702086a30 | 105 | timeout_secure++; |
ClementBreteau | 1:f3f702086a30 | 106 | } |
ClementBreteau | 1:f3f702086a30 | 107 | |
ClementBreteau | 1:f3f702086a30 | 108 | if ( timeout_secure == 100) |
ClementBreteau | 1:f3f702086a30 | 109 | { |
ClementBreteau | 1:f3f702086a30 | 110 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 111 | printf (" timeout secure error "); |
ClementBreteau | 1:f3f702086a30 | 112 | #endif |
ClementBreteau | 1:f3f702086a30 | 113 | return(-1); |
ClementBreteau | 1:f3f702086a30 | 114 | } |
ClementBreteau | 1:f3f702086a30 | 115 | return(error); |
ClementBreteau | 1:f3f702086a30 | 116 | } |
ClementBreteau | 1:f3f702086a30 | 117 | |
ClementBreteau | 1:f3f702086a30 | 118 | |
ClementBreteau | 1:f3f702086a30 | 119 | int AX12::Get_Return_Delay_Time(void) |
ClementBreteau | 1:f3f702086a30 | 120 | { |
ClementBreteau | 1:f3f702086a30 | 121 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 122 | int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data); |
ClementBreteau | 1:f3f702086a30 | 123 | int time = data[0]; |
ClementBreteau | 1:f3f702086a30 | 124 | time = 2.0 * time; |
ClementBreteau | 1:f3f702086a30 | 125 | return(time); |
ClementBreteau | 1:f3f702086a30 | 126 | } |
ClementBreteau | 1:f3f702086a30 | 127 | |
ClementBreteau | 1:f3f702086a30 | 128 | |
ClementBreteau | 1:f3f702086a30 | 129 | int AX12::Get_Baud_Rate(void) |
ClementBreteau | 1:f3f702086a30 | 130 | { |
ClementBreteau | 1:f3f702086a30 | 131 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 132 | int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data); |
ClementBreteau | 1:f3f702086a30 | 133 | int baud = data[0]; |
ClementBreteau | 1:f3f702086a30 | 134 | baud = 2000000 / ( baud + 1 ); |
ClementBreteau | 1:f3f702086a30 | 135 | return(baud); |
ClementBreteau | 1:f3f702086a30 | 136 | } |
ClementBreteau | 1:f3f702086a30 | 137 | |
ClementBreteau | 1:f3f702086a30 | 138 | |
ClementBreteau | 1:f3f702086a30 | 139 | /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur |
ClementBreteau | 1:f3f702086a30 | 140 | // minimum >> Ox000 >> decimal 0 |
ClementBreteau | 1:f3f702086a30 | 141 | // maximum >> 0x3FF >> decimal 1023 |
ClementBreteau | 1:f3f702086a30 | 142 | // deflaut >> 0x20 >> decimal 32 |
ClementBreteau | 1:f3f702086a30 | 143 | */ |
ClementBreteau | 1:f3f702086a30 | 144 | int AX12::Set_Punch(int punch) |
ClementBreteau | 1:f3f702086a30 | 145 | { |
ClementBreteau | 1:f3f702086a30 | 146 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 147 | |
ClementBreteau | 1:f3f702086a30 | 148 | data[0] = punch & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 149 | data[1] = punch >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 150 | |
ClementBreteau | 1:f3f702086a30 | 151 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 152 | return (write(_ID, AX12_REG_PUNCH, 2, data)); |
ClementBreteau | 1:f3f702086a30 | 153 | |
ClementBreteau | 1:f3f702086a30 | 154 | } |
ClementBreteau | 1:f3f702086a30 | 155 | |
ClementBreteau | 1:f3f702086a30 | 156 | /** Reset |
ClementBreteau | 1:f3f702086a30 | 157 | */ |
ClementBreteau | 1:f3f702086a30 | 158 | int AX12::Reset(int punch) |
ClementBreteau | 1:f3f702086a30 | 159 | { |
ClementBreteau | 1:f3f702086a30 | 160 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 161 | |
ClementBreteau | 1:f3f702086a30 | 162 | data[0] = punch & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 163 | data[1] = punch >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 164 | |
ClementBreteau | 1:f3f702086a30 | 165 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 166 | return (write(_ID, 0x06, 1,data)); |
ClementBreteau | 1:f3f702086a30 | 167 | |
ClementBreteau | 1:f3f702086a30 | 168 | } |
ClementBreteau | 1:f3f702086a30 | 169 | |
ClementBreteau | 1:f3f702086a30 | 170 | int AX12::Get_Punch (void) |
ClementBreteau | 1:f3f702086a30 | 171 | { |
ClementBreteau | 1:f3f702086a30 | 172 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 173 | int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data); |
ClementBreteau | 1:f3f702086a30 | 174 | int punch = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 175 | return(punch); |
ClementBreteau | 1:f3f702086a30 | 176 | } |
ClementBreteau | 1:f3f702086a30 | 177 | |
ClementBreteau | 1:f3f702086a30 | 178 | |
ClementBreteau | 1:f3f702086a30 | 179 | int AX12::Get_Load_Direction (void) |
ClementBreteau | 1:f3f702086a30 | 180 | { |
ClementBreteau | 1:f3f702086a30 | 181 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 182 | int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); |
ClementBreteau | 1:f3f702086a30 | 183 | int direction = (data[1]>>2) & 0x01; |
ClementBreteau | 1:f3f702086a30 | 184 | return(direction); |
ClementBreteau | 1:f3f702086a30 | 185 | } |
ClementBreteau | 1:f3f702086a30 | 186 | |
ClementBreteau | 1:f3f702086a30 | 187 | |
ClementBreteau | 1:f3f702086a30 | 188 | int AX12::Get_Load_Value (void) |
ClementBreteau | 1:f3f702086a30 | 189 | { |
ClementBreteau | 1:f3f702086a30 | 190 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 191 | int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); |
ClementBreteau | 1:f3f702086a30 | 192 | int Load = (data[0] | (data[1]<<8)) & 0x3FF; |
ClementBreteau | 1:f3f702086a30 | 193 | return(Load); |
ClementBreteau | 1:f3f702086a30 | 194 | } |
ClementBreteau | 1:f3f702086a30 | 195 | |
ClementBreteau | 1:f3f702086a30 | 196 | |
ClementBreteau | 1:f3f702086a30 | 197 | int AX12::Get_Present_Speed (void) |
ClementBreteau | 1:f3f702086a30 | 198 | { |
ClementBreteau | 1:f3f702086a30 | 199 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 200 | int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data); |
ClementBreteau | 1:f3f702086a30 | 201 | int speed = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 202 | return(speed); |
ClementBreteau | 1:f3f702086a30 | 203 | } |
ClementBreteau | 1:f3f702086a30 | 204 | |
ClementBreteau | 1:f3f702086a30 | 205 | |
ClementBreteau | 1:f3f702086a30 | 206 | int AX12::Get_CCW_Angle_Limit (void) |
ClementBreteau | 1:f3f702086a30 | 207 | { |
ClementBreteau | 1:f3f702086a30 | 208 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 209 | int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data); |
ClementBreteau | 1:f3f702086a30 | 210 | int angle = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 211 | angle = (angle * 300) / 1023; |
ClementBreteau | 1:f3f702086a30 | 212 | return(angle); |
ClementBreteau | 1:f3f702086a30 | 213 | } |
ClementBreteau | 1:f3f702086a30 | 214 | |
ClementBreteau | 1:f3f702086a30 | 215 | |
ClementBreteau | 1:f3f702086a30 | 216 | int AX12::Get_CW_Angle_Limit (void) |
ClementBreteau | 1:f3f702086a30 | 217 | { |
ClementBreteau | 1:f3f702086a30 | 218 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 219 | int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data); |
ClementBreteau | 1:f3f702086a30 | 220 | int angle = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 221 | angle = (angle * 300) / 1023; |
ClementBreteau | 1:f3f702086a30 | 222 | return(angle); |
ClementBreteau | 1:f3f702086a30 | 223 | } |
ClementBreteau | 1:f3f702086a30 | 224 | |
ClementBreteau | 1:f3f702086a30 | 225 | |
ClementBreteau | 1:f3f702086a30 | 226 | |
ClementBreteau | 1:f3f702086a30 | 227 | int AX12::Get_Torque_Enable(void) |
ClementBreteau | 1:f3f702086a30 | 228 | { |
ClementBreteau | 1:f3f702086a30 | 229 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 230 | int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data); |
ClementBreteau | 1:f3f702086a30 | 231 | int enable = data[0]; |
ClementBreteau | 1:f3f702086a30 | 232 | return(enable); |
ClementBreteau | 1:f3f702086a30 | 233 | } |
ClementBreteau | 1:f3f702086a30 | 234 | |
ClementBreteau | 1:f3f702086a30 | 235 | |
ClementBreteau | 1:f3f702086a30 | 236 | int AX12::Set_Torque_Enable(int etat) |
ClementBreteau | 1:f3f702086a30 | 237 | { |
ClementBreteau | 1:f3f702086a30 | 238 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 239 | data [0] = etat; |
ClementBreteau | 1:f3f702086a30 | 240 | |
ClementBreteau | 1:f3f702086a30 | 241 | int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data); |
ClementBreteau | 1:f3f702086a30 | 242 | return (error); |
ClementBreteau | 1:f3f702086a30 | 243 | } |
ClementBreteau | 1:f3f702086a30 | 244 | |
ClementBreteau | 1:f3f702086a30 | 245 | |
ClementBreteau | 1:f3f702086a30 | 246 | |
ClementBreteau | 1:f3f702086a30 | 247 | int AX12::Get_Up_Calibration (void) |
ClementBreteau | 1:f3f702086a30 | 248 | { |
ClementBreteau | 1:f3f702086a30 | 249 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 250 | int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data); |
ClementBreteau | 1:f3f702086a30 | 251 | int Up_calibration = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 252 | return(Up_calibration); |
ClementBreteau | 1:f3f702086a30 | 253 | } |
ClementBreteau | 1:f3f702086a30 | 254 | |
ClementBreteau | 1:f3f702086a30 | 255 | |
ClementBreteau | 1:f3f702086a30 | 256 | |
ClementBreteau | 1:f3f702086a30 | 257 | int AX12::Get_Down_Calibration (void) |
ClementBreteau | 1:f3f702086a30 | 258 | { |
ClementBreteau | 1:f3f702086a30 | 259 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 260 | int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data); |
ClementBreteau | 1:f3f702086a30 | 261 | int Dowm_calibration = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 262 | return(Dowm_calibration); |
ClementBreteau | 1:f3f702086a30 | 263 | } |
ClementBreteau | 1:f3f702086a30 | 264 | |
ClementBreteau | 1:f3f702086a30 | 265 | |
ClementBreteau | 1:f3f702086a30 | 266 | |
ClementBreteau | 1:f3f702086a30 | 267 | int AX12::Get_ID(void) |
ClementBreteau | 1:f3f702086a30 | 268 | { |
ClementBreteau | 1:f3f702086a30 | 269 | |
ClementBreteau | 1:f3f702086a30 | 270 | char data[1]={-1}; |
ClementBreteau | 1:f3f702086a30 | 271 | int ErrorCode = read(_ID, AX12_REG_ID, 1, data); |
ClementBreteau | 1:f3f702086a30 | 272 | int id = data[0]; |
ClementBreteau | 1:f3f702086a30 | 273 | return(id); |
ClementBreteau | 1:f3f702086a30 | 274 | } |
ClementBreteau | 1:f3f702086a30 | 275 | |
ClementBreteau | 1:f3f702086a30 | 276 | |
ClementBreteau | 1:f3f702086a30 | 277 | // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) |
ClementBreteau | 1:f3f702086a30 | 278 | int AX12::Get_Max_Torque (void) |
ClementBreteau | 1:f3f702086a30 | 279 | { |
ClementBreteau | 1:f3f702086a30 | 280 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 281 | int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data); |
ClementBreteau | 1:f3f702086a30 | 282 | int torque = data[0] | (data[1]<<8); |
ClementBreteau | 1:f3f702086a30 | 283 | return(torque); |
ClementBreteau | 1:f3f702086a30 | 284 | } |
ClementBreteau | 1:f3f702086a30 | 285 | |
ClementBreteau | 1:f3f702086a30 | 286 | |
ClementBreteau | 1:f3f702086a30 | 287 | |
ClementBreteau | 1:f3f702086a30 | 288 | /** Reglage du couple maximum de l'actionneur |
ClementBreteau | 1:f3f702086a30 | 289 | // minimum >> Ox000 >> decimal 0 |
ClementBreteau | 1:f3f702086a30 | 290 | // maximum >> 0x3FF >> decimal 1023 |
ClementBreteau | 1:f3f702086a30 | 291 | // deflaut >> >> decimal |
ClementBreteau | 1:f3f702086a30 | 292 | */ |
ClementBreteau | 1:f3f702086a30 | 293 | int AX12::Set_Max_Torque(int torque) |
ClementBreteau | 1:f3f702086a30 | 294 | { |
ClementBreteau | 1:f3f702086a30 | 295 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 296 | |
ClementBreteau | 1:f3f702086a30 | 297 | data[0] = torque & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 298 | data[1] = torque >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 299 | |
ClementBreteau | 1:f3f702086a30 | 300 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 301 | return (write(_ID, AX12_REG_MAX_TORQUE, 2, data)); |
ClementBreteau | 1:f3f702086a30 | 302 | |
ClementBreteau | 1:f3f702086a30 | 303 | } |
ClementBreteau | 1:f3f702086a30 | 304 | |
ClementBreteau | 1:f3f702086a30 | 305 | |
ClementBreteau | 1:f3f702086a30 | 306 | |
ClementBreteau | 1:f3f702086a30 | 307 | |
ClementBreteau | 1:f3f702086a30 | 308 | /** Reglage de la desactivation des actionneurs si une erreur le concernant se produit |
ClementBreteau | 1:f3f702086a30 | 309 | Bit Function |
ClementBreteau | 1:f3f702086a30 | 310 | Bit 7 0 |
ClementBreteau | 1:f3f702086a30 | 311 | Bit 6 If set to 1, torque off when an Instruction Error occurs |
ClementBreteau | 1:f3f702086a30 | 312 | Bit 5 If set to 1, torque off when an Overload Error occurs |
ClementBreteau | 1:f3f702086a30 | 313 | Bit 4 If set to 1, torque off when a Checksum Error occurs |
ClementBreteau | 1:f3f702086a30 | 314 | Bit 3 If set to 1, torque off when a Range Error occurs |
ClementBreteau | 1:f3f702086a30 | 315 | Bit 2 If set to 1, torque off when an Overheating Error occurs |
ClementBreteau | 1:f3f702086a30 | 316 | Bit 1 If set to 1, torque off when an Angle Limit Error occurs |
ClementBreteau | 1:f3f702086a30 | 317 | Bit 0 If set to 1, torque off when an Input Voltage Error occurs |
ClementBreteau | 1:f3f702086a30 | 318 | */ |
ClementBreteau | 1:f3f702086a30 | 319 | int AX12::Set_Alarm_Shutdown(int valeur) |
ClementBreteau | 1:f3f702086a30 | 320 | { |
ClementBreteau | 1:f3f702086a30 | 321 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 322 | data [0] = valeur; |
ClementBreteau | 1:f3f702086a30 | 323 | |
ClementBreteau | 1:f3f702086a30 | 324 | int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data); |
ClementBreteau | 1:f3f702086a30 | 325 | return (val_alarm_shutdown); |
ClementBreteau | 1:f3f702086a30 | 326 | } |
ClementBreteau | 1:f3f702086a30 | 327 | |
ClementBreteau | 1:f3f702086a30 | 328 | |
ClementBreteau | 1:f3f702086a30 | 329 | |
ClementBreteau | 1:f3f702086a30 | 330 | /** Reglage de l'activation de l'alarme |
ClementBreteau | 1:f3f702086a30 | 331 | Bit Function |
ClementBreteau | 1:f3f702086a30 | 332 | Bit 7 0 |
ClementBreteau | 1:f3f702086a30 | 333 | Bit 6 If set to 1, the LED blinks when an Instruction Error occurs |
ClementBreteau | 1:f3f702086a30 | 334 | Bit 5 If set to 1, the LED blinks when an Overload Error occurs |
ClementBreteau | 1:f3f702086a30 | 335 | Bit 4 If set to 1, the LED blinks when a Checksum Error occurs |
ClementBreteau | 1:f3f702086a30 | 336 | Bit 3 If set to 1, the LED blinks when a Range Error occurs |
ClementBreteau | 1:f3f702086a30 | 337 | Bit 2 If set to 1, the LED blinks when an Overheating Error occurs |
ClementBreteau | 1:f3f702086a30 | 338 | Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs |
ClementBreteau | 1:f3f702086a30 | 339 | Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs |
ClementBreteau | 1:f3f702086a30 | 340 | */ |
ClementBreteau | 1:f3f702086a30 | 341 | int AX12::Set_Alarm_LED(int valeur) |
ClementBreteau | 1:f3f702086a30 | 342 | { |
ClementBreteau | 1:f3f702086a30 | 343 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 344 | data [0] = valeur; |
ClementBreteau | 1:f3f702086a30 | 345 | |
ClementBreteau | 1:f3f702086a30 | 346 | int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data); |
ClementBreteau | 1:f3f702086a30 | 347 | return (val_alarmLED); |
ClementBreteau | 1:f3f702086a30 | 348 | } |
ClementBreteau | 1:f3f702086a30 | 349 | |
ClementBreteau | 1:f3f702086a30 | 350 | |
ClementBreteau | 1:f3f702086a30 | 351 | |
ClementBreteau | 1:f3f702086a30 | 352 | |
ClementBreteau | 1:f3f702086a30 | 353 | // Reglage de la réponse à une instruction |
ClementBreteau | 1:f3f702086a30 | 354 | // 0 >> ne repond a aucune instructions |
ClementBreteau | 1:f3f702086a30 | 355 | // 1 >> repond seulement aux instructions READ_DATA |
ClementBreteau | 1:f3f702086a30 | 356 | // 2 >> repond à toutes les instructions |
ClementBreteau | 1:f3f702086a30 | 357 | int AX12::Set_Status_Return_Level(int etat) |
ClementBreteau | 1:f3f702086a30 | 358 | { |
ClementBreteau | 1:f3f702086a30 | 359 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 360 | data [0] = etat; |
ClementBreteau | 1:f3f702086a30 | 361 | |
ClementBreteau | 1:f3f702086a30 | 362 | int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data); |
ClementBreteau | 1:f3f702086a30 | 363 | return (val_return_lvl); |
ClementBreteau | 1:f3f702086a30 | 364 | } |
ClementBreteau | 1:f3f702086a30 | 365 | |
ClementBreteau | 1:f3f702086a30 | 366 | |
ClementBreteau | 1:f3f702086a30 | 367 | // Reglage de la tension minimale |
ClementBreteau | 1:f3f702086a30 | 368 | // minimum >> Ox32 >> decimal 50 |
ClementBreteau | 1:f3f702086a30 | 369 | // maximum >> 0xFA >> decimal 250 |
ClementBreteau | 1:f3f702086a30 | 370 | // deflaut >> 0x3C >> decimal 60 |
ClementBreteau | 1:f3f702086a30 | 371 | int AX12::Set_Lowest_Voltage(int val_lowest_voltage) |
ClementBreteau | 1:f3f702086a30 | 372 | { |
ClementBreteau | 1:f3f702086a30 | 373 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 374 | data [0] = val_lowest_voltage; |
ClementBreteau | 1:f3f702086a30 | 375 | |
ClementBreteau | 1:f3f702086a30 | 376 | int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data); |
ClementBreteau | 1:f3f702086a30 | 377 | return (val_lowvolt); |
ClementBreteau | 1:f3f702086a30 | 378 | } |
ClementBreteau | 1:f3f702086a30 | 379 | |
ClementBreteau | 1:f3f702086a30 | 380 | |
ClementBreteau | 1:f3f702086a30 | 381 | // Reglage de la tension maximale |
ClementBreteau | 1:f3f702086a30 | 382 | // minimum >> Ox32 >> decimal 50 |
ClementBreteau | 1:f3f702086a30 | 383 | // maximum >> 0xFA >> decimal 250 |
ClementBreteau | 1:f3f702086a30 | 384 | // deflaut >> 0xBE >> decimal 190 |
ClementBreteau | 1:f3f702086a30 | 385 | int AX12::Set_Highest_Voltage(int val_highest_voltage) |
ClementBreteau | 1:f3f702086a30 | 386 | { |
ClementBreteau | 1:f3f702086a30 | 387 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 388 | data [0] = val_highest_voltage; |
ClementBreteau | 1:f3f702086a30 | 389 | |
ClementBreteau | 1:f3f702086a30 | 390 | int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data); |
ClementBreteau | 1:f3f702086a30 | 391 | return (val_highvolt); |
ClementBreteau | 1:f3f702086a30 | 392 | } |
ClementBreteau | 1:f3f702086a30 | 393 | |
ClementBreteau | 1:f3f702086a30 | 394 | |
ClementBreteau | 1:f3f702086a30 | 395 | // Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time |
ClementBreteau | 1:f3f702086a30 | 396 | // minimum >> 0 us |
ClementBreteau | 1:f3f702086a30 | 397 | // maximum >> 508 us |
ClementBreteau | 1:f3f702086a30 | 398 | // deflaut >> 125 us |
ClementBreteau | 1:f3f702086a30 | 399 | int AX12::Set_Delay_Time (int val_delay_time ) |
ClementBreteau | 1:f3f702086a30 | 400 | { |
ClementBreteau | 1:f3f702086a30 | 401 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 402 | data [0] = val_delay_time/2.0; |
ClementBreteau | 1:f3f702086a30 | 403 | |
ClementBreteau | 1:f3f702086a30 | 404 | int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data); |
ClementBreteau | 1:f3f702086a30 | 405 | return (valdelay_time ); |
ClementBreteau | 1:f3f702086a30 | 406 | } |
ClementBreteau | 1:f3f702086a30 | 407 | |
ClementBreteau | 1:f3f702086a30 | 408 | |
ClementBreteau | 1:f3f702086a30 | 409 | // Reglage de la température max du cervo |
ClementBreteau | 1:f3f702086a30 | 410 | // minimum >> Ox00 >> decimal 0 |
ClementBreteau | 1:f3f702086a30 | 411 | // maximum >> 0x96 >> decimal 150 |
ClementBreteau | 1:f3f702086a30 | 412 | int AX12::Set_Temperature_Max (int val_temperature ) |
ClementBreteau | 1:f3f702086a30 | 413 | { |
ClementBreteau | 1:f3f702086a30 | 414 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 415 | data [0] = val_temperature; |
ClementBreteau | 1:f3f702086a30 | 416 | |
ClementBreteau | 1:f3f702086a30 | 417 | int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data); |
ClementBreteau | 1:f3f702086a30 | 418 | return (valtemp_max ); |
ClementBreteau | 1:f3f702086a30 | 419 | } |
ClementBreteau | 1:f3f702086a30 | 420 | |
ClementBreteau | 1:f3f702086a30 | 421 | // Etat LED |
ClementBreteau | 1:f3f702086a30 | 422 | // 0 = off |
ClementBreteau | 1:f3f702086a30 | 423 | // 1 = on |
ClementBreteau | 1:f3f702086a30 | 424 | int AX12::Set_Etat_LED(int etat) |
ClementBreteau | 1:f3f702086a30 | 425 | { |
ClementBreteau | 1:f3f702086a30 | 426 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 427 | data [0] = etat; |
ClementBreteau | 1:f3f702086a30 | 428 | |
ClementBreteau | 1:f3f702086a30 | 429 | int valLED = write(_ID, AX12_REG_LED, 1, data); |
ClementBreteau | 1:f3f702086a30 | 430 | return (valLED); |
ClementBreteau | 1:f3f702086a30 | 431 | } |
ClementBreteau | 1:f3f702086a30 | 432 | |
ClementBreteau | 1:f3f702086a30 | 433 | // Set the mode of the servo |
ClementBreteau | 1:f3f702086a30 | 434 | // 0 = Positional (0-300 degrees) |
ClementBreteau | 1:f3f702086a30 | 435 | // 1 = Rotational -1 to 1 speed |
ClementBreteau | 1:f3f702086a30 | 436 | int AX12::Set_Mode(int mode) |
ClementBreteau | 1:f3f702086a30 | 437 | { |
ClementBreteau | 1:f3f702086a30 | 438 | |
ClementBreteau | 1:f3f702086a30 | 439 | if (mode == 1) { // set CR |
ClementBreteau | 1:f3f702086a30 | 440 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 441 | Set_CW_Angle_Limit(0); |
ClementBreteau | 1:f3f702086a30 | 442 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 443 | Set_CCW_Angle_Limit(0); |
ClementBreteau | 1:f3f702086a30 | 444 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 445 | Set_CR_Speed(0.0); |
ClementBreteau | 1:f3f702086a30 | 446 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 447 | } else { |
ClementBreteau | 1:f3f702086a30 | 448 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 449 | Set_CW_Angle_Limit(0); |
ClementBreteau | 1:f3f702086a30 | 450 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 451 | Set_CCW_Angle_Limit(300); |
ClementBreteau | 1:f3f702086a30 | 452 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 453 | Set_CR_Speed(0.0); |
ClementBreteau | 1:f3f702086a30 | 454 | //wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 455 | } |
ClementBreteau | 1:f3f702086a30 | 456 | return(0); |
ClementBreteau | 1:f3f702086a30 | 457 | } |
ClementBreteau | 1:f3f702086a30 | 458 | |
ClementBreteau | 1:f3f702086a30 | 459 | int AX12::Set_Goal_speed(int speed, int flags) |
ClementBreteau | 1:f3f702086a30 | 460 | { |
ClementBreteau | 1:f3f702086a30 | 461 | |
ClementBreteau | 1:f3f702086a30 | 462 | char reg_flag = 0; |
ClementBreteau | 1:f3f702086a30 | 463 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 464 | |
ClementBreteau | 1:f3f702086a30 | 465 | // set the flag is only the register bit is set in the flag |
ClementBreteau | 1:f3f702086a30 | 466 | if (flags == 0x2) { |
ClementBreteau | 1:f3f702086a30 | 467 | reg_flag = 1; |
ClementBreteau | 1:f3f702086a30 | 468 | } |
ClementBreteau | 1:f3f702086a30 | 469 | |
ClementBreteau | 1:f3f702086a30 | 470 | data[0] = speed & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 471 | data[1] = speed >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 472 | |
ClementBreteau | 1:f3f702086a30 | 473 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 474 | int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag); |
ClementBreteau | 1:f3f702086a30 | 475 | |
ClementBreteau | 1:f3f702086a30 | 476 | /*if (flags == 1) { |
ClementBreteau | 1:f3f702086a30 | 477 | // block until it comes to a halt |
ClementBreteau | 1:f3f702086a30 | 478 | while (isMoving()) |
ClementBreteau | 1:f3f702086a30 | 479 | { |
ClementBreteau | 1:f3f702086a30 | 480 | } |
ClementBreteau | 1:f3f702086a30 | 481 | |
ClementBreteau | 1:f3f702086a30 | 482 | }*/ |
ClementBreteau | 1:f3f702086a30 | 483 | return(rVal); |
ClementBreteau | 1:f3f702086a30 | 484 | } |
ClementBreteau | 1:f3f702086a30 | 485 | |
ClementBreteau | 1:f3f702086a30 | 486 | |
ClementBreteau | 1:f3f702086a30 | 487 | // if flag[0] is set, were blocking |
ClementBreteau | 1:f3f702086a30 | 488 | // if flag[1] is set, we're registering |
ClementBreteau | 1:f3f702086a30 | 489 | // they are mutually exclusive operations |
ClementBreteau | 1:f3f702086a30 | 490 | int AX12::Set_Goal(int degrees, int flags) |
ClementBreteau | 1:f3f702086a30 | 491 | { |
ClementBreteau | 1:f3f702086a30 | 492 | |
ClementBreteau | 1:f3f702086a30 | 493 | char reg_flag = 0; |
ClementBreteau | 1:f3f702086a30 | 494 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 495 | |
ClementBreteau | 1:f3f702086a30 | 496 | // set the flag is only the register bit is set in the flag |
ClementBreteau | 1:f3f702086a30 | 497 | if (flags == 0x2) { |
ClementBreteau | 1:f3f702086a30 | 498 | reg_flag = 1; |
ClementBreteau | 1:f3f702086a30 | 499 | } |
ClementBreteau | 1:f3f702086a30 | 500 | |
ClementBreteau | 1:f3f702086a30 | 501 | // 1023 / 300 * degrees |
ClementBreteau | 1:f3f702086a30 | 502 | short goal = (1023 * degrees) / 300; |
ClementBreteau | 1:f3f702086a30 | 503 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 504 | printf("SetGoal to 0x%x\n",goal); |
ClementBreteau | 1:f3f702086a30 | 505 | #endif |
ClementBreteau | 1:f3f702086a30 | 506 | |
ClementBreteau | 1:f3f702086a30 | 507 | data[0] = goal & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 508 | data[1] = goal >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 509 | |
ClementBreteau | 1:f3f702086a30 | 510 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 511 | int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); |
ClementBreteau | 1:f3f702086a30 | 512 | |
ClementBreteau | 1:f3f702086a30 | 513 | /*if (flags == 1) { |
ClementBreteau | 1:f3f702086a30 | 514 | // block until it comes to a halt |
ClementBreteau | 1:f3f702086a30 | 515 | while (isMoving()) |
ClementBreteau | 1:f3f702086a30 | 516 | { |
ClementBreteau | 1:f3f702086a30 | 517 | } |
ClementBreteau | 1:f3f702086a30 | 518 | |
ClementBreteau | 1:f3f702086a30 | 519 | }*/ |
ClementBreteau | 1:f3f702086a30 | 520 | return(rVal); |
ClementBreteau | 1:f3f702086a30 | 521 | } |
ClementBreteau | 1:f3f702086a30 | 522 | |
ClementBreteau | 1:f3f702086a30 | 523 | |
ClementBreteau | 1:f3f702086a30 | 524 | // Set continuous rotation speed from -1 to 1 |
ClementBreteau | 1:f3f702086a30 | 525 | int AX12::Set_CR_Speed(float speed) |
ClementBreteau | 1:f3f702086a30 | 526 | { |
ClementBreteau | 1:f3f702086a30 | 527 | |
ClementBreteau | 1:f3f702086a30 | 528 | // bit 10 = direction, 0 = CCW, 1=CW |
ClementBreteau | 1:f3f702086a30 | 529 | // bits 9-0 = Speed |
ClementBreteau | 1:f3f702086a30 | 530 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 531 | |
ClementBreteau | 1:f3f702086a30 | 532 | int goal = (0x3ff * abs(speed)); |
ClementBreteau | 1:f3f702086a30 | 533 | |
ClementBreteau | 1:f3f702086a30 | 534 | // Set direction CW if we have a negative speed |
ClementBreteau | 1:f3f702086a30 | 535 | if (speed < 0) { |
ClementBreteau | 1:f3f702086a30 | 536 | goal |= (0x1 << 10); |
ClementBreteau | 1:f3f702086a30 | 537 | } |
ClementBreteau | 1:f3f702086a30 | 538 | |
ClementBreteau | 1:f3f702086a30 | 539 | data[0] = goal & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 540 | data[1] = goal >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 541 | |
ClementBreteau | 1:f3f702086a30 | 542 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 543 | int rVal = write(_ID, 0x20, 2, data); |
ClementBreteau | 1:f3f702086a30 | 544 | |
ClementBreteau | 1:f3f702086a30 | 545 | return(rVal); |
ClementBreteau | 1:f3f702086a30 | 546 | } |
ClementBreteau | 1:f3f702086a30 | 547 | |
ClementBreteau | 1:f3f702086a30 | 548 | |
ClementBreteau | 1:f3f702086a30 | 549 | int AX12::Set_CW_Angle_Limit (int degrees) |
ClementBreteau | 1:f3f702086a30 | 550 | { |
ClementBreteau | 1:f3f702086a30 | 551 | |
ClementBreteau | 1:f3f702086a30 | 552 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 553 | |
ClementBreteau | 1:f3f702086a30 | 554 | // 1023 / 300 * degrees |
ClementBreteau | 1:f3f702086a30 | 555 | short limit = (1023 * degrees) / 300; |
ClementBreteau | 1:f3f702086a30 | 556 | |
ClementBreteau | 1:f3f702086a30 | 557 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 558 | printf("SetCWLimit to 0x%x\n",limit); |
ClementBreteau | 1:f3f702086a30 | 559 | #endif |
ClementBreteau | 1:f3f702086a30 | 560 | |
ClementBreteau | 1:f3f702086a30 | 561 | data[0] = limit & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 562 | data[1] = limit >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 563 | |
ClementBreteau | 1:f3f702086a30 | 564 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 565 | return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); |
ClementBreteau | 1:f3f702086a30 | 566 | |
ClementBreteau | 1:f3f702086a30 | 567 | } |
ClementBreteau | 1:f3f702086a30 | 568 | |
ClementBreteau | 1:f3f702086a30 | 569 | int AX12::Set_CCW_Angle_Limit (int degrees) |
ClementBreteau | 1:f3f702086a30 | 570 | { |
ClementBreteau | 1:f3f702086a30 | 571 | |
ClementBreteau | 1:f3f702086a30 | 572 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 573 | |
ClementBreteau | 1:f3f702086a30 | 574 | // 1023 / 300 * degrees |
ClementBreteau | 1:f3f702086a30 | 575 | short limit = (1023 * degrees) / 300; |
ClementBreteau | 1:f3f702086a30 | 576 | |
ClementBreteau | 1:f3f702086a30 | 577 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 578 | printf("SetCCWLimit to 0x%x\n",limit); |
ClementBreteau | 1:f3f702086a30 | 579 | #endif |
ClementBreteau | 1:f3f702086a30 | 580 | |
ClementBreteau | 1:f3f702086a30 | 581 | data[0] = limit & 0xff; // bottom 8 bits |
ClementBreteau | 1:f3f702086a30 | 582 | data[1] = limit >> 8; // top 8 bits |
ClementBreteau | 1:f3f702086a30 | 583 | |
ClementBreteau | 1:f3f702086a30 | 584 | // write the packet, return the error code |
ClementBreteau | 1:f3f702086a30 | 585 | return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); |
ClementBreteau | 1:f3f702086a30 | 586 | } |
ClementBreteau | 1:f3f702086a30 | 587 | |
ClementBreteau | 1:f3f702086a30 | 588 | |
ClementBreteau | 1:f3f702086a30 | 589 | int AX12::Set_ID (int CurrentID, int NewID) |
ClementBreteau | 1:f3f702086a30 | 590 | { |
ClementBreteau | 1:f3f702086a30 | 591 | |
ClementBreteau | 1:f3f702086a30 | 592 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 593 | data[0] = NewID; |
ClementBreteau | 1:f3f702086a30 | 594 | |
ClementBreteau | 1:f3f702086a30 | 595 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 596 | printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); |
ClementBreteau | 1:f3f702086a30 | 597 | #endif |
ClementBreteau | 1:f3f702086a30 | 598 | |
ClementBreteau | 1:f3f702086a30 | 599 | return (write(CurrentID, AX12_REG_ID, 1, data)); |
ClementBreteau | 1:f3f702086a30 | 600 | |
ClementBreteau | 1:f3f702086a30 | 601 | } |
ClementBreteau | 1:f3f702086a30 | 602 | |
ClementBreteau | 1:f3f702086a30 | 603 | |
ClementBreteau | 1:f3f702086a30 | 604 | int AX12::Set_Baud (int baud) |
ClementBreteau | 1:f3f702086a30 | 605 | { |
ClementBreteau | 1:f3f702086a30 | 606 | |
ClementBreteau | 1:f3f702086a30 | 607 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 608 | data[0] = baud; |
ClementBreteau | 1:f3f702086a30 | 609 | |
ClementBreteau | 1:f3f702086a30 | 610 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 611 | printf("Setting Baud rate to %d\n",baud); |
ClementBreteau | 1:f3f702086a30 | 612 | #endif |
ClementBreteau | 1:f3f702086a30 | 613 | |
ClementBreteau | 1:f3f702086a30 | 614 | return (write(_ID, AX12_REG_BAUD, 1, data)); |
ClementBreteau | 1:f3f702086a30 | 615 | |
ClementBreteau | 1:f3f702086a30 | 616 | } |
ClementBreteau | 1:f3f702086a30 | 617 | |
ClementBreteau | 1:f3f702086a30 | 618 | |
ClementBreteau | 1:f3f702086a30 | 619 | |
ClementBreteau | 1:f3f702086a30 | 620 | // return 1 is the servo is still in flight |
ClementBreteau | 1:f3f702086a30 | 621 | int AX12::isMoving(void) |
ClementBreteau | 1:f3f702086a30 | 622 | { |
ClementBreteau | 1:f3f702086a30 | 623 | |
ClementBreteau | 1:f3f702086a30 | 624 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 625 | read(_ID,AX12_REG_MOVING,1,data); |
ClementBreteau | 1:f3f702086a30 | 626 | return(data[0]); |
ClementBreteau | 1:f3f702086a30 | 627 | } |
ClementBreteau | 1:f3f702086a30 | 628 | |
ClementBreteau | 1:f3f702086a30 | 629 | void AX12::reset() |
ClementBreteau | 1:f3f702086a30 | 630 | { |
ClementBreteau | 1:f3f702086a30 | 631 | |
ClementBreteau | 1:f3f702086a30 | 632 | unsigned char TxBuf[16]; |
ClementBreteau | 1:f3f702086a30 | 633 | unsigned char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 634 | unsigned long debut=0; |
ClementBreteau | 1:f3f702086a30 | 635 | |
ClementBreteau | 1:f3f702086a30 | 636 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 637 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 638 | printf("\nreset\n"); |
ClementBreteau | 1:f3f702086a30 | 639 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
ClementBreteau | 1:f3f702086a30 | 640 | #endif |
ClementBreteau | 1:f3f702086a30 | 641 | |
ClementBreteau | 1:f3f702086a30 | 642 | TxBuf[0] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 643 | TxBuf[1] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 644 | |
ClementBreteau | 1:f3f702086a30 | 645 | // ID - Broadcast |
ClementBreteau | 1:f3f702086a30 | 646 | TxBuf[2] =_ID; |
ClementBreteau | 1:f3f702086a30 | 647 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 648 | |
ClementBreteau | 1:f3f702086a30 | 649 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 650 | printf(" ID : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 651 | #endif |
ClementBreteau | 1:f3f702086a30 | 652 | |
ClementBreteau | 1:f3f702086a30 | 653 | // Length |
ClementBreteau | 1:f3f702086a30 | 654 | TxBuf[3] = 0x02; |
ClementBreteau | 1:f3f702086a30 | 655 | sum += TxBuf[3]; |
ClementBreteau | 1:f3f702086a30 | 656 | |
ClementBreteau | 1:f3f702086a30 | 657 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 658 | printf(" Length %d\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 659 | #endif |
ClementBreteau | 1:f3f702086a30 | 660 | |
ClementBreteau | 1:f3f702086a30 | 661 | // Instruction - ACTION |
ClementBreteau | 1:f3f702086a30 | 662 | TxBuf[4] = 0x06; //reset |
ClementBreteau | 1:f3f702086a30 | 663 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 664 | |
ClementBreteau | 1:f3f702086a30 | 665 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 666 | printf(" Instruction 0x%X\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 667 | #endif |
ClementBreteau | 1:f3f702086a30 | 668 | |
ClementBreteau | 1:f3f702086a30 | 669 | // Checksum |
ClementBreteau | 1:f3f702086a30 | 670 | TxBuf[5] = 0xFF - sum; |
ClementBreteau | 1:f3f702086a30 | 671 | //#ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 672 | printf(" Checksum 0x%X\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 673 | //#endif |
ClementBreteau | 1:f3f702086a30 | 674 | |
ClementBreteau | 1:f3f702086a30 | 675 | // Transmit the packet in one burst with no pausing |
ClementBreteau | 1:f3f702086a30 | 676 | for (int i = 0; i < 6 ; i++) |
ClementBreteau | 1:f3f702086a30 | 677 | { |
ClementBreteau | 1:f3f702086a30 | 678 | while(_ax12.writeable()==0); |
ClementBreteau | 1:f3f702086a30 | 679 | _ax12.putc(TxBuf[i]); |
ClementBreteau | 1:f3f702086a30 | 680 | |
ClementBreteau | 1:f3f702086a30 | 681 | } |
ClementBreteau | 1:f3f702086a30 | 682 | wait(0.001); |
ClementBreteau | 1:f3f702086a30 | 683 | debut=t.read_ms(); |
ClementBreteau | 1:f3f702086a30 | 684 | |
ClementBreteau | 1:f3f702086a30 | 685 | do |
ClementBreteau | 1:f3f702086a30 | 686 | { |
ClementBreteau | 1:f3f702086a30 | 687 | if (_ax12.readable()==-1) // reception du premier Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 688 | printf("%02x",_ax12.getc()); |
ClementBreteau | 1:f3f702086a30 | 689 | } |
ClementBreteau | 1:f3f702086a30 | 690 | while((t.read_ms()-debut)<500); |
ClementBreteau | 1:f3f702086a30 | 691 | |
ClementBreteau | 1:f3f702086a30 | 692 | printf("\n"); |
ClementBreteau | 1:f3f702086a30 | 693 | return; |
ClementBreteau | 1:f3f702086a30 | 694 | } |
ClementBreteau | 1:f3f702086a30 | 695 | |
ClementBreteau | 1:f3f702086a30 | 696 | void AX12::read_all_info(unsigned char start, unsigned char longueur) |
ClementBreteau | 1:f3f702086a30 | 697 | { |
ClementBreteau | 1:f3f702086a30 | 698 | |
ClementBreteau | 1:f3f702086a30 | 699 | unsigned char TxBuf[16]; |
ClementBreteau | 1:f3f702086a30 | 700 | unsigned char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 701 | unsigned long debut=0; |
ClementBreteau | 1:f3f702086a30 | 702 | |
ClementBreteau | 1:f3f702086a30 | 703 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 704 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 705 | printf("\nreset\n"); |
ClementBreteau | 1:f3f702086a30 | 706 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
ClementBreteau | 1:f3f702086a30 | 707 | #endif |
ClementBreteau | 1:f3f702086a30 | 708 | |
ClementBreteau | 1:f3f702086a30 | 709 | TxBuf[0] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 710 | TxBuf[1] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 711 | |
ClementBreteau | 1:f3f702086a30 | 712 | // ID - Broadcast |
ClementBreteau | 1:f3f702086a30 | 713 | TxBuf[2] =_ID; |
ClementBreteau | 1:f3f702086a30 | 714 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 715 | |
ClementBreteau | 1:f3f702086a30 | 716 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 717 | printf(" ID : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 718 | #endif |
ClementBreteau | 1:f3f702086a30 | 719 | |
ClementBreteau | 1:f3f702086a30 | 720 | // Length |
ClementBreteau | 1:f3f702086a30 | 721 | TxBuf[3] = 0x04; |
ClementBreteau | 1:f3f702086a30 | 722 | sum += TxBuf[3]; |
ClementBreteau | 1:f3f702086a30 | 723 | |
ClementBreteau | 1:f3f702086a30 | 724 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 725 | printf(" Length %d\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 726 | #endif |
ClementBreteau | 1:f3f702086a30 | 727 | |
ClementBreteau | 1:f3f702086a30 | 728 | // Instruction - ACTION |
ClementBreteau | 1:f3f702086a30 | 729 | TxBuf[4] = INST_READ; //reset |
ClementBreteau | 1:f3f702086a30 | 730 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 731 | |
ClementBreteau | 1:f3f702086a30 | 732 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 733 | printf(" Instruction 0x%X\n",TxBuf[4]); |
ClementBreteau | 1:f3f702086a30 | 734 | #endif |
ClementBreteau | 1:f3f702086a30 | 735 | |
ClementBreteau | 1:f3f702086a30 | 736 | TxBuf[5] = start; //reset |
ClementBreteau | 1:f3f702086a30 | 737 | sum += TxBuf[5]; |
ClementBreteau | 1:f3f702086a30 | 738 | |
ClementBreteau | 1:f3f702086a30 | 739 | TxBuf[6] = longueur; //reset |
ClementBreteau | 1:f3f702086a30 | 740 | sum += TxBuf[6]; |
ClementBreteau | 1:f3f702086a30 | 741 | |
ClementBreteau | 1:f3f702086a30 | 742 | |
ClementBreteau | 1:f3f702086a30 | 743 | // Checksum |
ClementBreteau | 1:f3f702086a30 | 744 | TxBuf[7] = 0xFF - sum; |
ClementBreteau | 1:f3f702086a30 | 745 | //#ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 746 | //printf(" Checksum 0x%X\n\r",TxBuf[7]); |
ClementBreteau | 1:f3f702086a30 | 747 | //#endif |
ClementBreteau | 1:f3f702086a30 | 748 | |
ClementBreteau | 1:f3f702086a30 | 749 | // Transmit the packet in one burst with no pausing |
ClementBreteau | 1:f3f702086a30 | 750 | for (int i = 0; i < 8 ; i++) |
ClementBreteau | 1:f3f702086a30 | 751 | { |
ClementBreteau | 1:f3f702086a30 | 752 | while(_ax12.writeable()==0); |
ClementBreteau | 1:f3f702086a30 | 753 | _ax12.putc(TxBuf[i]); |
ClementBreteau | 1:f3f702086a30 | 754 | |
ClementBreteau | 1:f3f702086a30 | 755 | } |
ClementBreteau | 1:f3f702086a30 | 756 | |
ClementBreteau | 1:f3f702086a30 | 757 | debut=t.read_ms(); |
ClementBreteau | 1:f3f702086a30 | 758 | int i=0; |
ClementBreteau | 1:f3f702086a30 | 759 | do |
ClementBreteau | 1:f3f702086a30 | 760 | { |
ClementBreteau | 1:f3f702086a30 | 761 | if (_ax12.readable()) |
ClementBreteau | 1:f3f702086a30 | 762 | { // reception du premier Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 763 | printf("%02d:%02x ",start+i,_ax12.getc()); |
ClementBreteau | 1:f3f702086a30 | 764 | i++; |
ClementBreteau | 1:f3f702086a30 | 765 | } |
ClementBreteau | 1:f3f702086a30 | 766 | } |
ClementBreteau | 1:f3f702086a30 | 767 | while((t.read_ms()-debut)<5000); |
ClementBreteau | 1:f3f702086a30 | 768 | |
ClementBreteau | 1:f3f702086a30 | 769 | printf("\n"); |
ClementBreteau | 1:f3f702086a30 | 770 | return; |
ClementBreteau | 1:f3f702086a30 | 771 | } |
ClementBreteau | 1:f3f702086a30 | 772 | |
ClementBreteau | 1:f3f702086a30 | 773 | |
ClementBreteau | 1:f3f702086a30 | 774 | void AX12::trigger(void) |
ClementBreteau | 1:f3f702086a30 | 775 | { |
ClementBreteau | 1:f3f702086a30 | 776 | |
ClementBreteau | 1:f3f702086a30 | 777 | char TxBuf[16]; |
ClementBreteau | 1:f3f702086a30 | 778 | char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 779 | |
ClementBreteau | 1:f3f702086a30 | 780 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 781 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 782 | printf("\nTriggered\n"); |
ClementBreteau | 1:f3f702086a30 | 783 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
ClementBreteau | 1:f3f702086a30 | 784 | #endif |
ClementBreteau | 1:f3f702086a30 | 785 | |
ClementBreteau | 1:f3f702086a30 | 786 | TxBuf[0] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 787 | TxBuf[1] = 0xFF; |
ClementBreteau | 1:f3f702086a30 | 788 | |
ClementBreteau | 1:f3f702086a30 | 789 | // ID - Broadcast |
ClementBreteau | 1:f3f702086a30 | 790 | TxBuf[2] = 0xFE; |
ClementBreteau | 1:f3f702086a30 | 791 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 792 | |
ClementBreteau | 1:f3f702086a30 | 793 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 794 | printf(" ID : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 795 | #endif |
ClementBreteau | 1:f3f702086a30 | 796 | |
ClementBreteau | 1:f3f702086a30 | 797 | // Length |
ClementBreteau | 1:f3f702086a30 | 798 | TxBuf[3] = 0x02; |
ClementBreteau | 1:f3f702086a30 | 799 | sum += TxBuf[3]; |
ClementBreteau | 1:f3f702086a30 | 800 | |
ClementBreteau | 1:f3f702086a30 | 801 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 802 | printf(" Length %d\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 803 | #endif |
ClementBreteau | 1:f3f702086a30 | 804 | |
ClementBreteau | 1:f3f702086a30 | 805 | // Instruction - ACTION |
ClementBreteau | 1:f3f702086a30 | 806 | TxBuf[4] = 0x04; |
ClementBreteau | 1:f3f702086a30 | 807 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 808 | |
ClementBreteau | 1:f3f702086a30 | 809 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 810 | printf(" Instruction 0x%X\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 811 | #endif |
ClementBreteau | 1:f3f702086a30 | 812 | |
ClementBreteau | 1:f3f702086a30 | 813 | // Checksum |
ClementBreteau | 1:f3f702086a30 | 814 | TxBuf[5] = 0xFF - sum; |
ClementBreteau | 1:f3f702086a30 | 815 | #ifdef AX12_TRIGGER_DEBUG |
ClementBreteau | 1:f3f702086a30 | 816 | printf(" Checksum 0x%X\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 817 | #endif |
ClementBreteau | 1:f3f702086a30 | 818 | |
ClementBreteau | 1:f3f702086a30 | 819 | // Transmit the packet in one burst with no pausing |
ClementBreteau | 1:f3f702086a30 | 820 | for (int i = 0; i < 6 ; i++) { |
ClementBreteau | 1:f3f702086a30 | 821 | _ax12.putc(TxBuf[i]); |
ClementBreteau | 1:f3f702086a30 | 822 | } |
ClementBreteau | 1:f3f702086a30 | 823 | |
ClementBreteau | 1:f3f702086a30 | 824 | // This is a broadcast packet, so there will be no reply |
ClementBreteau | 1:f3f702086a30 | 825 | return; |
ClementBreteau | 1:f3f702086a30 | 826 | } |
ClementBreteau | 1:f3f702086a30 | 827 | |
ClementBreteau | 1:f3f702086a30 | 828 | |
ClementBreteau | 1:f3f702086a30 | 829 | float AX12::Get_Position(void) |
ClementBreteau | 1:f3f702086a30 | 830 | { |
ClementBreteau | 1:f3f702086a30 | 831 | |
ClementBreteau | 1:f3f702086a30 | 832 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 833 | printf("\nGetPositionID(%d)",_ID); |
ClementBreteau | 1:f3f702086a30 | 834 | #endif |
ClementBreteau | 1:f3f702086a30 | 835 | |
ClementBreteau | 1:f3f702086a30 | 836 | char data[2]; |
ClementBreteau | 1:f3f702086a30 | 837 | |
ClementBreteau | 1:f3f702086a30 | 838 | int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); |
ClementBreteau | 1:f3f702086a30 | 839 | int position = data[0] | (data[1] << 8); |
ClementBreteau | 1:f3f702086a30 | 840 | float angle = ((float)position * 300.0)/1023.0; |
ClementBreteau | 1:f3f702086a30 | 841 | |
ClementBreteau | 1:f3f702086a30 | 842 | return (angle); |
ClementBreteau | 1:f3f702086a30 | 843 | } |
ClementBreteau | 1:f3f702086a30 | 844 | |
ClementBreteau | 1:f3f702086a30 | 845 | |
ClementBreteau | 1:f3f702086a30 | 846 | float AX12::Get_Temp () |
ClementBreteau | 1:f3f702086a30 | 847 | { |
ClementBreteau | 1:f3f702086a30 | 848 | |
ClementBreteau | 1:f3f702086a30 | 849 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 850 | printf("\nGetTemp(%d)",_ID); |
ClementBreteau | 1:f3f702086a30 | 851 | #endif |
ClementBreteau | 1:f3f702086a30 | 852 | |
ClementBreteau | 1:f3f702086a30 | 853 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 854 | int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); |
ClementBreteau | 1:f3f702086a30 | 855 | float temp = data[0]; |
ClementBreteau | 1:f3f702086a30 | 856 | return(temp); |
ClementBreteau | 1:f3f702086a30 | 857 | } |
ClementBreteau | 1:f3f702086a30 | 858 | |
ClementBreteau | 1:f3f702086a30 | 859 | |
ClementBreteau | 1:f3f702086a30 | 860 | float AX12::Get_Volts (void) |
ClementBreteau | 1:f3f702086a30 | 861 | { |
ClementBreteau | 1:f3f702086a30 | 862 | |
ClementBreteau | 1:f3f702086a30 | 863 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 864 | printf("\nGetVolts(%d)",_ID); |
ClementBreteau | 1:f3f702086a30 | 865 | #endif |
ClementBreteau | 1:f3f702086a30 | 866 | |
ClementBreteau | 1:f3f702086a30 | 867 | char data[1]; |
ClementBreteau | 1:f3f702086a30 | 868 | int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); |
ClementBreteau | 1:f3f702086a30 | 869 | float volts = data[0]/10.0; |
ClementBreteau | 1:f3f702086a30 | 870 | return(volts); |
ClementBreteau | 1:f3f702086a30 | 871 | } |
ClementBreteau | 1:f3f702086a30 | 872 | |
ClementBreteau | 1:f3f702086a30 | 873 | |
ClementBreteau | 1:f3f702086a30 | 874 | int AX12::read(int ID, int start, int bytes, char* data) { |
ClementBreteau | 1:f3f702086a30 | 875 | |
ClementBreteau | 1:f3f702086a30 | 876 | |
ClementBreteau | 1:f3f702086a30 | 877 | char PacketLength = 0x3; |
ClementBreteau | 1:f3f702086a30 | 878 | char TxBuf[16]; |
ClementBreteau | 1:f3f702086a30 | 879 | char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 880 | char Status[16]; |
ClementBreteau | 1:f3f702086a30 | 881 | |
ClementBreteau | 1:f3f702086a30 | 882 | int timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 883 | int plen = 0; |
ClementBreteau | 1:f3f702086a30 | 884 | int flag_out = 0; |
ClementBreteau | 1:f3f702086a30 | 885 | int timeout_transmit = 0; |
ClementBreteau | 1:f3f702086a30 | 886 | int i = 0; |
ClementBreteau | 1:f3f702086a30 | 887 | int enable = 0; |
ClementBreteau | 1:f3f702086a30 | 888 | // int poubelle = 0; |
ClementBreteau | 1:f3f702086a30 | 889 | // int count = 0; |
ClementBreteau | 1:f3f702086a30 | 890 | // char vidage[50]; |
ClementBreteau | 1:f3f702086a30 | 891 | |
ClementBreteau | 1:f3f702086a30 | 892 | typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat; |
ClementBreteau | 1:f3f702086a30 | 893 | type_etat etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 894 | |
ClementBreteau | 1:f3f702086a30 | 895 | Status[4] = 0xFE; // return code |
ClementBreteau | 1:f3f702086a30 | 896 | |
ClementBreteau | 1:f3f702086a30 | 897 | |
ClementBreteau | 1:f3f702086a30 | 898 | |
ClementBreteau | 1:f3f702086a30 | 899 | |
ClementBreteau | 1:f3f702086a30 | 900 | |
ClementBreteau | 1:f3f702086a30 | 901 | /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/ |
ClementBreteau | 1:f3f702086a30 | 902 | |
ClementBreteau | 1:f3f702086a30 | 903 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 904 | printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); |
ClementBreteau | 1:f3f702086a30 | 905 | #endif |
ClementBreteau | 1:f3f702086a30 | 906 | |
ClementBreteau | 1:f3f702086a30 | 907 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 908 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 909 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
ClementBreteau | 1:f3f702086a30 | 910 | #endif |
ClementBreteau | 1:f3f702086a30 | 911 | |
ClementBreteau | 1:f3f702086a30 | 912 | TxBuf[0] = 0xff; |
ClementBreteau | 1:f3f702086a30 | 913 | TxBuf[1] = 0xff; |
ClementBreteau | 1:f3f702086a30 | 914 | |
ClementBreteau | 1:f3f702086a30 | 915 | // ID |
ClementBreteau | 1:f3f702086a30 | 916 | TxBuf[2] = ID; |
ClementBreteau | 1:f3f702086a30 | 917 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 918 | |
ClementBreteau | 1:f3f702086a30 | 919 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 920 | printf(" ID : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 921 | #endif |
ClementBreteau | 1:f3f702086a30 | 922 | |
ClementBreteau | 1:f3f702086a30 | 923 | // Packet Length |
ClementBreteau | 1:f3f702086a30 | 924 | TxBuf[3] = 4;//PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
ClementBreteau | 1:f3f702086a30 | 925 | sum += TxBuf[3]; // Accululate the packet sum |
ClementBreteau | 1:f3f702086a30 | 926 | |
ClementBreteau | 1:f3f702086a30 | 927 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 928 | printf(" Length : 0x%x\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 929 | #endif |
ClementBreteau | 1:f3f702086a30 | 930 | |
ClementBreteau | 1:f3f702086a30 | 931 | // Instruction - Read |
ClementBreteau | 1:f3f702086a30 | 932 | TxBuf[4] = 0x2; |
ClementBreteau | 1:f3f702086a30 | 933 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 934 | |
ClementBreteau | 1:f3f702086a30 | 935 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 936 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
ClementBreteau | 1:f3f702086a30 | 937 | #endif |
ClementBreteau | 1:f3f702086a30 | 938 | |
ClementBreteau | 1:f3f702086a30 | 939 | // Start Address |
ClementBreteau | 1:f3f702086a30 | 940 | TxBuf[5] = start; |
ClementBreteau | 1:f3f702086a30 | 941 | sum += TxBuf[5]; |
ClementBreteau | 1:f3f702086a30 | 942 | |
ClementBreteau | 1:f3f702086a30 | 943 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 944 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 945 | #endif |
ClementBreteau | 1:f3f702086a30 | 946 | |
ClementBreteau | 1:f3f702086a30 | 947 | // Bytes to read |
ClementBreteau | 1:f3f702086a30 | 948 | TxBuf[6] = bytes; |
ClementBreteau | 1:f3f702086a30 | 949 | sum += TxBuf[6]; |
ClementBreteau | 1:f3f702086a30 | 950 | |
ClementBreteau | 1:f3f702086a30 | 951 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 952 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
ClementBreteau | 1:f3f702086a30 | 953 | #endif |
ClementBreteau | 1:f3f702086a30 | 954 | |
ClementBreteau | 1:f3f702086a30 | 955 | // Checksum |
ClementBreteau | 1:f3f702086a30 | 956 | TxBuf[7] = 0xFF - sum; |
ClementBreteau | 1:f3f702086a30 | 957 | #ifdef AX12_READ_DEBUG |
ClementBreteau | 1:f3f702086a30 | 958 | printf(" Checksum : 0x%x\n",TxBuf[7]); |
ClementBreteau | 1:f3f702086a30 | 959 | #endif |
ClementBreteau | 1:f3f702086a30 | 960 | /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/ |
ClementBreteau | 1:f3f702086a30 | 961 | |
ClementBreteau | 1:f3f702086a30 | 962 | |
ClementBreteau | 1:f3f702086a30 | 963 | |
ClementBreteau | 1:f3f702086a30 | 964 | |
ClementBreteau | 1:f3f702086a30 | 965 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
ClementBreteau | 1:f3f702086a30 | 966 | */ |
ClementBreteau | 1:f3f702086a30 | 967 | while ((timeout_transmit<5000) && (i < (7+bytes))) |
ClementBreteau | 1:f3f702086a30 | 968 | { |
ClementBreteau | 1:f3f702086a30 | 969 | if (_ax12.writeable()) |
ClementBreteau | 1:f3f702086a30 | 970 | { |
ClementBreteau | 1:f3f702086a30 | 971 | _ax12.putc(TxBuf[i]); |
ClementBreteau | 1:f3f702086a30 | 972 | i++; |
ClementBreteau | 1:f3f702086a30 | 973 | timeout_transmit = 0; |
ClementBreteau | 1:f3f702086a30 | 974 | } |
ClementBreteau | 1:f3f702086a30 | 975 | else timeout_transmit++; |
ClementBreteau | 1:f3f702086a30 | 976 | } |
ClementBreteau | 1:f3f702086a30 | 977 | |
ClementBreteau | 1:f3f702086a30 | 978 | if (timeout_transmit == 5000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
ClementBreteau | 1:f3f702086a30 | 979 | { |
ClementBreteau | 1:f3f702086a30 | 980 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 981 | printf ("timeout transmit erreur\r\n"); |
ClementBreteau | 1:f3f702086a30 | 982 | #endif |
ClementBreteau | 1:f3f702086a30 | 983 | return(-1); |
ClementBreteau | 1:f3f702086a30 | 984 | } |
ClementBreteau | 1:f3f702086a30 | 985 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
ClementBreteau | 1:f3f702086a30 | 986 | */ |
ClementBreteau | 1:f3f702086a30 | 987 | |
ClementBreteau | 1:f3f702086a30 | 988 | |
ClementBreteau | 1:f3f702086a30 | 989 | // Wait for the bytes to be transmitted |
ClementBreteau | 1:f3f702086a30 | 990 | wait (0.001); |
ClementBreteau | 1:f3f702086a30 | 991 | |
ClementBreteau | 1:f3f702086a30 | 992 | |
ClementBreteau | 1:f3f702086a30 | 993 | |
ClementBreteau | 1:f3f702086a30 | 994 | // Skip if the read was to the broadcast address |
ClementBreteau | 1:f3f702086a30 | 995 | if (_ID != 0xFE) { |
ClementBreteau | 1:f3f702086a30 | 996 | |
ClementBreteau | 1:f3f702086a30 | 997 | |
ClementBreteau | 1:f3f702086a30 | 998 | |
ClementBreteau | 1:f3f702086a30 | 999 | /* Partie de reception de la trame de retour |
ClementBreteau | 1:f3f702086a30 | 1000 | */ |
ClementBreteau | 1:f3f702086a30 | 1001 | while ((flag_out != 1) && (timeout < (1000*bytes))) |
ClementBreteau | 1:f3f702086a30 | 1002 | { |
ClementBreteau | 1:f3f702086a30 | 1003 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
ClementBreteau | 1:f3f702086a30 | 1004 | switch (etat) |
ClementBreteau | 1:f3f702086a30 | 1005 | { |
ClementBreteau | 1:f3f702086a30 | 1006 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1007 | { |
ClementBreteau | 1:f3f702086a30 | 1008 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1009 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1010 | if (Status[plen] == 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1011 | { |
ClementBreteau | 1:f3f702086a30 | 1012 | etat = Header2; |
ClementBreteau | 1:f3f702086a30 | 1013 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1014 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1015 | #endif |
ClementBreteau | 1:f3f702086a30 | 1016 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1017 | |
ClementBreteau | 1:f3f702086a30 | 1018 | } |
ClementBreteau | 1:f3f702086a30 | 1019 | else etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1020 | } |
ClementBreteau | 1:f3f702086a30 | 1021 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1022 | break; |
ClementBreteau | 1:f3f702086a30 | 1023 | |
ClementBreteau | 1:f3f702086a30 | 1024 | |
ClementBreteau | 1:f3f702086a30 | 1025 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1026 | { |
ClementBreteau | 1:f3f702086a30 | 1027 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1028 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1029 | if (Status[plen] == 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1030 | { |
ClementBreteau | 1:f3f702086a30 | 1031 | etat = ident; |
ClementBreteau | 1:f3f702086a30 | 1032 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1033 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1034 | #endif |
ClementBreteau | 1:f3f702086a30 | 1035 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1036 | |
ClementBreteau | 1:f3f702086a30 | 1037 | } |
ClementBreteau | 1:f3f702086a30 | 1038 | else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!! |
ClementBreteau | 1:f3f702086a30 | 1039 | { |
ClementBreteau | 1:f3f702086a30 | 1040 | Status[plen] = 0; |
ClementBreteau | 1:f3f702086a30 | 1041 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1042 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1043 | #endif |
ClementBreteau | 1:f3f702086a30 | 1044 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1045 | Status[plen] = ID; |
ClementBreteau | 1:f3f702086a30 | 1046 | etat = length; |
ClementBreteau | 1:f3f702086a30 | 1047 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1048 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1049 | #endif |
ClementBreteau | 1:f3f702086a30 | 1050 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1051 | |
ClementBreteau | 1:f3f702086a30 | 1052 | } |
ClementBreteau | 1:f3f702086a30 | 1053 | else |
ClementBreteau | 1:f3f702086a30 | 1054 | { |
ClementBreteau | 1:f3f702086a30 | 1055 | |
ClementBreteau | 1:f3f702086a30 | 1056 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1057 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1058 | } |
ClementBreteau | 1:f3f702086a30 | 1059 | } |
ClementBreteau | 1:f3f702086a30 | 1060 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1061 | break; |
ClementBreteau | 1:f3f702086a30 | 1062 | |
ClementBreteau | 1:f3f702086a30 | 1063 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
ClementBreteau | 1:f3f702086a30 | 1064 | { |
ClementBreteau | 1:f3f702086a30 | 1065 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1066 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1067 | if (Status[plen] == ID ) |
ClementBreteau | 1:f3f702086a30 | 1068 | { |
ClementBreteau | 1:f3f702086a30 | 1069 | etat = length; |
ClementBreteau | 1:f3f702086a30 | 1070 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1071 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1072 | #endif |
ClementBreteau | 1:f3f702086a30 | 1073 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1074 | |
ClementBreteau | 1:f3f702086a30 | 1075 | } |
ClementBreteau | 1:f3f702086a30 | 1076 | else |
ClementBreteau | 1:f3f702086a30 | 1077 | { |
ClementBreteau | 1:f3f702086a30 | 1078 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1079 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1080 | } |
ClementBreteau | 1:f3f702086a30 | 1081 | } |
ClementBreteau | 1:f3f702086a30 | 1082 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1083 | break; |
ClementBreteau | 1:f3f702086a30 | 1084 | |
ClementBreteau | 1:f3f702086a30 | 1085 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
ClementBreteau | 1:f3f702086a30 | 1086 | { |
ClementBreteau | 1:f3f702086a30 | 1087 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1088 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1089 | if (Status[plen] == (bytes+2) ) |
ClementBreteau | 1:f3f702086a30 | 1090 | { |
ClementBreteau | 1:f3f702086a30 | 1091 | etat = erreur; |
ClementBreteau | 1:f3f702086a30 | 1092 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1093 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1094 | #endif |
ClementBreteau | 1:f3f702086a30 | 1095 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1096 | |
ClementBreteau | 1:f3f702086a30 | 1097 | } |
ClementBreteau | 1:f3f702086a30 | 1098 | else |
ClementBreteau | 1:f3f702086a30 | 1099 | { |
ClementBreteau | 1:f3f702086a30 | 1100 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1101 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1102 | } |
ClementBreteau | 1:f3f702086a30 | 1103 | } |
ClementBreteau | 1:f3f702086a30 | 1104 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1105 | break; |
ClementBreteau | 1:f3f702086a30 | 1106 | |
ClementBreteau | 1:f3f702086a30 | 1107 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
ClementBreteau | 1:f3f702086a30 | 1108 | { |
ClementBreteau | 1:f3f702086a30 | 1109 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1110 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1111 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1112 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1113 | #endif |
ClementBreteau | 1:f3f702086a30 | 1114 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1115 | |
ClementBreteau | 1:f3f702086a30 | 1116 | etat = reception; |
ClementBreteau | 1:f3f702086a30 | 1117 | } |
ClementBreteau | 1:f3f702086a30 | 1118 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1119 | |
ClementBreteau | 1:f3f702086a30 | 1120 | case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length ) |
ClementBreteau | 1:f3f702086a30 | 1121 | { |
ClementBreteau | 1:f3f702086a30 | 1122 | if (_ax12.readable()) |
ClementBreteau | 1:f3f702086a30 | 1123 | { |
ClementBreteau | 1:f3f702086a30 | 1124 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1125 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1126 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1127 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1128 | #endif |
ClementBreteau | 1:f3f702086a30 | 1129 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1130 | enable++; |
ClementBreteau | 1:f3f702086a30 | 1131 | |
ClementBreteau | 1:f3f702086a30 | 1132 | } |
ClementBreteau | 1:f3f702086a30 | 1133 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1134 | } |
ClementBreteau | 1:f3f702086a30 | 1135 | etat = checksum; |
ClementBreteau | 1:f3f702086a30 | 1136 | break; |
ClementBreteau | 1:f3f702086a30 | 1137 | |
ClementBreteau | 1:f3f702086a30 | 1138 | case checksum: if (_ax12.readable()) // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!! |
ClementBreteau | 1:f3f702086a30 | 1139 | { |
ClementBreteau | 1:f3f702086a30 | 1140 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1141 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1142 | flag_out = 1; |
ClementBreteau | 1:f3f702086a30 | 1143 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1144 | |
ClementBreteau | 1:f3f702086a30 | 1145 | #ifdef AX12_DEBUG_READ |
ClementBreteau | 1:f3f702086a30 | 1146 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1147 | #endif |
ClementBreteau | 1:f3f702086a30 | 1148 | } |
ClementBreteau | 1:f3f702086a30 | 1149 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1150 | break; |
ClementBreteau | 1:f3f702086a30 | 1151 | |
ClementBreteau | 1:f3f702086a30 | 1152 | default: break; |
ClementBreteau | 1:f3f702086a30 | 1153 | } |
ClementBreteau | 1:f3f702086a30 | 1154 | } |
ClementBreteau | 1:f3f702086a30 | 1155 | |
ClementBreteau | 1:f3f702086a30 | 1156 | |
ClementBreteau | 1:f3f702086a30 | 1157 | if (timeout == (1000*bytes) ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames |
ClementBreteau | 1:f3f702086a30 | 1158 | { |
ClementBreteau | 1:f3f702086a30 | 1159 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1160 | printf ("timeout erreur\n"); |
ClementBreteau | 1:f3f702086a30 | 1161 | #endif |
ClementBreteau | 1:f3f702086a30 | 1162 | return(-1); |
ClementBreteau | 1:f3f702086a30 | 1163 | } |
ClementBreteau | 1:f3f702086a30 | 1164 | |
ClementBreteau | 1:f3f702086a30 | 1165 | |
ClementBreteau | 1:f3f702086a30 | 1166 | // copie des données dans le tableau data |
ClementBreteau | 1:f3f702086a30 | 1167 | for (int i=0; i < Status[3]-2 ; i++) |
ClementBreteau | 1:f3f702086a30 | 1168 | { |
ClementBreteau | 1:f3f702086a30 | 1169 | data[i] = Status[5+i]; |
ClementBreteau | 1:f3f702086a30 | 1170 | } |
ClementBreteau | 1:f3f702086a30 | 1171 | |
ClementBreteau | 1:f3f702086a30 | 1172 | } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE) |
ClementBreteau | 1:f3f702086a30 | 1173 | |
ClementBreteau | 1:f3f702086a30 | 1174 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
ClementBreteau | 1:f3f702086a30 | 1175 | } |
ClementBreteau | 1:f3f702086a30 | 1176 | |
ClementBreteau | 1:f3f702086a30 | 1177 | void AX12::multiple_goal_and_speed(int number_ax12,char* tab) |
ClementBreteau | 1:f3f702086a30 | 1178 | { |
ClementBreteau | 1:f3f702086a30 | 1179 | char TxBuf[50]; |
ClementBreteau | 1:f3f702086a30 | 1180 | char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 1181 | int timeout_transmit =0; |
ClementBreteau | 1:f3f702086a30 | 1182 | int i=0, k=0, j=0; |
ClementBreteau | 1:f3f702086a30 | 1183 | int L=4; // nombre instructions par paquets |
ClementBreteau | 1:f3f702086a30 | 1184 | int bytes= ((L+1)*number_ax12)+4; |
ClementBreteau | 1:f3f702086a30 | 1185 | |
ClementBreteau | 1:f3f702086a30 | 1186 | typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; |
ClementBreteau | 1:f3f702086a30 | 1187 | type_etat etat= Header1; |
ClementBreteau | 1:f3f702086a30 | 1188 | |
ClementBreteau | 1:f3f702086a30 | 1189 | for(j=0; j<50; j++) |
ClementBreteau | 1:f3f702086a30 | 1190 | { |
ClementBreteau | 1:f3f702086a30 | 1191 | TxBuf[i]=0; |
ClementBreteau | 1:f3f702086a30 | 1192 | } |
ClementBreteau | 1:f3f702086a30 | 1193 | |
ClementBreteau | 1:f3f702086a30 | 1194 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1195 | //printf(" MULTIPLE_GOAL_AND_SPEED \n "); |
ClementBreteau | 1:f3f702086a30 | 1196 | #endif |
ClementBreteau | 1:f3f702086a30 | 1197 | |
ClementBreteau | 1:f3f702086a30 | 1198 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 1199 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1200 | //printf("\nInstruction Packet\n Header :%d, %d\n",TxBuf[0], TxBuf[1]); |
ClementBreteau | 1:f3f702086a30 | 1201 | #endif |
ClementBreteau | 1:f3f702086a30 | 1202 | |
ClementBreteau | 1:f3f702086a30 | 1203 | TxBuf[0]=0xFF; // bit de start |
ClementBreteau | 1:f3f702086a30 | 1204 | TxBuf[1]=0xFF; // bit de start |
ClementBreteau | 1:f3f702086a30 | 1205 | |
ClementBreteau | 1:f3f702086a30 | 1206 | TxBuf[2] = 0xFE; //ID broadcast |
ClementBreteau | 1:f3f702086a30 | 1207 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 1208 | |
ClementBreteau | 1:f3f702086a30 | 1209 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1210 | printf(" adresse de difusion : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 1211 | #endif |
ClementBreteau | 1:f3f702086a30 | 1212 | |
ClementBreteau | 1:f3f702086a30 | 1213 | TxBuf[3] =bytes; // longueur |
ClementBreteau | 1:f3f702086a30 | 1214 | sum += TxBuf[3]; |
ClementBreteau | 1:f3f702086a30 | 1215 | |
ClementBreteau | 1:f3f702086a30 | 1216 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1217 | printf(" Longueur : %d\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 1218 | #endif |
ClementBreteau | 1:f3f702086a30 | 1219 | |
ClementBreteau | 1:f3f702086a30 | 1220 | TxBuf[4]=0x83; //SYNC_WRITE |
ClementBreteau | 1:f3f702086a30 | 1221 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 1222 | |
ClementBreteau | 1:f3f702086a30 | 1223 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1224 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
ClementBreteau | 1:f3f702086a30 | 1225 | #endif |
ClementBreteau | 1:f3f702086a30 | 1226 | |
ClementBreteau | 1:f3f702086a30 | 1227 | TxBuf[5] = 0x1E; // addresse "GOAL_POSITION" |
ClementBreteau | 1:f3f702086a30 | 1228 | sum += TxBuf[5]; |
ClementBreteau | 1:f3f702086a30 | 1229 | |
ClementBreteau | 1:f3f702086a30 | 1230 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1231 | printf(" Adresse de debut : 0x%x\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 1232 | #endif |
ClementBreteau | 1:f3f702086a30 | 1233 | |
ClementBreteau | 1:f3f702086a30 | 1234 | TxBuf[6]=L; // Nombre instruction par paquets |
ClementBreteau | 1:f3f702086a30 | 1235 | sum += TxBuf[6]; |
ClementBreteau | 1:f3f702086a30 | 1236 | |
ClementBreteau | 1:f3f702086a30 | 1237 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1238 | printf(" nombre instruction/paquet : 0x%x\n",TxBuf[6]); |
ClementBreteau | 1:f3f702086a30 | 1239 | #endif |
ClementBreteau | 1:f3f702086a30 | 1240 | |
ClementBreteau | 1:f3f702086a30 | 1241 | for(i=0; i<(number_ax12*5); i++) // Copie des data de TAB sur TxBuf |
ClementBreteau | 1:f3f702086a30 | 1242 | { |
ClementBreteau | 1:f3f702086a30 | 1243 | |
ClementBreteau | 1:f3f702086a30 | 1244 | TxBuf[i+7]=tab[i]; |
ClementBreteau | 1:f3f702086a30 | 1245 | sum += TxBuf[i+7]; |
ClementBreteau | 1:f3f702086a30 | 1246 | |
ClementBreteau | 1:f3f702086a30 | 1247 | } |
ClementBreteau | 1:f3f702086a30 | 1248 | |
ClementBreteau | 1:f3f702086a30 | 1249 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1250 | for(i=0; i<(number_ax12*5); i++) |
ClementBreteau | 1:f3f702086a30 | 1251 | { |
ClementBreteau | 1:f3f702086a30 | 1252 | |
ClementBreteau | 1:f3f702086a30 | 1253 | printf(" Data : 0x%x\n",TxBuf[i+7]); |
ClementBreteau | 1:f3f702086a30 | 1254 | |
ClementBreteau | 1:f3f702086a30 | 1255 | } |
ClementBreteau | 1:f3f702086a30 | 1256 | #endif |
ClementBreteau | 1:f3f702086a30 | 1257 | |
ClementBreteau | 1:f3f702086a30 | 1258 | TxBuf[(number_ax12*5)+7] = 0xFF - sum ; // CHECKSUM |
ClementBreteau | 1:f3f702086a30 | 1259 | |
ClementBreteau | 1:f3f702086a30 | 1260 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1261 | printf(" Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]); |
ClementBreteau | 1:f3f702086a30 | 1262 | #endif |
ClementBreteau | 1:f3f702086a30 | 1263 | |
ClementBreteau | 1:f3f702086a30 | 1264 | for(k=0; k<((number_ax12*5)+8); k++) // TRANSMISSION DE LA TRAME |
ClementBreteau | 1:f3f702086a30 | 1265 | { |
ClementBreteau | 1:f3f702086a30 | 1266 | _ax12.putc(TxBuf[k]); |
ClementBreteau | 1:f3f702086a30 | 1267 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1268 | printf(" transmission : 0x%x\n",TxBuf[k]); |
ClementBreteau | 1:f3f702086a30 | 1269 | #endif |
ClementBreteau | 1:f3f702086a30 | 1270 | } |
ClementBreteau | 1:f3f702086a30 | 1271 | |
ClementBreteau | 1:f3f702086a30 | 1272 | |
ClementBreteau | 1:f3f702086a30 | 1273 | } |
ClementBreteau | 1:f3f702086a30 | 1274 | |
ClementBreteau | 1:f3f702086a30 | 1275 | float AX12::read_and_test(float angle,char* Tab) |
ClementBreteau | 1:f3f702086a30 | 1276 | { |
ClementBreteau | 1:f3f702086a30 | 1277 | int k=0; |
ClementBreteau | 1:f3f702086a30 | 1278 | unsigned short val_angle=0, val_reche=0; |
ClementBreteau | 1:f3f702086a30 | 1279 | |
ClementBreteau | 1:f3f702086a30 | 1280 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1281 | printf("\nread_and_test"); |
ClementBreteau | 1:f3f702086a30 | 1282 | #endif |
ClementBreteau | 1:f3f702086a30 | 1283 | |
ClementBreteau | 1:f3f702086a30 | 1284 | if( _ID==0x12) |
ClementBreteau | 1:f3f702086a30 | 1285 | { k=1;} |
ClementBreteau | 1:f3f702086a30 | 1286 | else if( _ID==0x04) |
ClementBreteau | 1:f3f702086a30 | 1287 | { k=6;} |
ClementBreteau | 1:f3f702086a30 | 1288 | else if( _ID==0x07) |
ClementBreteau | 1:f3f702086a30 | 1289 | { k=11;} |
ClementBreteau | 1:f3f702086a30 | 1290 | else if( _ID==0x0F) |
ClementBreteau | 1:f3f702086a30 | 1291 | { k=16;} |
ClementBreteau | 1:f3f702086a30 | 1292 | |
ClementBreteau | 1:f3f702086a30 | 1293 | val_angle = (unsigned short) (angle/0.3); |
ClementBreteau | 1:f3f702086a30 | 1294 | val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8); |
ClementBreteau | 1:f3f702086a30 | 1295 | |
ClementBreteau | 1:f3f702086a30 | 1296 | if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28)))) |
ClementBreteau | 1:f3f702086a30 | 1297 | { |
ClementBreteau | 1:f3f702086a30 | 1298 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1299 | printf("\nreturn1"); |
ClementBreteau | 1:f3f702086a30 | 1300 | #endif |
ClementBreteau | 1:f3f702086a30 | 1301 | return 1; |
ClementBreteau | 1:f3f702086a30 | 1302 | } |
ClementBreteau | 1:f3f702086a30 | 1303 | else |
ClementBreteau | 1:f3f702086a30 | 1304 | { |
ClementBreteau | 1:f3f702086a30 | 1305 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1306 | printf("\nreturn0"); |
ClementBreteau | 1:f3f702086a30 | 1307 | #endif |
ClementBreteau | 1:f3f702086a30 | 1308 | return 0; |
ClementBreteau | 1:f3f702086a30 | 1309 | } |
ClementBreteau | 1:f3f702086a30 | 1310 | |
ClementBreteau | 1:f3f702086a30 | 1311 | } |
ClementBreteau | 1:f3f702086a30 | 1312 | |
ClementBreteau | 1:f3f702086a30 | 1313 | int AX12::write(int ID, int start, int bytes, char* data, int flag) |
ClementBreteau | 1:f3f702086a30 | 1314 | { |
ClementBreteau | 1:f3f702086a30 | 1315 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
ClementBreteau | 1:f3f702086a30 | 1316 | |
ClementBreteau | 1:f3f702086a30 | 1317 | char TxBuf[16]; |
ClementBreteau | 1:f3f702086a30 | 1318 | char sum = 0; |
ClementBreteau | 1:f3f702086a30 | 1319 | char Status[6]; |
ClementBreteau | 1:f3f702086a30 | 1320 | |
ClementBreteau | 1:f3f702086a30 | 1321 | int timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1322 | int plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1323 | int flag_out = 0; |
ClementBreteau | 1:f3f702086a30 | 1324 | int timeout_transmit = 0; |
ClementBreteau | 1:f3f702086a30 | 1325 | int i = 0; |
matthieuvignon | 3:43843ab8af47 | 1326 | //int poubelle = 0; |
ClementBreteau | 1:f3f702086a30 | 1327 | int count = 0; |
ClementBreteau | 1:f3f702086a30 | 1328 | char vidage[50]; |
ClementBreteau | 1:f3f702086a30 | 1329 | |
ClementBreteau | 1:f3f702086a30 | 1330 | typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; |
ClementBreteau | 1:f3f702086a30 | 1331 | type_etat etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1332 | |
ClementBreteau | 1:f3f702086a30 | 1333 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1334 | printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); |
ClementBreteau | 1:f3f702086a30 | 1335 | #endif |
ClementBreteau | 1:f3f702086a30 | 1336 | |
ClementBreteau | 1:f3f702086a30 | 1337 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 1338 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1339 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
ClementBreteau | 1:f3f702086a30 | 1340 | #endif |
ClementBreteau | 1:f3f702086a30 | 1341 | |
ClementBreteau | 1:f3f702086a30 | 1342 | TxBuf[0] = 0xff; |
ClementBreteau | 1:f3f702086a30 | 1343 | TxBuf[1] = 0xff; |
ClementBreteau | 1:f3f702086a30 | 1344 | |
ClementBreteau | 1:f3f702086a30 | 1345 | // ID |
ClementBreteau | 1:f3f702086a30 | 1346 | TxBuf[2] = ID; |
ClementBreteau | 1:f3f702086a30 | 1347 | sum += TxBuf[2]; |
ClementBreteau | 1:f3f702086a30 | 1348 | |
ClementBreteau | 1:f3f702086a30 | 1349 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1350 | printf(" ID : %d\n",TxBuf[2]); |
ClementBreteau | 1:f3f702086a30 | 1351 | #endif |
ClementBreteau | 1:f3f702086a30 | 1352 | |
ClementBreteau | 1:f3f702086a30 | 1353 | // packet Length |
ClementBreteau | 1:f3f702086a30 | 1354 | TxBuf[3] = 3+bytes; |
ClementBreteau | 1:f3f702086a30 | 1355 | sum += TxBuf[3]; |
ClementBreteau | 1:f3f702086a30 | 1356 | |
ClementBreteau | 1:f3f702086a30 | 1357 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1358 | printf(" Length : %d\n",TxBuf[3]); |
ClementBreteau | 1:f3f702086a30 | 1359 | #endif |
ClementBreteau | 1:f3f702086a30 | 1360 | |
ClementBreteau | 1:f3f702086a30 | 1361 | // Instruction |
ClementBreteau | 1:f3f702086a30 | 1362 | if (flag == 1) { |
ClementBreteau | 1:f3f702086a30 | 1363 | TxBuf[4]=0x04; |
ClementBreteau | 1:f3f702086a30 | 1364 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 1365 | } else { |
ClementBreteau | 1:f3f702086a30 | 1366 | TxBuf[4]=0x03; |
ClementBreteau | 1:f3f702086a30 | 1367 | sum += TxBuf[4]; |
ClementBreteau | 1:f3f702086a30 | 1368 | } |
ClementBreteau | 1:f3f702086a30 | 1369 | |
ClementBreteau | 1:f3f702086a30 | 1370 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1371 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
ClementBreteau | 1:f3f702086a30 | 1372 | #endif |
ClementBreteau | 1:f3f702086a30 | 1373 | |
ClementBreteau | 1:f3f702086a30 | 1374 | // Start Address |
ClementBreteau | 1:f3f702086a30 | 1375 | TxBuf[5] = start; |
ClementBreteau | 1:f3f702086a30 | 1376 | sum += TxBuf[5]; |
ClementBreteau | 1:f3f702086a30 | 1377 | |
ClementBreteau | 1:f3f702086a30 | 1378 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1379 | printf(" Start : 0x%x\n",TxBuf[5]); |
ClementBreteau | 1:f3f702086a30 | 1380 | #endif |
ClementBreteau | 1:f3f702086a30 | 1381 | |
ClementBreteau | 1:f3f702086a30 | 1382 | // data |
ClementBreteau | 1:f3f702086a30 | 1383 | for (char i=0; i<bytes ; i++) { |
ClementBreteau | 1:f3f702086a30 | 1384 | TxBuf[6+i] = data[i]; |
ClementBreteau | 1:f3f702086a30 | 1385 | sum += TxBuf[6+i]; |
ClementBreteau | 1:f3f702086a30 | 1386 | |
ClementBreteau | 1:f3f702086a30 | 1387 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1388 | printf(" Data : 0x%x\n",TxBuf[6+i]); |
ClementBreteau | 1:f3f702086a30 | 1389 | #endif |
ClementBreteau | 1:f3f702086a30 | 1390 | |
ClementBreteau | 1:f3f702086a30 | 1391 | } |
ClementBreteau | 1:f3f702086a30 | 1392 | |
ClementBreteau | 1:f3f702086a30 | 1393 | // checksum |
ClementBreteau | 1:f3f702086a30 | 1394 | TxBuf[6+bytes] = 0xFF - sum; |
ClementBreteau | 1:f3f702086a30 | 1395 | |
ClementBreteau | 1:f3f702086a30 | 1396 | #ifdef AX12_WRITE_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1397 | printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); |
ClementBreteau | 1:f3f702086a30 | 1398 | #endif |
ClementBreteau | 1:f3f702086a30 | 1399 | |
ClementBreteau | 1:f3f702086a30 | 1400 | |
ClementBreteau | 1:f3f702086a30 | 1401 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
ClementBreteau | 1:f3f702086a30 | 1402 | */ |
ClementBreteau | 1:f3f702086a30 | 1403 | while ((timeout_transmit<100) && (i < (7+bytes))) |
ClementBreteau | 1:f3f702086a30 | 1404 | { |
ClementBreteau | 1:f3f702086a30 | 1405 | if (_ax12.writeable()) |
ClementBreteau | 1:f3f702086a30 | 1406 | { |
ClementBreteau | 1:f3f702086a30 | 1407 | _ax12.putc(TxBuf[i]); |
ClementBreteau | 1:f3f702086a30 | 1408 | i++; |
ClementBreteau | 1:f3f702086a30 | 1409 | timeout_transmit = 0; |
ClementBreteau | 1:f3f702086a30 | 1410 | } |
ClementBreteau | 1:f3f702086a30 | 1411 | else timeout_transmit++; |
ClementBreteau | 1:f3f702086a30 | 1412 | } |
ClementBreteau | 1:f3f702086a30 | 1413 | |
ClementBreteau | 1:f3f702086a30 | 1414 | if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
ClementBreteau | 1:f3f702086a30 | 1415 | { |
ClementBreteau | 1:f3f702086a30 | 1416 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1417 | printf ("TIMEOUT TRANSMIT ERROR\r\n"); |
ClementBreteau | 1:f3f702086a30 | 1418 | #endif |
ClementBreteau | 1:f3f702086a30 | 1419 | return(-1); |
ClementBreteau | 1:f3f702086a30 | 1420 | } |
ClementBreteau | 1:f3f702086a30 | 1421 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
ClementBreteau | 1:f3f702086a30 | 1422 | */ |
ClementBreteau | 1:f3f702086a30 | 1423 | |
ClementBreteau | 1:f3f702086a30 | 1424 | |
ClementBreteau | 1:f3f702086a30 | 1425 | // Wait for data to transmit |
ClementBreteau | 1:f3f702086a30 | 1426 | wait (0.005); |
ClementBreteau | 1:f3f702086a30 | 1427 | |
ClementBreteau | 1:f3f702086a30 | 1428 | // make sure we have a valid return |
ClementBreteau | 1:f3f702086a30 | 1429 | Status[4]=0x00; |
ClementBreteau | 1:f3f702086a30 | 1430 | |
ClementBreteau | 1:f3f702086a30 | 1431 | // we'll only get a reply if it was not broadcast |
ClementBreteau | 1:f3f702086a30 | 1432 | if (_ID!=0xFE) { |
ClementBreteau | 1:f3f702086a30 | 1433 | |
ClementBreteau | 1:f3f702086a30 | 1434 | |
ClementBreteau | 1:f3f702086a30 | 1435 | /* Partie de reception de la trame de retour |
ClementBreteau | 1:f3f702086a30 | 1436 | */ |
ClementBreteau | 1:f3f702086a30 | 1437 | while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) |
ClementBreteau | 1:f3f702086a30 | 1438 | { |
ClementBreteau | 1:f3f702086a30 | 1439 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
ClementBreteau | 1:f3f702086a30 | 1440 | switch (etat) |
ClementBreteau | 1:f3f702086a30 | 1441 | { |
ClementBreteau | 1:f3f702086a30 | 1442 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1443 | { |
ClementBreteau | 1:f3f702086a30 | 1444 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1445 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1446 | if (Status[plen] == 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1447 | { |
ClementBreteau | 1:f3f702086a30 | 1448 | etat = Header2; |
ClementBreteau | 1:f3f702086a30 | 1449 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1450 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1451 | #endif |
ClementBreteau | 1:f3f702086a30 | 1452 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1453 | |
ClementBreteau | 1:f3f702086a30 | 1454 | } |
ClementBreteau | 1:f3f702086a30 | 1455 | else etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1456 | } |
ClementBreteau | 1:f3f702086a30 | 1457 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1458 | break; |
ClementBreteau | 1:f3f702086a30 | 1459 | |
ClementBreteau | 1:f3f702086a30 | 1460 | |
ClementBreteau | 1:f3f702086a30 | 1461 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1462 | { |
ClementBreteau | 1:f3f702086a30 | 1463 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1464 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1465 | if (Status[plen] == 0xFF ) |
ClementBreteau | 1:f3f702086a30 | 1466 | { |
ClementBreteau | 1:f3f702086a30 | 1467 | etat = ident; |
ClementBreteau | 1:f3f702086a30 | 1468 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1469 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1470 | #endif |
ClementBreteau | 1:f3f702086a30 | 1471 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1472 | } |
ClementBreteau | 1:f3f702086a30 | 1473 | else |
ClementBreteau | 1:f3f702086a30 | 1474 | { |
ClementBreteau | 1:f3f702086a30 | 1475 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1476 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1477 | } |
ClementBreteau | 1:f3f702086a30 | 1478 | } |
ClementBreteau | 1:f3f702086a30 | 1479 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1480 | break; |
ClementBreteau | 1:f3f702086a30 | 1481 | |
ClementBreteau | 1:f3f702086a30 | 1482 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
ClementBreteau | 1:f3f702086a30 | 1483 | { |
ClementBreteau | 1:f3f702086a30 | 1484 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1485 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1486 | if (Status[plen] == ID ) |
ClementBreteau | 1:f3f702086a30 | 1487 | { |
ClementBreteau | 1:f3f702086a30 | 1488 | etat = length; |
ClementBreteau | 1:f3f702086a30 | 1489 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1490 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1491 | #endif |
ClementBreteau | 1:f3f702086a30 | 1492 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1493 | } |
ClementBreteau | 1:f3f702086a30 | 1494 | else |
ClementBreteau | 1:f3f702086a30 | 1495 | { |
ClementBreteau | 1:f3f702086a30 | 1496 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1497 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1498 | } |
ClementBreteau | 1:f3f702086a30 | 1499 | } |
ClementBreteau | 1:f3f702086a30 | 1500 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1501 | break; |
ClementBreteau | 1:f3f702086a30 | 1502 | |
ClementBreteau | 1:f3f702086a30 | 1503 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
ClementBreteau | 1:f3f702086a30 | 1504 | { |
ClementBreteau | 1:f3f702086a30 | 1505 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1506 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1507 | if (Status[plen] == 2 ) // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!! |
ClementBreteau | 1:f3f702086a30 | 1508 | { |
ClementBreteau | 1:f3f702086a30 | 1509 | etat = erreur; |
ClementBreteau | 1:f3f702086a30 | 1510 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1511 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1512 | #endif |
ClementBreteau | 1:f3f702086a30 | 1513 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1514 | } |
ClementBreteau | 1:f3f702086a30 | 1515 | else |
ClementBreteau | 1:f3f702086a30 | 1516 | { |
ClementBreteau | 1:f3f702086a30 | 1517 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1518 | plen = 0; |
ClementBreteau | 1:f3f702086a30 | 1519 | } |
ClementBreteau | 1:f3f702086a30 | 1520 | } |
ClementBreteau | 1:f3f702086a30 | 1521 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1522 | break; |
ClementBreteau | 1:f3f702086a30 | 1523 | |
ClementBreteau | 1:f3f702086a30 | 1524 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
ClementBreteau | 1:f3f702086a30 | 1525 | { |
ClementBreteau | 1:f3f702086a30 | 1526 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1527 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1528 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1529 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1530 | #endif |
ClementBreteau | 1:f3f702086a30 | 1531 | plen++; |
ClementBreteau | 1:f3f702086a30 | 1532 | etat = checksum; |
ClementBreteau | 1:f3f702086a30 | 1533 | } |
ClementBreteau | 1:f3f702086a30 | 1534 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1535 | |
ClementBreteau | 1:f3f702086a30 | 1536 | case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write |
ClementBreteau | 1:f3f702086a30 | 1537 | { |
ClementBreteau | 1:f3f702086a30 | 1538 | Status[plen] = _ax12.getc(); |
ClementBreteau | 1:f3f702086a30 | 1539 | timeout = 0; |
ClementBreteau | 1:f3f702086a30 | 1540 | flag_out = 1; |
ClementBreteau | 1:f3f702086a30 | 1541 | etat = Header1; |
ClementBreteau | 1:f3f702086a30 | 1542 | #ifdef AX12_DEBUG_WRITE |
ClementBreteau | 1:f3f702086a30 | 1543 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1544 | #endif |
ClementBreteau | 1:f3f702086a30 | 1545 | } |
ClementBreteau | 1:f3f702086a30 | 1546 | else timeout++; |
ClementBreteau | 1:f3f702086a30 | 1547 | break; |
ClementBreteau | 1:f3f702086a30 | 1548 | } |
ClementBreteau | 1:f3f702086a30 | 1549 | } |
ClementBreteau | 1:f3f702086a30 | 1550 | |
ClementBreteau | 1:f3f702086a30 | 1551 | |
ClementBreteau | 1:f3f702086a30 | 1552 | if ( Status[4] != 0 ) |
ClementBreteau | 1:f3f702086a30 | 1553 | { |
ClementBreteau | 1:f3f702086a30 | 1554 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1555 | printf ("erreur ! \r\n"); |
ClementBreteau | 1:f3f702086a30 | 1556 | #endif |
ClementBreteau | 1:f3f702086a30 | 1557 | for (int i = 0; i<5; i++) |
ClementBreteau | 1:f3f702086a30 | 1558 | { |
ClementBreteau | 1:f3f702086a30 | 1559 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1560 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
ClementBreteau | 1:f3f702086a30 | 1561 | #endif |
ClementBreteau | 1:f3f702086a30 | 1562 | } |
ClementBreteau | 1:f3f702086a30 | 1563 | } |
ClementBreteau | 1:f3f702086a30 | 1564 | |
ClementBreteau | 1:f3f702086a30 | 1565 | if (timeout == MAX_TIMEOUT ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames |
ClementBreteau | 1:f3f702086a30 | 1566 | { |
ClementBreteau | 1:f3f702086a30 | 1567 | #ifdef AX12_DEBUG |
ClementBreteau | 1:f3f702086a30 | 1568 | printf ("timeout erreur\n\r"); |
ClementBreteau | 1:f3f702086a30 | 1569 | #endif |
ClementBreteau | 1:f3f702086a30 | 1570 | return(-1); |
ClementBreteau | 1:f3f702086a30 | 1571 | } |
ClementBreteau | 1:f3f702086a30 | 1572 | |
ClementBreteau | 1:f3f702086a30 | 1573 | // Build the TxPacket first in RAM, then we'll send in one go |
ClementBreteau | 1:f3f702086a30 | 1574 | } |
ClementBreteau | 1:f3f702086a30 | 1575 | |
ClementBreteau | 1:f3f702086a30 | 1576 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
ClementBreteau | 1:f3f702086a30 | 1577 | } |