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