BE@R lab / BEAR_Protocol

ใช้สื่อสารกันระหว่าง Brain และ Motion

Dependencies:   Fork_Boss_Communication_Robot

Dependents:   Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more

BEAR_Protocol.cpp@2:d17ccaf938f6, 2015-12-23 (annotated)

Committer:
b0ssiz
Date:
Wed Dec 23 13:20:18 2015 +0000
Revision:
2:d17ccaf938f6
Parent:
0:fc963e08d580
Child:
4:9fbe67ca2f1b
debug

Who changed what in which revision?

UserRevisionLine numberNew 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 0:fc963e08d580 4 DigitalOut rs485_dirc(RS485_DIRC);
b0ssiz 0:fc963e08d580 5
b0ssiz 0:fc963e08d580 6 Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate)
b0ssiz 0:fc963e08d580 7 {
b0ssiz 0:fc963e08d580 8 com = new COMMUNICATION(tx,rx,baudrate);
b0ssiz 0:fc963e08d580 9 }
b0ssiz 0:fc963e08d580 10
b0ssiz 0:fc963e08d580 11
b0ssiz 0:fc963e08d580 12
b0ssiz 0:fc963e08d580 13 uint8_t Bear_Communicate::SetID(uint8_t id,uint8_t new_id)
b0ssiz 0:fc963e08d580 14 {
b0ssiz 0:fc963e08d580 15 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 16
b0ssiz 0:fc963e08d580 17 package.robotId = id;
b0ssiz 0:fc963e08d580 18 package.length = 4;
b0ssiz 0:fc963e08d580 19 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 20 package.parameter[0]=SET_ID;
b0ssiz 0:fc963e08d580 21 package.parameter[1]=new_id;
b0ssiz 0:fc963e08d580 22
b0ssiz 0:fc963e08d580 23 rs485_dirc=1;
b0ssiz 0:fc963e08d580 24 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 25 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 26 }
b0ssiz 0:fc963e08d580 27
b0ssiz 0:fc963e08d580 28
b0ssiz 0:fc963e08d580 29
b0ssiz 0:fc963e08d580 30 uint8_t Bear_Communicate::SetMotorPos(uint8_t id,float up_angle,float low_angle)
b0ssiz 0:fc963e08d580 31 {
b0ssiz 0:fc963e08d580 32 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 33
b0ssiz 0:fc963e08d580 34 package.robotId = id;
b0ssiz 0:fc963e08d580 35 package.length = 5;
b0ssiz 0:fc963e08d580 36 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 37 package.parameter[0]=SET_MOTOR_UPPER_ANG;
b0ssiz 0:fc963e08d580 38 package.parameter[1]=up_angle;
b0ssiz 0:fc963e08d580 39 package.parameter[2]=low_angle;
b0ssiz 0:fc963e08d580 40
b0ssiz 0:fc963e08d580 41 rs485_dirc=1;
b0ssiz 0:fc963e08d580 42 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 43 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 44 }
b0ssiz 0:fc963e08d580 45
b0ssiz 0:fc963e08d580 46
b0ssiz 0:fc963e08d580 47
b0ssiz 0:fc963e08d580 48 uint8_t Bear_Communicate::GetMotorPos(uint8_t id,float *up_angle,float *low_angle)
b0ssiz 0:fc963e08d580 49 {
b0ssiz 0:fc963e08d580 50 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 51
b0ssiz 0:fc963e08d580 52 package.robotId = id;
b0ssiz 0:fc963e08d580 53 package.length = 4;
b0ssiz 0:fc963e08d580 54 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 55 package.parameter[0] = GET_MOTOR_UPPER_ANG;
b0ssiz 0:fc963e08d580 56 package.parameter[1] = 0x02;
b0ssiz 0:fc963e08d580 57
b0ssiz 0:fc963e08d580 58 rs485_dirc=1;
b0ssiz 0:fc963e08d580 59 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 60 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 61
b0ssiz 0:fc963e08d580 62 rs485_dirc=0;
b0ssiz 0:fc963e08d580 63 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 64 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 65
b0ssiz 0:fc963e08d580 66 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 67
b0ssiz 0:fc963e08d580 68 *up_angle=(float)package.parameter[0];
b0ssiz 0:fc963e08d580 69 *low_angle=(float)package.parameter[1];
b0ssiz 0:fc963e08d580 70
b0ssiz 0:fc963e08d580 71 }
b0ssiz 0:fc963e08d580 72 return status;
b0ssiz 0:fc963e08d580 73 }
b0ssiz 0:fc963e08d580 74
b0ssiz 0:fc963e08d580 75
b0ssiz 0:fc963e08d580 76
b0ssiz 0:fc963e08d580 77 uint8_t Bear_Communicate::SetKp(uint8_t id,float Kp)
b0ssiz 0:fc963e08d580 78 {
b0ssiz 0:fc963e08d580 79 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 80
b0ssiz 0:fc963e08d580 81 package.robotId = id;
b0ssiz 0:fc963e08d580 82 package.length = 4;
b0ssiz 0:fc963e08d580 83 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 84 package.parameter[0]=SET_KP;
b0ssiz 0:fc963e08d580 85 package.parameter[1]=Kp;
b0ssiz 0:fc963e08d580 86
b0ssiz 0:fc963e08d580 87 rs485_dirc=1;
b0ssiz 0:fc963e08d580 88 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 89 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 90 }
b0ssiz 0:fc963e08d580 91
b0ssiz 0:fc963e08d580 92
b0ssiz 0:fc963e08d580 93
b0ssiz 0:fc963e08d580 94 uint8_t Bear_Communicate::SetKi(uint8_t id,float Ki)
b0ssiz 0:fc963e08d580 95 {
b0ssiz 0:fc963e08d580 96 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 97
b0ssiz 0:fc963e08d580 98 package.robotId = id;
b0ssiz 0:fc963e08d580 99 package.length = 4;
b0ssiz 0:fc963e08d580 100 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 101 package.parameter[0]=SET_KI;
b0ssiz 0:fc963e08d580 102 package.parameter[1]=Ki;
b0ssiz 0:fc963e08d580 103
b0ssiz 0:fc963e08d580 104
b0ssiz 0:fc963e08d580 105 rs485_dirc=1;
b0ssiz 0:fc963e08d580 106 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 107 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 108 }
b0ssiz 0:fc963e08d580 109
b0ssiz 0:fc963e08d580 110
b0ssiz 0:fc963e08d580 111
b0ssiz 0:fc963e08d580 112 uint8_t Bear_Communicate::SetKd(uint8_t id,float Kd)
b0ssiz 0:fc963e08d580 113 {
b0ssiz 0:fc963e08d580 114 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 115
b0ssiz 0:fc963e08d580 116 package.robotId = id;
b0ssiz 0:fc963e08d580 117 package.length = 4;
b0ssiz 0:fc963e08d580 118 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 119 package.parameter[0]=SET_KD;
b0ssiz 0:fc963e08d580 120 package.parameter[1]=Kd;
b0ssiz 0:fc963e08d580 121
b0ssiz 0:fc963e08d580 122
b0ssiz 0:fc963e08d580 123 rs485_dirc=1;
b0ssiz 0:fc963e08d580 124 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 125 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 126 }
b0ssiz 0:fc963e08d580 127
b0ssiz 0:fc963e08d580 128
b0ssiz 0:fc963e08d580 129
b0ssiz 0:fc963e08d580 130 uint8_t Bear_Communicate::GetKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 0:fc963e08d580 131 {
b0ssiz 0:fc963e08d580 132 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 133
b0ssiz 0:fc963e08d580 134 package.robotId = id;
b0ssiz 0:fc963e08d580 135 package.length = 4;
b0ssiz 0:fc963e08d580 136 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 137 package.parameter[0]=GET_KP;
b0ssiz 0:fc963e08d580 138 package.parameter[1]=0x03;
b0ssiz 0:fc963e08d580 139
b0ssiz 0:fc963e08d580 140
b0ssiz 0:fc963e08d580 141 rs485_dirc=1;
b0ssiz 0:fc963e08d580 142 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 143 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 144
b0ssiz 0:fc963e08d580 145 rs485_dirc=0;
b0ssiz 0:fc963e08d580 146 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 147 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 148 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 149
b0ssiz 0:fc963e08d580 150 *Kp=package.parameter[0];
b0ssiz 0:fc963e08d580 151 *Ki=package.parameter[1];
b0ssiz 0:fc963e08d580 152 *Kd=package.parameter[2];
b0ssiz 0:fc963e08d580 153 }
b0ssiz 0:fc963e08d580 154 return status;
b0ssiz 0:fc963e08d580 155 }
b0ssiz 0:fc963e08d580 156
b0ssiz 0:fc963e08d580 157
b0ssiz 0:fc963e08d580 158
b0ssiz 0:fc963e08d580 159 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
b0ssiz 0:fc963e08d580 160
b0ssiz 0:fc963e08d580 161
b0ssiz 0:fc963e08d580 162 uint8_t Bear_Communicate::SetMargin(uint8_t id,float margin)
b0ssiz 0:fc963e08d580 163 {
b0ssiz 0:fc963e08d580 164 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 165
b0ssiz 0:fc963e08d580 166 package.robotId = id;
b0ssiz 0:fc963e08d580 167 package.length = 4;
b0ssiz 0:fc963e08d580 168 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 169 package.parameter[0]=SET_MARGIN;
b0ssiz 0:fc963e08d580 170 package.parameter[1]=margin;
b0ssiz 0:fc963e08d580 171
b0ssiz 0:fc963e08d580 172 rs485_dirc=1;
b0ssiz 0:fc963e08d580 173 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 174 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 175 }
b0ssiz 0:fc963e08d580 176
b0ssiz 0:fc963e08d580 177
b0ssiz 0:fc963e08d580 178
b0ssiz 0:fc963e08d580 179 uint8_t Bear_Communicate::GetMargin(uint8_t id,float *margin)
b0ssiz 0:fc963e08d580 180 {
b0ssiz 0:fc963e08d580 181 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 182
b0ssiz 0:fc963e08d580 183 package.robotId = id;
b0ssiz 0:fc963e08d580 184 package.length = 4;
b0ssiz 0:fc963e08d580 185 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 186 package.parameter[0]=GET_MARGIN;
b0ssiz 0:fc963e08d580 187 package.parameter[1]=0x01;
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 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 193
b0ssiz 0:fc963e08d580 194 rs485_dirc=0;
b0ssiz 0:fc963e08d580 195 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 196 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 197 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 198
b0ssiz 0:fc963e08d580 199 *margin=package.parameter[0];
b0ssiz 0:fc963e08d580 200 }
b0ssiz 0:fc963e08d580 201 return status;
b0ssiz 0:fc963e08d580 202 }
b0ssiz 0:fc963e08d580 203
b0ssiz 0:fc963e08d580 204
b0ssiz 0:fc963e08d580 205
b0ssiz 0:fc963e08d580 206 uint8_t Bear_Communicate::SetHeight(uint8_t id,float height)
b0ssiz 0:fc963e08d580 207 {
b0ssiz 0:fc963e08d580 208 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 209
b0ssiz 0:fc963e08d580 210 package.robotId = id;
b0ssiz 0:fc963e08d580 211 package.length = 4;
b0ssiz 0:fc963e08d580 212 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 213 package.parameter[0]=SET_HEIGHT;
b0ssiz 0:fc963e08d580 214 package.parameter[1]=height;
b0ssiz 0:fc963e08d580 215
b0ssiz 0:fc963e08d580 216 rs485_dirc=1;
b0ssiz 0:fc963e08d580 217 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 218 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 219 }
b0ssiz 0:fc963e08d580 220
b0ssiz 0:fc963e08d580 221
b0ssiz 0:fc963e08d580 222
b0ssiz 0:fc963e08d580 223 uint8_t Bear_Communicate::GetHeight(uint8_t id,float *height)
b0ssiz 0:fc963e08d580 224 {
b0ssiz 0:fc963e08d580 225 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 226
b0ssiz 0:fc963e08d580 227 package.robotId = id;
b0ssiz 0:fc963e08d580 228 package.length = 4;
b0ssiz 0:fc963e08d580 229 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 230 package.parameter[0]=GET_HEIGHT;
b0ssiz 0:fc963e08d580 231 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 232
b0ssiz 0:fc963e08d580 233 rs485_dirc=1;
b0ssiz 0:fc963e08d580 234 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 235 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 236
b0ssiz 0:fc963e08d580 237 rs485_dirc=0;
b0ssiz 0:fc963e08d580 238 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 239 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 240 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 241
b0ssiz 0:fc963e08d580 242 *height = package.parameter[0];
b0ssiz 0:fc963e08d580 243
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 uint8_t Bear_Communicate::SetWheelPos(uint8_t id,float WheelPos)
b0ssiz 0:fc963e08d580 251 {
b0ssiz 0:fc963e08d580 252 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 253
b0ssiz 0:fc963e08d580 254 package.robotId = id;
b0ssiz 0:fc963e08d580 255 package.length = 4;
b0ssiz 0:fc963e08d580 256 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 257 package.parameter[0]=SET_WHEELPOS;
b0ssiz 0:fc963e08d580 258 package.parameter[1]=WheelPos;
b0ssiz 0:fc963e08d580 259
b0ssiz 0:fc963e08d580 260 rs485_dirc=1;
b0ssiz 0:fc963e08d580 261 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 262 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 263 }
b0ssiz 0:fc963e08d580 264
b0ssiz 0:fc963e08d580 265
b0ssiz 0:fc963e08d580 266
b0ssiz 0:fc963e08d580 267 uint8_t Bear_Communicate::GetWheelPos(uint8_t id,float *WheelPos)
b0ssiz 0:fc963e08d580 268 {
b0ssiz 0:fc963e08d580 269 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 270
b0ssiz 0:fc963e08d580 271 package.robotId = id;
b0ssiz 0:fc963e08d580 272 package.length = 4;
b0ssiz 0:fc963e08d580 273 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 274 package.parameter[0]=GET_WHEELPOS;
b0ssiz 0:fc963e08d580 275 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 276
b0ssiz 0:fc963e08d580 277 rs485_dirc=1;
b0ssiz 0:fc963e08d580 278 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 279 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 280
b0ssiz 0:fc963e08d580 281 rs485_dirc=0;
b0ssiz 0:fc963e08d580 282 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 283 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 284 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 285
b0ssiz 0:fc963e08d580 286 *WheelPos = package.parameter[0];
b0ssiz 0:fc963e08d580 287 }
b0ssiz 0:fc963e08d580 288 return status;
b0ssiz 0:fc963e08d580 289 }
b0ssiz 0:fc963e08d580 290
b0ssiz 0:fc963e08d580 291 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 292 {
b0ssiz 0:fc963e08d580 293 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 294
b0ssiz 0:fc963e08d580 295 package.robotId = id;
b0ssiz 0:fc963e08d580 296 package.length = 9;
b0ssiz 0:fc963e08d580 297 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 298 package.parameter[0]=SET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 299 package.parameter[1]=x_max;
b0ssiz 0:fc963e08d580 300 package.parameter[2]=y_max;
b0ssiz 0:fc963e08d580 301 package.parameter[3]=z_max;
b0ssiz 0:fc963e08d580 302 package.parameter[4]=x_min;
b0ssiz 0:fc963e08d580 303 package.parameter[5]=y_min;
b0ssiz 0:fc963e08d580 304 package.parameter[6]=z_min;
b0ssiz 0:fc963e08d580 305
b0ssiz 0:fc963e08d580 306 rs485_dirc=1;
b0ssiz 0:fc963e08d580 307 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 308 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 309 }
b0ssiz 0:fc963e08d580 310
b0ssiz 0:fc963e08d580 311 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 312 {
b0ssiz 0:fc963e08d580 313 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 314
b0ssiz 0:fc963e08d580 315 package.robotId = id;
b0ssiz 0:fc963e08d580 316 package.length = 4;
b0ssiz 0:fc963e08d580 317 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 318 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 319 package.parameter[1]=0x06;
b0ssiz 0:fc963e08d580 320
b0ssiz 0:fc963e08d580 321 rs485_dirc=1;
b0ssiz 0:fc963e08d580 322 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 323 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 324
b0ssiz 0:fc963e08d580 325 rs485_dirc=0;
b0ssiz 0:fc963e08d580 326 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 327 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 328 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 329
b0ssiz 0:fc963e08d580 330 *x_max=package.parameter[0];
b0ssiz 0:fc963e08d580 331 *y_max=package.parameter[1];
b0ssiz 0:fc963e08d580 332 *z_max=package.parameter[2];
b0ssiz 0:fc963e08d580 333 *x_min=package.parameter[3];
b0ssiz 0:fc963e08d580 334 *y_min=package.parameter[4];
b0ssiz 0:fc963e08d580 335 *z_min=package.parameter[5];
b0ssiz 0:fc963e08d580 336
b0ssiz 0:fc963e08d580 337 }
b0ssiz 0:fc963e08d580 338 return status;
b0ssiz 0:fc963e08d580 339 }
b0ssiz 0:fc963e08d580 340
b0ssiz 0:fc963e08d580 341
b0ssiz 0:fc963e08d580 342
b0ssiz 0:fc963e08d580 343 uint8_t Bear_Communicate::SetOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 0:fc963e08d580 344 {
b0ssiz 0:fc963e08d580 345 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 346
b0ssiz 0:fc963e08d580 347 package.robotId = id;
b0ssiz 0:fc963e08d580 348 package.length = 5;
b0ssiz 0:fc963e08d580 349 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 350 package.parameter[0]=SET_OFFSET_Y;
b0ssiz 0:fc963e08d580 351 package.parameter[1]=offset_y;
b0ssiz 0:fc963e08d580 352 package.parameter[2]=offset_z;
b0ssiz 0:fc963e08d580 353
b0ssiz 0:fc963e08d580 354 rs485_dirc=1;
b0ssiz 0:fc963e08d580 355 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 356 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 357 }
b0ssiz 0:fc963e08d580 358
b0ssiz 0:fc963e08d580 359
b0ssiz 0:fc963e08d580 360
b0ssiz 0:fc963e08d580 361 uint8_t Bear_Communicate::GetOffset(uint8_t id,float *offset_y,float *offset_z)
b0ssiz 0:fc963e08d580 362 {
b0ssiz 0:fc963e08d580 363 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 364
b0ssiz 0:fc963e08d580 365 package.robotId = id;
b0ssiz 0:fc963e08d580 366 package.length = 4;
b0ssiz 0:fc963e08d580 367 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 368 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 369 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 370
b0ssiz 0:fc963e08d580 371 rs485_dirc=1;
b0ssiz 0:fc963e08d580 372 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 373 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 374
b0ssiz 0:fc963e08d580 375 rs485_dirc=0;
b0ssiz 0:fc963e08d580 376 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 377 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 378 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 379
b0ssiz 0:fc963e08d580 380 *offset_y=package.parameter[0];
b0ssiz 0:fc963e08d580 381 *offset_z=package.parameter[1];
b0ssiz 0:fc963e08d580 382 }
b0ssiz 0:fc963e08d580 383 return status;
b0ssiz 0:fc963e08d580 384 }
b0ssiz 0:fc963e08d580 385
b0ssiz 0:fc963e08d580 386
b0ssiz 0:fc963e08d580 387
b0ssiz 0:fc963e08d580 388 uint8_t Bear_Communicate::SetBodyLength(uint8_t id,float body_length)
b0ssiz 0:fc963e08d580 389 {
b0ssiz 0:fc963e08d580 390 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 391
b0ssiz 0:fc963e08d580 392 package.robotId = id;
b0ssiz 0:fc963e08d580 393 package.length = 4;
b0ssiz 0:fc963e08d580 394 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 395 package.parameter[0]=SET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 396 package.parameter[1]=body_length;
b0ssiz 0:fc963e08d580 397
b0ssiz 0:fc963e08d580 398 rs485_dirc=1;
b0ssiz 0:fc963e08d580 399 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 400 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 401 }
b0ssiz 0:fc963e08d580 402
b0ssiz 0:fc963e08d580 403
b0ssiz 0:fc963e08d580 404
b0ssiz 0:fc963e08d580 405
b0ssiz 0:fc963e08d580 406 uint8_t Bear_Communicate::GetBodyLength(uint8_t id,float *body_length)
b0ssiz 0:fc963e08d580 407 {
b0ssiz 0:fc963e08d580 408 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 409
b0ssiz 0:fc963e08d580 410 package.robotId = id;
b0ssiz 0:fc963e08d580 411 package.length = 4;
b0ssiz 0:fc963e08d580 412 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 413 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 414 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 415
b0ssiz 0:fc963e08d580 416 rs485_dirc=1;
b0ssiz 0:fc963e08d580 417 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 418 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 419
b0ssiz 0:fc963e08d580 420 rs485_dirc=0;
b0ssiz 0:fc963e08d580 421 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 422 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 423 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 424
b0ssiz 0:fc963e08d580 425 *body_length=package.parameter[0];
b0ssiz 0:fc963e08d580 426 }
b0ssiz 0:fc963e08d580 427 return status;
b0ssiz 0:fc963e08d580 428 }
b0ssiz 0:fc963e08d580 429
b0ssiz 0:fc963e08d580 430 uint8_t Bear_Communicate::SetAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 0:fc963e08d580 431 {
b0ssiz 0:fc963e08d580 432 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 433
b0ssiz 0:fc963e08d580 434 package.robotId = id;
b0ssiz 0:fc963e08d580 435 package.length = 5;
b0ssiz 0:fc963e08d580 436 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 437 package.parameter[0]=SET_MAX_ANGLE;
b0ssiz 0:fc963e08d580 438 package.parameter[1]=max_angle;
b0ssiz 0:fc963e08d580 439 package.parameter[2]=min_angle;
b0ssiz 0:fc963e08d580 440
b0ssiz 0:fc963e08d580 441 rs485_dirc=1;
b0ssiz 0:fc963e08d580 442 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 443 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 444 }
b0ssiz 0:fc963e08d580 445
b0ssiz 0:fc963e08d580 446
b0ssiz 0:fc963e08d580 447
b0ssiz 0:fc963e08d580 448
b0ssiz 0:fc963e08d580 449 uint8_t Bear_Communicate::GetAngleRange(uint8_t id,float *max_angle,float *min_angle)
b0ssiz 0:fc963e08d580 450 {
b0ssiz 0:fc963e08d580 451
b0ssiz 0:fc963e08d580 452 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 453
b0ssiz 0:fc963e08d580 454 package.robotId = id;
b0ssiz 0:fc963e08d580 455 package.length = 4;
b0ssiz 0:fc963e08d580 456 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 457 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 458 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 459
b0ssiz 0:fc963e08d580 460 rs485_dirc=1;
b0ssiz 0:fc963e08d580 461 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 462 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 463
b0ssiz 0:fc963e08d580 464 rs485_dirc=0;
b0ssiz 0:fc963e08d580 465 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 466 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 467 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 468
b0ssiz 0:fc963e08d580 469 *max_angle=package.parameter[0];
b0ssiz 0:fc963e08d580 470 *min_angle=package.parameter[1];
b0ssiz 0:fc963e08d580 471
b0ssiz 0:fc963e08d580 472 }
b0ssiz 0:fc963e08d580 473 return status;
b0ssiz 0:fc963e08d580 474 }