palm and chin
/
BEAR_Protocol_Edited
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Communication_Robot
Fork of
BEAR_Protocol
by 
BE@R lab
BEAR_Protocol.cpp@4:9fbe67ca2f1b, 2016-01-15 (annotated)
- Committer:
- b0ssiz
- Date:
- Fri Jan 15 15:52:31 2016 +0000
- Revision:
- 4:9fbe67ca2f1b
- Parent:
- 2:d17ccaf938f6
- Child:
- 5:6f30b4ea4020
update floating point
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| b0ssiz | 0:fc963e08d580 | 1 | #include "mbed.h" |
| b0ssiz | 0:fc963e08d580 | 2 | #include "BEAR_Protocol.h" |
| b0ssiz | 2:d17ccaf938f6 | 3 | #define RS485_DELAY 90 |
| b0ssiz | 4:9fbe67ca2f1b | 4 | #define FLOAT_CONVERTER 10000 |
| b0ssiz | 4:9fbe67ca2f1b | 5 | |
| b0ssiz | 0:fc963e08d580 | 6 | DigitalOut rs485_dirc(RS485_DIRC); |
| b0ssiz | 0:fc963e08d580 | 7 | |
| b0ssiz | 0:fc963e08d580 | 8 | Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate) |
| b0ssiz | 0:fc963e08d580 | 9 | { |
| b0ssiz | 0:fc963e08d580 | 10 | com = new COMMUNICATION(tx,rx,baudrate); |
| b0ssiz | 0:fc963e08d580 | 11 | } |
| b0ssiz | 0:fc963e08d580 | 12 | |
| b0ssiz | 0:fc963e08d580 | 13 | |
| b0ssiz | 0:fc963e08d580 | 14 | |
| b0ssiz | 4:9fbe67ca2f1b | 15 | void Bear_Communicate::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array) |
| b0ssiz | 4:9fbe67ca2f1b | 16 | { |
| b0ssiz | 4:9fbe67ca2f1b | 17 | float float_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 18 | float int_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 19 | int16_t integer; |
| b0ssiz | 4:9fbe67ca2f1b | 20 | int16_t floating_point; |
| b0ssiz | 4:9fbe67ca2f1b | 21 | |
| b0ssiz | 4:9fbe67ca2f1b | 22 | float_buffer=modf(input_float,&int_buffer); |
| b0ssiz | 4:9fbe67ca2f1b | 23 | float_buffer*=FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 24 | integer=(int16_t)int_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 25 | floating_point=(int16_t)float_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 26 | Utilities::ConvertInt16ToUInt8Array(integer,int_data_array); |
| b0ssiz | 4:9fbe67ca2f1b | 27 | Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array); |
| b0ssiz | 4:9fbe67ca2f1b | 28 | } |
| b0ssiz | 4:9fbe67ca2f1b | 29 | |
| b0ssiz | 4:9fbe67ca2f1b | 30 | |
| b0ssiz | 4:9fbe67ca2f1b | 31 | |
| b0ssiz | 4:9fbe67ca2f1b | 32 | uint8_t Bear_Communicate::setID(uint8_t id,uint8_t new_id) |
| b0ssiz | 0:fc963e08d580 | 33 | { |
| b0ssiz | 0:fc963e08d580 | 34 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 35 | |
| b0ssiz | 0:fc963e08d580 | 36 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 37 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 38 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 39 | package.parameter[0]=SET_ID; |
| b0ssiz | 0:fc963e08d580 | 40 | package.parameter[1]=new_id; |
| b0ssiz | 0:fc963e08d580 | 41 | |
| b0ssiz | 0:fc963e08d580 | 42 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 43 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 44 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 45 | } |
| b0ssiz | 0:fc963e08d580 | 46 | |
| b0ssiz | 0:fc963e08d580 | 47 | |
| b0ssiz | 0:fc963e08d580 | 48 | |
| b0ssiz | 4:9fbe67ca2f1b | 49 | uint8_t Bear_Communicate::setMotorPos(uint8_t id,float up_angle,float low_angle) |
| b0ssiz | 0:fc963e08d580 | 50 | { |
| b0ssiz | 4:9fbe67ca2f1b | 51 | uint8_t IntUpAngle[2],FloatUpAngle[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 52 | uint8_t IntLowAngle[2],FloatLowAngle[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 53 | FloatSep(up_angle,IntUpAngle,FloatUpAngle); |
| b0ssiz | 4:9fbe67ca2f1b | 54 | FloatSep(low_angle,IntLowAngle,FloatLowAngle); |
| b0ssiz | 4:9fbe67ca2f1b | 55 | |
| b0ssiz | 0:fc963e08d580 | 56 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 57 | |
| b0ssiz | 0:fc963e08d580 | 58 | package.robotId = id; |
| b0ssiz | 4:9fbe67ca2f1b | 59 | package.length = 11; |
| b0ssiz | 0:fc963e08d580 | 60 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 61 | package.parameter[0]=SET_MOTOR_UPPER_ANG; |
| b0ssiz | 4:9fbe67ca2f1b | 62 | package.parameter[1]=IntUpAngle[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 63 | package.parameter[2]=IntUpAngle[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 64 | package.parameter[3]=FloatUpAngle[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 65 | package.parameter[4]=FloatUpAngle[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 66 | package.parameter[5]=IntLowAngle[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 67 | package.parameter[6]=IntLowAngle[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 68 | package.parameter[7]=FloatLowAngle[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 69 | package.parameter[8]=FloatLowAngle[1]; |
| b0ssiz | 0:fc963e08d580 | 70 | |
| b0ssiz | 0:fc963e08d580 | 71 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 72 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 73 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 74 | } |
| b0ssiz | 0:fc963e08d580 | 75 | |
| b0ssiz | 0:fc963e08d580 | 76 | |
| b0ssiz | 0:fc963e08d580 | 77 | |
| b0ssiz | 4:9fbe67ca2f1b | 78 | uint8_t Bear_Communicate::getMotorPos(uint8_t id,float *up_angle,float *low_angle) |
| b0ssiz | 0:fc963e08d580 | 79 | { |
| b0ssiz | 4:9fbe67ca2f1b | 80 | uint8_t IntUpAngle[2],FloatUpAngle[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 81 | uint8_t IntLowAngle[2],FloatLowAngle[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 82 | float int_buffer,float_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 83 | |
| b0ssiz | 0:fc963e08d580 | 84 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 85 | |
| b0ssiz | 0:fc963e08d580 | 86 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 87 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 88 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 89 | package.parameter[0] = GET_MOTOR_UPPER_ANG; |
| b0ssiz | 0:fc963e08d580 | 90 | package.parameter[1] = 0x02; |
| b0ssiz | 0:fc963e08d580 | 91 | |
| b0ssiz | 0:fc963e08d580 | 92 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 93 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 94 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 95 | |
| b0ssiz | 0:fc963e08d580 | 96 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 97 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 98 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 99 | |
| b0ssiz | 0:fc963e08d580 | 100 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 4:9fbe67ca2f1b | 101 | IntUpAngle[0]=package.parameter[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 102 | IntUpAngle[1]=package.parameter[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 103 | FloatUpAngle[0]=package.parameter[3]; |
| b0ssiz | 4:9fbe67ca2f1b | 104 | FloatUpAngle[1]=package.parameter[4]; |
| b0ssiz | 4:9fbe67ca2f1b | 105 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); |
| b0ssiz | 4:9fbe67ca2f1b | 106 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 107 | *up_angle=int_buffer+float_buffer; |
| b0ssiz | 0:fc963e08d580 | 108 | |
| b0ssiz | 4:9fbe67ca2f1b | 109 | IntLowAngle[0]=package.parameter[5]; |
| b0ssiz | 4:9fbe67ca2f1b | 110 | IntLowAngle[1]=package.parameter[6]; |
| b0ssiz | 4:9fbe67ca2f1b | 111 | FloatLowAngle[0]=package.parameter[7]; |
| b0ssiz | 4:9fbe67ca2f1b | 112 | FloatLowAngle[1]=package.parameter[8]; |
| b0ssiz | 4:9fbe67ca2f1b | 113 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); |
| b0ssiz | 4:9fbe67ca2f1b | 114 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 115 | *low_angle=int_buffer+float_buffer;; |
| b0ssiz | 0:fc963e08d580 | 116 | |
| b0ssiz | 0:fc963e08d580 | 117 | } |
| b0ssiz | 0:fc963e08d580 | 118 | return status; |
| b0ssiz | 0:fc963e08d580 | 119 | } |
| b0ssiz | 0:fc963e08d580 | 120 | |
| b0ssiz | 0:fc963e08d580 | 121 | |
| b0ssiz | 0:fc963e08d580 | 122 | |
| b0ssiz | 4:9fbe67ca2f1b | 123 | uint8_t Bear_Communicate::setKp(uint8_t id,float Kp) |
| b0ssiz | 0:fc963e08d580 | 124 | { |
| b0ssiz | 4:9fbe67ca2f1b | 125 | uint8_t IntKp[2],FloatKp[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 126 | FloatSep(Kp,IntKp,FloatKp); |
| b0ssiz | 4:9fbe67ca2f1b | 127 | |
| b0ssiz | 0:fc963e08d580 | 128 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 129 | |
| b0ssiz | 0:fc963e08d580 | 130 | package.robotId = id; |
| b0ssiz | 4:9fbe67ca2f1b | 131 | package.length = 7; |
| b0ssiz | 0:fc963e08d580 | 132 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 133 | package.parameter[0]=SET_KP; |
| b0ssiz | 4:9fbe67ca2f1b | 134 | package.parameter[1]=IntKp[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 135 | package.parameter[2]=IntKp[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 136 | package.parameter[3]=FloatKp[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 137 | package.parameter[4]=FloatKp[1]; |
| b0ssiz | 0:fc963e08d580 | 138 | |
| b0ssiz | 0:fc963e08d580 | 139 | |
| b0ssiz | 0:fc963e08d580 | 140 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 141 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 142 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 143 | } |
| b0ssiz | 0:fc963e08d580 | 144 | |
| b0ssiz | 0:fc963e08d580 | 145 | |
| b0ssiz | 0:fc963e08d580 | 146 | |
| b0ssiz | 4:9fbe67ca2f1b | 147 | uint8_t Bear_Communicate::setKi(uint8_t id,float Ki) |
| b0ssiz | 0:fc963e08d580 | 148 | { |
| b0ssiz | 4:9fbe67ca2f1b | 149 | uint8_t IntKi[2],FloatKi[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 150 | FloatSep(Ki,IntKi,FloatKi); |
| b0ssiz | 4:9fbe67ca2f1b | 151 | |
| b0ssiz | 0:fc963e08d580 | 152 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 153 | |
| b0ssiz | 0:fc963e08d580 | 154 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 155 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 156 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 4:9fbe67ca2f1b | 157 | package.parameter[0]=SET_KI; |
| b0ssiz | 4:9fbe67ca2f1b | 158 | package.parameter[1]=IntKi[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 159 | package.parameter[2]=IntKi[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 160 | package.parameter[3]=FloatKi[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 161 | package.parameter[4]=FloatKi[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 162 | |
| b0ssiz | 4:9fbe67ca2f1b | 163 | |
| b0ssiz | 4:9fbe67ca2f1b | 164 | |
| b0ssiz | 4:9fbe67ca2f1b | 165 | rs485_dirc=1; |
| b0ssiz | 4:9fbe67ca2f1b | 166 | wait_us(RS485_DELAY); |
| b0ssiz | 4:9fbe67ca2f1b | 167 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 4:9fbe67ca2f1b | 168 | |
| b0ssiz | 4:9fbe67ca2f1b | 169 | } |
| b0ssiz | 4:9fbe67ca2f1b | 170 | |
| b0ssiz | 4:9fbe67ca2f1b | 171 | |
| b0ssiz | 4:9fbe67ca2f1b | 172 | |
| b0ssiz | 4:9fbe67ca2f1b | 173 | uint8_t Bear_Communicate::setKd(uint8_t id,float Kd) |
| b0ssiz | 4:9fbe67ca2f1b | 174 | { |
| b0ssiz | 4:9fbe67ca2f1b | 175 | uint8_t IntKd[2],FloatKd[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 176 | FloatSep(Kd,IntKd,FloatKd); |
| b0ssiz | 4:9fbe67ca2f1b | 177 | |
| b0ssiz | 4:9fbe67ca2f1b | 178 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 4:9fbe67ca2f1b | 179 | |
| b0ssiz | 4:9fbe67ca2f1b | 180 | package.robotId = id; |
| b0ssiz | 4:9fbe67ca2f1b | 181 | package.length = 7; |
| b0ssiz | 4:9fbe67ca2f1b | 182 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 4:9fbe67ca2f1b | 183 | package.parameter[0]=SET_KP; |
| b0ssiz | 4:9fbe67ca2f1b | 184 | package.parameter[1]=IntKd[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 185 | package.parameter[2]=IntKd[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 186 | package.parameter[3]=FloatKd[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 187 | package.parameter[4]=FloatKd[1]; |
| b0ssiz | 0:fc963e08d580 | 188 | |
| b0ssiz | 0:fc963e08d580 | 189 | |
| b0ssiz | 0:fc963e08d580 | 190 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 191 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 192 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 193 | } |
| b0ssiz | 0:fc963e08d580 | 194 | |
| b0ssiz | 0:fc963e08d580 | 195 | |
| b0ssiz | 0:fc963e08d580 | 196 | |
| b0ssiz | 4:9fbe67ca2f1b | 197 | uint8_t Bear_Communicate::getKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd) |
| b0ssiz | 0:fc963e08d580 | 198 | { |
| b0ssiz | 4:9fbe67ca2f1b | 199 | uint8_t IntKp[2],FloatKp[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 200 | uint8_t IntKi[2],FloatKi[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 201 | uint8_t IntKd[2],FloatKd[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 202 | float int_buffer,float_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 203 | |
| b0ssiz | 0:fc963e08d580 | 204 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 205 | |
| b0ssiz | 0:fc963e08d580 | 206 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 207 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 208 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 209 | package.parameter[0]=GET_KP; |
| b0ssiz | 0:fc963e08d580 | 210 | package.parameter[1]=0x03; |
| b0ssiz | 0:fc963e08d580 | 211 | |
| b0ssiz | 0:fc963e08d580 | 212 | |
| b0ssiz | 0:fc963e08d580 | 213 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 214 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 215 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 216 | |
| b0ssiz | 0:fc963e08d580 | 217 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 218 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 219 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 220 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 4:9fbe67ca2f1b | 221 | IntKp[0]=package.parameter[0]; |
| b0ssiz | 4:9fbe67ca2f1b | 222 | IntKp[1]=package.parameter[1]; |
| b0ssiz | 4:9fbe67ca2f1b | 223 | FloatKp[0]=package.parameter[2]; |
| b0ssiz | 4:9fbe67ca2f1b | 224 | FloatKp[1]=package.parameter[3]; |
| b0ssiz | 4:9fbe67ca2f1b | 225 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp); |
| b0ssiz | 4:9fbe67ca2f1b | 226 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 227 | *Kp=int_buffer+float_buffer; |
| b0ssiz | 0:fc963e08d580 | 228 | |
| b0ssiz | 4:9fbe67ca2f1b | 229 | IntKi[0]=package.parameter[4]; |
| b0ssiz | 4:9fbe67ca2f1b | 230 | IntKi[1]=package.parameter[5]; |
| b0ssiz | 4:9fbe67ca2f1b | 231 | FloatKi[0]=package.parameter[6]; |
| b0ssiz | 4:9fbe67ca2f1b | 232 | FloatKi[1]=package.parameter[7]; |
| b0ssiz | 4:9fbe67ca2f1b | 233 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi); |
| b0ssiz | 4:9fbe67ca2f1b | 234 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 235 | *Ki=int_buffer+float_buffer; |
| b0ssiz | 4:9fbe67ca2f1b | 236 | |
| b0ssiz | 4:9fbe67ca2f1b | 237 | IntKi[0]=package.parameter[8]; |
| b0ssiz | 4:9fbe67ca2f1b | 238 | IntKi[1]=package.parameter[9]; |
| b0ssiz | 4:9fbe67ca2f1b | 239 | FloatKi[0]=package.parameter[10]; |
| b0ssiz | 4:9fbe67ca2f1b | 240 | FloatKi[1]=package.parameter[11]; |
| b0ssiz | 4:9fbe67ca2f1b | 241 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd); |
| b0ssiz | 4:9fbe67ca2f1b | 242 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER; |
| b0ssiz | 4:9fbe67ca2f1b | 243 | *Kd=int_buffer+float_buffer; |
| b0ssiz | 0:fc963e08d580 | 244 | } |
| b0ssiz | 0:fc963e08d580 | 245 | return status; |
| b0ssiz | 0:fc963e08d580 | 246 | } |
| b0ssiz | 0:fc963e08d580 | 247 | |
| b0ssiz | 0:fc963e08d580 | 248 | |
| b0ssiz | 0:fc963e08d580 | 249 | |
| b0ssiz | 0:fc963e08d580 | 250 | ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ |
| b0ssiz | 0:fc963e08d580 | 251 | |
| b0ssiz | 0:fc963e08d580 | 252 | |
| b0ssiz | 4:9fbe67ca2f1b | 253 | uint8_t Bear_Communicate::setMargin(uint8_t id,float margin) |
| b0ssiz | 0:fc963e08d580 | 254 | { |
| b0ssiz | 0:fc963e08d580 | 255 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 256 | |
| b0ssiz | 0:fc963e08d580 | 257 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 258 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 259 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 260 | package.parameter[0]=SET_MARGIN; |
| b0ssiz | 0:fc963e08d580 | 261 | package.parameter[1]=margin; |
| b0ssiz | 0:fc963e08d580 | 262 | |
| b0ssiz | 0:fc963e08d580 | 263 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 264 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 265 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 266 | } |
| b0ssiz | 0:fc963e08d580 | 267 | |
| b0ssiz | 0:fc963e08d580 | 268 | |
| b0ssiz | 0:fc963e08d580 | 269 | |
| b0ssiz | 4:9fbe67ca2f1b | 270 | uint8_t Bear_Communicate::getMargin(uint8_t id,float *margin) |
| b0ssiz | 0:fc963e08d580 | 271 | { |
| b0ssiz | 0:fc963e08d580 | 272 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 273 | |
| b0ssiz | 0:fc963e08d580 | 274 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 275 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 276 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 277 | package.parameter[0]=GET_MARGIN; |
| b0ssiz | 0:fc963e08d580 | 278 | package.parameter[1]=0x01; |
| b0ssiz | 0:fc963e08d580 | 279 | |
| b0ssiz | 0:fc963e08d580 | 280 | |
| b0ssiz | 0:fc963e08d580 | 281 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 282 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 283 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 284 | |
| b0ssiz | 0:fc963e08d580 | 285 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 286 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 287 | uint8_t status=com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 288 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 289 | |
| b0ssiz | 0:fc963e08d580 | 290 | *margin=package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 291 | } |
| b0ssiz | 0:fc963e08d580 | 292 | return status; |
| b0ssiz | 0:fc963e08d580 | 293 | } |
| b0ssiz | 0:fc963e08d580 | 294 | |
| b0ssiz | 0:fc963e08d580 | 295 | |
| b0ssiz | 0:fc963e08d580 | 296 | |
| b0ssiz | 4:9fbe67ca2f1b | 297 | uint8_t Bear_Communicate::setHeight(uint8_t id,float height) |
| b0ssiz | 0:fc963e08d580 | 298 | { |
| b0ssiz | 0:fc963e08d580 | 299 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 300 | |
| b0ssiz | 0:fc963e08d580 | 301 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 302 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 303 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 304 | package.parameter[0]=SET_HEIGHT; |
| b0ssiz | 0:fc963e08d580 | 305 | package.parameter[1]=height; |
| b0ssiz | 0:fc963e08d580 | 306 | |
| b0ssiz | 0:fc963e08d580 | 307 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 308 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 309 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 310 | } |
| b0ssiz | 0:fc963e08d580 | 311 | |
| b0ssiz | 0:fc963e08d580 | 312 | |
| b0ssiz | 0:fc963e08d580 | 313 | |
| b0ssiz | 4:9fbe67ca2f1b | 314 | uint8_t Bear_Communicate::getHeight(uint8_t id,float *height) |
| b0ssiz | 0:fc963e08d580 | 315 | { |
| b0ssiz | 0:fc963e08d580 | 316 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 317 | |
| b0ssiz | 0:fc963e08d580 | 318 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 319 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 320 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 321 | package.parameter[0]=GET_HEIGHT; |
| b0ssiz | 0:fc963e08d580 | 322 | package.parameter[1]=0x01; |
| b0ssiz | 0:fc963e08d580 | 323 | |
| b0ssiz | 0:fc963e08d580 | 324 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 325 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 326 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 327 | |
| b0ssiz | 0:fc963e08d580 | 328 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 329 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 330 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 331 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 332 | |
| b0ssiz | 0:fc963e08d580 | 333 | *height = package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 334 | |
| b0ssiz | 0:fc963e08d580 | 335 | } |
| b0ssiz | 0:fc963e08d580 | 336 | return status; |
| b0ssiz | 0:fc963e08d580 | 337 | } |
| b0ssiz | 0:fc963e08d580 | 338 | |
| b0ssiz | 0:fc963e08d580 | 339 | |
| b0ssiz | 0:fc963e08d580 | 340 | |
| b0ssiz | 4:9fbe67ca2f1b | 341 | uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos) |
| b0ssiz | 0:fc963e08d580 | 342 | { |
| b0ssiz | 0:fc963e08d580 | 343 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 344 | |
| b0ssiz | 0:fc963e08d580 | 345 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 346 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 347 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 348 | package.parameter[0]=SET_WHEELPOS; |
| b0ssiz | 0:fc963e08d580 | 349 | package.parameter[1]=WheelPos; |
| b0ssiz | 0:fc963e08d580 | 350 | |
| b0ssiz | 0:fc963e08d580 | 351 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 352 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 353 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 354 | } |
| b0ssiz | 0:fc963e08d580 | 355 | |
| b0ssiz | 0:fc963e08d580 | 356 | |
| b0ssiz | 0:fc963e08d580 | 357 | |
| b0ssiz | 4:9fbe67ca2f1b | 358 | uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos) |
| b0ssiz | 0:fc963e08d580 | 359 | { |
| b0ssiz | 0:fc963e08d580 | 360 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 361 | |
| b0ssiz | 0:fc963e08d580 | 362 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 363 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 364 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 365 | package.parameter[0]=GET_WHEELPOS; |
| b0ssiz | 0:fc963e08d580 | 366 | package.parameter[1]=0x01; |
| b0ssiz | 0:fc963e08d580 | 367 | |
| b0ssiz | 0:fc963e08d580 | 368 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 369 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 370 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 371 | |
| b0ssiz | 0:fc963e08d580 | 372 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 373 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 374 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 375 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 376 | |
| b0ssiz | 0:fc963e08d580 | 377 | *WheelPos = package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 378 | } |
| b0ssiz | 0:fc963e08d580 | 379 | return status; |
| b0ssiz | 0:fc963e08d580 | 380 | } |
| b0ssiz | 0:fc963e08d580 | 381 | |
| b0ssiz | 4:9fbe67ca2f1b | 382 | uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float y_max,float z_max,float x_min,float y_min,float z_min) |
| b0ssiz | 0:fc963e08d580 | 383 | { |
| b0ssiz | 0:fc963e08d580 | 384 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 385 | |
| b0ssiz | 0:fc963e08d580 | 386 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 387 | package.length = 9; |
| b0ssiz | 0:fc963e08d580 | 388 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 389 | package.parameter[0]=SET_MAG_X_MAX; |
| b0ssiz | 0:fc963e08d580 | 390 | package.parameter[1]=x_max; |
| b0ssiz | 0:fc963e08d580 | 391 | package.parameter[2]=y_max; |
| b0ssiz | 0:fc963e08d580 | 392 | package.parameter[3]=z_max; |
| b0ssiz | 0:fc963e08d580 | 393 | package.parameter[4]=x_min; |
| b0ssiz | 0:fc963e08d580 | 394 | package.parameter[5]=y_min; |
| b0ssiz | 0:fc963e08d580 | 395 | package.parameter[6]=z_min; |
| b0ssiz | 0:fc963e08d580 | 396 | |
| b0ssiz | 0:fc963e08d580 | 397 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 398 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 399 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 400 | } |
| b0ssiz | 0:fc963e08d580 | 401 | |
| b0ssiz | 4:9fbe67ca2f1b | 402 | uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *y_max,float *z_max,float *x_min,float *y_min,float *z_min) |
| b0ssiz | 0:fc963e08d580 | 403 | { |
| b0ssiz | 0:fc963e08d580 | 404 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 405 | |
| b0ssiz | 0:fc963e08d580 | 406 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 407 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 408 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 409 | package.parameter[0]=GET_MAG_X_MAX; |
| b0ssiz | 0:fc963e08d580 | 410 | package.parameter[1]=0x06; |
| b0ssiz | 0:fc963e08d580 | 411 | |
| b0ssiz | 0:fc963e08d580 | 412 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 413 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 414 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 415 | |
| b0ssiz | 0:fc963e08d580 | 416 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 417 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 418 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 419 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 420 | |
| b0ssiz | 0:fc963e08d580 | 421 | *x_max=package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 422 | *y_max=package.parameter[1]; |
| b0ssiz | 0:fc963e08d580 | 423 | *z_max=package.parameter[2]; |
| b0ssiz | 0:fc963e08d580 | 424 | *x_min=package.parameter[3]; |
| b0ssiz | 0:fc963e08d580 | 425 | *y_min=package.parameter[4]; |
| b0ssiz | 0:fc963e08d580 | 426 | *z_min=package.parameter[5]; |
| b0ssiz | 0:fc963e08d580 | 427 | |
| b0ssiz | 0:fc963e08d580 | 428 | } |
| b0ssiz | 0:fc963e08d580 | 429 | return status; |
| b0ssiz | 0:fc963e08d580 | 430 | } |
| b0ssiz | 0:fc963e08d580 | 431 | |
| b0ssiz | 0:fc963e08d580 | 432 | |
| b0ssiz | 0:fc963e08d580 | 433 | |
| b0ssiz | 4:9fbe67ca2f1b | 434 | uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z) |
| b0ssiz | 0:fc963e08d580 | 435 | { |
| b0ssiz | 0:fc963e08d580 | 436 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 437 | |
| b0ssiz | 0:fc963e08d580 | 438 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 439 | package.length = 5; |
| b0ssiz | 0:fc963e08d580 | 440 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 441 | package.parameter[0]=SET_OFFSET_Y; |
| b0ssiz | 0:fc963e08d580 | 442 | package.parameter[1]=offset_y; |
| b0ssiz | 0:fc963e08d580 | 443 | package.parameter[2]=offset_z; |
| b0ssiz | 0:fc963e08d580 | 444 | |
| b0ssiz | 0:fc963e08d580 | 445 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 446 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 447 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 448 | } |
| b0ssiz | 0:fc963e08d580 | 449 | |
| b0ssiz | 0:fc963e08d580 | 450 | |
| b0ssiz | 0:fc963e08d580 | 451 | |
| b0ssiz | 4:9fbe67ca2f1b | 452 | uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z) |
| b0ssiz | 0:fc963e08d580 | 453 | { |
| b0ssiz | 0:fc963e08d580 | 454 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 455 | |
| b0ssiz | 0:fc963e08d580 | 456 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 457 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 458 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 459 | package.parameter[0]=GET_MAG_X_MAX; |
| b0ssiz | 0:fc963e08d580 | 460 | package.parameter[1]=0x02; |
| b0ssiz | 0:fc963e08d580 | 461 | |
| b0ssiz | 0:fc963e08d580 | 462 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 463 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 464 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 465 | |
| b0ssiz | 0:fc963e08d580 | 466 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 467 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 468 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 469 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 470 | |
| b0ssiz | 0:fc963e08d580 | 471 | *offset_y=package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 472 | *offset_z=package.parameter[1]; |
| b0ssiz | 0:fc963e08d580 | 473 | } |
| b0ssiz | 0:fc963e08d580 | 474 | return status; |
| b0ssiz | 0:fc963e08d580 | 475 | } |
| b0ssiz | 0:fc963e08d580 | 476 | |
| b0ssiz | 0:fc963e08d580 | 477 | |
| b0ssiz | 0:fc963e08d580 | 478 | |
| b0ssiz | 4:9fbe67ca2f1b | 479 | uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length) |
| b0ssiz | 0:fc963e08d580 | 480 | { |
| b0ssiz | 0:fc963e08d580 | 481 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 482 | |
| b0ssiz | 0:fc963e08d580 | 483 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 484 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 485 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 486 | package.parameter[0]=SET_BODY_LENGTH; |
| b0ssiz | 0:fc963e08d580 | 487 | package.parameter[1]=body_length; |
| b0ssiz | 0:fc963e08d580 | 488 | |
| b0ssiz | 0:fc963e08d580 | 489 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 490 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 491 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 492 | } |
| b0ssiz | 0:fc963e08d580 | 493 | |
| b0ssiz | 0:fc963e08d580 | 494 | |
| b0ssiz | 0:fc963e08d580 | 495 | |
| b0ssiz | 0:fc963e08d580 | 496 | |
| b0ssiz | 4:9fbe67ca2f1b | 497 | uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length) |
| b0ssiz | 0:fc963e08d580 | 498 | { |
| b0ssiz | 0:fc963e08d580 | 499 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 500 | |
| b0ssiz | 0:fc963e08d580 | 501 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 502 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 503 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 504 | package.parameter[0]=GET_BODY_LENGTH; |
| b0ssiz | 0:fc963e08d580 | 505 | package.parameter[1]=0x01; |
| b0ssiz | 0:fc963e08d580 | 506 | |
| b0ssiz | 0:fc963e08d580 | 507 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 508 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 509 | com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 510 | |
| b0ssiz | 0:fc963e08d580 | 511 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 512 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 513 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 514 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 515 | |
| b0ssiz | 0:fc963e08d580 | 516 | *body_length=package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 517 | } |
| b0ssiz | 0:fc963e08d580 | 518 | return status; |
| b0ssiz | 0:fc963e08d580 | 519 | } |
| b0ssiz | 0:fc963e08d580 | 520 | |
| b0ssiz | 4:9fbe67ca2f1b | 521 | uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle) |
| b0ssiz | 0:fc963e08d580 | 522 | { |
| b0ssiz | 0:fc963e08d580 | 523 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 524 | |
| b0ssiz | 0:fc963e08d580 | 525 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 526 | package.length = 5; |
| b0ssiz | 0:fc963e08d580 | 527 | package.instructionErrorId = WRITE_DATA; |
| b0ssiz | 0:fc963e08d580 | 528 | package.parameter[0]=SET_MAX_ANGLE; |
| b0ssiz | 0:fc963e08d580 | 529 | package.parameter[1]=max_angle; |
| b0ssiz | 0:fc963e08d580 | 530 | package.parameter[2]=min_angle; |
| b0ssiz | 0:fc963e08d580 | 531 | |
| b0ssiz | 0:fc963e08d580 | 532 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 533 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 534 | return com->sendCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 535 | } |
| b0ssiz | 0:fc963e08d580 | 536 | |
| b0ssiz | 0:fc963e08d580 | 537 | |
| b0ssiz | 0:fc963e08d580 | 538 | |
| b0ssiz | 0:fc963e08d580 | 539 | |
| b0ssiz | 4:9fbe67ca2f1b | 540 | uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle) |
| b0ssiz | 0:fc963e08d580 | 541 | { |
| b0ssiz | 0:fc963e08d580 | 542 | |
| b0ssiz | 0:fc963e08d580 | 543 | ANDANTE_PROTOCOL_PACKET package; |
| b0ssiz | 0:fc963e08d580 | 544 | |
| b0ssiz | 0:fc963e08d580 | 545 | package.robotId = id; |
| b0ssiz | 0:fc963e08d580 | 546 | package.length = 4; |
| b0ssiz | 0:fc963e08d580 | 547 | package.instructionErrorId = READ_DATA; |
| b0ssiz | 0:fc963e08d580 | 548 | package.parameter[0]=GET_BODY_LENGTH; |
| b0ssiz | 0:fc963e08d580 | 549 | package.parameter[1]=0x02; |
| b0ssiz | 0:fc963e08d580 | 550 | |
| b0ssiz | 0:fc963e08d580 | 551 | rs485_dirc=1; |
| b0ssiz | 0:fc963e08d580 | 552 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 553 | com->sendCommunicatePacket(&package); |
| b0ssiz | 4:9fbe67ca2f1b | 554 | |
| b0ssiz | 0:fc963e08d580 | 555 | rs485_dirc=0; |
| b0ssiz | 0:fc963e08d580 | 556 | wait_us(RS485_DELAY); |
| b0ssiz | 0:fc963e08d580 | 557 | uint8_t status = com->receiveCommunicatePacket(&package); |
| b0ssiz | 0:fc963e08d580 | 558 | if(status == ANDANTE_ERRBIT_NONE) { |
| b0ssiz | 0:fc963e08d580 | 559 | |
| b0ssiz | 0:fc963e08d580 | 560 | *max_angle=package.parameter[0]; |
| b0ssiz | 0:fc963e08d580 | 561 | *min_angle=package.parameter[1]; |
| b0ssiz | 0:fc963e08d580 | 562 | |
| b0ssiz | 0:fc963e08d580 | 563 | } |
| b0ssiz | 0:fc963e08d580 | 564 | return status; |
| b0ssiz | 0:fc963e08d580 | 565 | } |