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@0:fc963e08d580, 2015-12-23 (annotated)

Committer:
b0ssiz
Date:
Wed Dec 23 13:05:53 2015 +0000
Revision:
0:fc963e08d580
Child:
2:d17ccaf938f6
Create BEAR_Protocol;

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 0:fc963e08d580 3
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 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 72 printf("up_angle : %f low_angle : %f \r\n", *up_angle,*low_angle);
b0ssiz 0:fc963e08d580 73 #endif
b0ssiz 0:fc963e08d580 74 }
b0ssiz 0:fc963e08d580 75 return status;
b0ssiz 0:fc963e08d580 76 }
b0ssiz 0:fc963e08d580 77
b0ssiz 0:fc963e08d580 78
b0ssiz 0:fc963e08d580 79
b0ssiz 0:fc963e08d580 80 uint8_t Bear_Communicate::SetKp(uint8_t id,float Kp)
b0ssiz 0:fc963e08d580 81 {
b0ssiz 0:fc963e08d580 82 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 83
b0ssiz 0:fc963e08d580 84 package.robotId = id;
b0ssiz 0:fc963e08d580 85 package.length = 4;
b0ssiz 0:fc963e08d580 86 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 87 package.parameter[0]=SET_KP;
b0ssiz 0:fc963e08d580 88 package.parameter[1]=Kp;
b0ssiz 0:fc963e08d580 89
b0ssiz 0:fc963e08d580 90 rs485_dirc=1;
b0ssiz 0:fc963e08d580 91 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 92 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 93 }
b0ssiz 0:fc963e08d580 94
b0ssiz 0:fc963e08d580 95
b0ssiz 0:fc963e08d580 96
b0ssiz 0:fc963e08d580 97 uint8_t Bear_Communicate::SetKi(uint8_t id,float Ki)
b0ssiz 0:fc963e08d580 98 {
b0ssiz 0:fc963e08d580 99 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 100
b0ssiz 0:fc963e08d580 101 package.robotId = id;
b0ssiz 0:fc963e08d580 102 package.length = 4;
b0ssiz 0:fc963e08d580 103 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 104 package.parameter[0]=SET_KI;
b0ssiz 0:fc963e08d580 105 package.parameter[1]=Ki;
b0ssiz 0:fc963e08d580 106
b0ssiz 0:fc963e08d580 107
b0ssiz 0:fc963e08d580 108 rs485_dirc=1;
b0ssiz 0:fc963e08d580 109 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 110 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 111 }
b0ssiz 0:fc963e08d580 112
b0ssiz 0:fc963e08d580 113
b0ssiz 0:fc963e08d580 114
b0ssiz 0:fc963e08d580 115 uint8_t Bear_Communicate::SetKd(uint8_t id,float Kd)
b0ssiz 0:fc963e08d580 116 {
b0ssiz 0:fc963e08d580 117 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 118
b0ssiz 0:fc963e08d580 119 package.robotId = id;
b0ssiz 0:fc963e08d580 120 package.length = 4;
b0ssiz 0:fc963e08d580 121 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 122 package.parameter[0]=SET_KD;
b0ssiz 0:fc963e08d580 123 package.parameter[1]=Kd;
b0ssiz 0:fc963e08d580 124
b0ssiz 0:fc963e08d580 125
b0ssiz 0:fc963e08d580 126 rs485_dirc=1;
b0ssiz 0:fc963e08d580 127 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 128 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 129 }
b0ssiz 0:fc963e08d580 130
b0ssiz 0:fc963e08d580 131
b0ssiz 0:fc963e08d580 132
b0ssiz 0:fc963e08d580 133 uint8_t Bear_Communicate::GetKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 0:fc963e08d580 134 {
b0ssiz 0:fc963e08d580 135 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 136
b0ssiz 0:fc963e08d580 137 package.robotId = id;
b0ssiz 0:fc963e08d580 138 package.length = 4;
b0ssiz 0:fc963e08d580 139 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 140 package.parameter[0]=GET_KP;
b0ssiz 0:fc963e08d580 141 package.parameter[1]=0x03;
b0ssiz 0:fc963e08d580 142
b0ssiz 0:fc963e08d580 143
b0ssiz 0:fc963e08d580 144 rs485_dirc=1;
b0ssiz 0:fc963e08d580 145 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 146 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 147
b0ssiz 0:fc963e08d580 148 rs485_dirc=0;
b0ssiz 0:fc963e08d580 149 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 150 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 151 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 152
b0ssiz 0:fc963e08d580 153 *Kp=package.parameter[0];
b0ssiz 0:fc963e08d580 154 *Ki=package.parameter[1];
b0ssiz 0:fc963e08d580 155 *Kd=package.parameter[2];
b0ssiz 0:fc963e08d580 156 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 157 //printf("Kp : 0x%02X Ki : 0x%02X Kd : 0x%02X \r\n", *Kp,*Ki,*Kd);
b0ssiz 0:fc963e08d580 158 #endif
b0ssiz 0:fc963e08d580 159 }
b0ssiz 0:fc963e08d580 160 return status;
b0ssiz 0:fc963e08d580 161 }
b0ssiz 0:fc963e08d580 162
b0ssiz 0:fc963e08d580 163
b0ssiz 0:fc963e08d580 164
b0ssiz 0:fc963e08d580 165 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
b0ssiz 0:fc963e08d580 166
b0ssiz 0:fc963e08d580 167
b0ssiz 0:fc963e08d580 168 uint8_t Bear_Communicate::SetMargin(uint8_t id,float margin)
b0ssiz 0:fc963e08d580 169 {
b0ssiz 0:fc963e08d580 170 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 171
b0ssiz 0:fc963e08d580 172 package.robotId = id;
b0ssiz 0:fc963e08d580 173 package.length = 4;
b0ssiz 0:fc963e08d580 174 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 175 package.parameter[0]=SET_MARGIN;
b0ssiz 0:fc963e08d580 176 package.parameter[1]=margin;
b0ssiz 0:fc963e08d580 177
b0ssiz 0:fc963e08d580 178 rs485_dirc=1;
b0ssiz 0:fc963e08d580 179 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 180 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 181 }
b0ssiz 0:fc963e08d580 182
b0ssiz 0:fc963e08d580 183
b0ssiz 0:fc963e08d580 184
b0ssiz 0:fc963e08d580 185 uint8_t Bear_Communicate::GetMargin(uint8_t id,float *margin)
b0ssiz 0:fc963e08d580 186 {
b0ssiz 0:fc963e08d580 187 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 188
b0ssiz 0:fc963e08d580 189 package.robotId = id;
b0ssiz 0:fc963e08d580 190 package.length = 4;
b0ssiz 0:fc963e08d580 191 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 192 package.parameter[0]=GET_MARGIN;
b0ssiz 0:fc963e08d580 193 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 194
b0ssiz 0:fc963e08d580 195
b0ssiz 0:fc963e08d580 196 rs485_dirc=1;
b0ssiz 0:fc963e08d580 197 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 198 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 199
b0ssiz 0:fc963e08d580 200 rs485_dirc=0;
b0ssiz 0:fc963e08d580 201 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 202 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 203 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 204
b0ssiz 0:fc963e08d580 205 *margin=package.parameter[0];
b0ssiz 0:fc963e08d580 206 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 207 //printf("Margin : 0x%02X \r\n", *margin);
b0ssiz 0:fc963e08d580 208 #endif
b0ssiz 0:fc963e08d580 209 }
b0ssiz 0:fc963e08d580 210 return status;
b0ssiz 0:fc963e08d580 211 }
b0ssiz 0:fc963e08d580 212
b0ssiz 0:fc963e08d580 213
b0ssiz 0:fc963e08d580 214
b0ssiz 0:fc963e08d580 215 uint8_t Bear_Communicate::SetHeight(uint8_t id,float height)
b0ssiz 0:fc963e08d580 216 {
b0ssiz 0:fc963e08d580 217 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 218
b0ssiz 0:fc963e08d580 219 package.robotId = id;
b0ssiz 0:fc963e08d580 220 package.length = 4;
b0ssiz 0:fc963e08d580 221 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 222 package.parameter[0]=SET_HEIGHT;
b0ssiz 0:fc963e08d580 223 package.parameter[1]=height;
b0ssiz 0:fc963e08d580 224
b0ssiz 0:fc963e08d580 225 rs485_dirc=1;
b0ssiz 0:fc963e08d580 226 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 227 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 228 }
b0ssiz 0:fc963e08d580 229
b0ssiz 0:fc963e08d580 230
b0ssiz 0:fc963e08d580 231
b0ssiz 0:fc963e08d580 232 uint8_t Bear_Communicate::GetHeight(uint8_t id,float *height)
b0ssiz 0:fc963e08d580 233 {
b0ssiz 0:fc963e08d580 234 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 235
b0ssiz 0:fc963e08d580 236 package.robotId = id;
b0ssiz 0:fc963e08d580 237 package.length = 4;
b0ssiz 0:fc963e08d580 238 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 239 package.parameter[0]=GET_HEIGHT;
b0ssiz 0:fc963e08d580 240 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 241
b0ssiz 0:fc963e08d580 242 rs485_dirc=1;
b0ssiz 0:fc963e08d580 243 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 244 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 245
b0ssiz 0:fc963e08d580 246 rs485_dirc=0;
b0ssiz 0:fc963e08d580 247 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 248 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 249 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 250
b0ssiz 0:fc963e08d580 251 *height = package.parameter[0];
b0ssiz 0:fc963e08d580 252
b0ssiz 0:fc963e08d580 253 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 254 //printf("Height : 0x%02X \r\n", *height);
b0ssiz 0:fc963e08d580 255 #endif
b0ssiz 0:fc963e08d580 256 }
b0ssiz 0:fc963e08d580 257 return status;
b0ssiz 0:fc963e08d580 258 }
b0ssiz 0:fc963e08d580 259
b0ssiz 0:fc963e08d580 260
b0ssiz 0:fc963e08d580 261
b0ssiz 0:fc963e08d580 262 uint8_t Bear_Communicate::SetWheelPos(uint8_t id,float WheelPos)
b0ssiz 0:fc963e08d580 263 {
b0ssiz 0:fc963e08d580 264 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 265
b0ssiz 0:fc963e08d580 266 package.robotId = id;
b0ssiz 0:fc963e08d580 267 package.length = 4;
b0ssiz 0:fc963e08d580 268 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 269 package.parameter[0]=SET_WHEELPOS;
b0ssiz 0:fc963e08d580 270 package.parameter[1]=WheelPos;
b0ssiz 0:fc963e08d580 271
b0ssiz 0:fc963e08d580 272 rs485_dirc=1;
b0ssiz 0:fc963e08d580 273 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 274 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 275 }
b0ssiz 0:fc963e08d580 276
b0ssiz 0:fc963e08d580 277
b0ssiz 0:fc963e08d580 278
b0ssiz 0:fc963e08d580 279 uint8_t Bear_Communicate::GetWheelPos(uint8_t id,float *WheelPos)
b0ssiz 0:fc963e08d580 280 {
b0ssiz 0:fc963e08d580 281 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 282
b0ssiz 0:fc963e08d580 283 package.robotId = id;
b0ssiz 0:fc963e08d580 284 package.length = 4;
b0ssiz 0:fc963e08d580 285 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 286 package.parameter[0]=GET_WHEELPOS;
b0ssiz 0:fc963e08d580 287 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 288
b0ssiz 0:fc963e08d580 289 rs485_dirc=1;
b0ssiz 0:fc963e08d580 290 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 291 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 292
b0ssiz 0:fc963e08d580 293 rs485_dirc=0;
b0ssiz 0:fc963e08d580 294 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 295 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 296 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 297
b0ssiz 0:fc963e08d580 298 *WheelPos = package.parameter[0];
b0ssiz 0:fc963e08d580 299
b0ssiz 0:fc963e08d580 300 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 301 //printf("Wheel_position : 0x%02X \r\n", *WheelPos);
b0ssiz 0:fc963e08d580 302 #endif
b0ssiz 0:fc963e08d580 303 }
b0ssiz 0:fc963e08d580 304 return status;
b0ssiz 0:fc963e08d580 305 }
b0ssiz 0:fc963e08d580 306
b0ssiz 0:fc963e08d580 307 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 308 {
b0ssiz 0:fc963e08d580 309 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 310
b0ssiz 0:fc963e08d580 311 package.robotId = id;
b0ssiz 0:fc963e08d580 312 package.length = 9;
b0ssiz 0:fc963e08d580 313 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 314 package.parameter[0]=SET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 315 package.parameter[1]=x_max;
b0ssiz 0:fc963e08d580 316 package.parameter[2]=y_max;
b0ssiz 0:fc963e08d580 317 package.parameter[3]=z_max;
b0ssiz 0:fc963e08d580 318 package.parameter[4]=x_min;
b0ssiz 0:fc963e08d580 319 package.parameter[5]=y_min;
b0ssiz 0:fc963e08d580 320 package.parameter[6]=z_min;
b0ssiz 0:fc963e08d580 321
b0ssiz 0:fc963e08d580 322 rs485_dirc=1;
b0ssiz 0:fc963e08d580 323 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 324 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 325 }
b0ssiz 0:fc963e08d580 326
b0ssiz 0:fc963e08d580 327 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 328 {
b0ssiz 0:fc963e08d580 329 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 330
b0ssiz 0:fc963e08d580 331 package.robotId = id;
b0ssiz 0:fc963e08d580 332 package.length = 4;
b0ssiz 0:fc963e08d580 333 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 334 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 335 package.parameter[1]=0x06;
b0ssiz 0:fc963e08d580 336
b0ssiz 0:fc963e08d580 337 rs485_dirc=1;
b0ssiz 0:fc963e08d580 338 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 339 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 340
b0ssiz 0:fc963e08d580 341 rs485_dirc=0;
b0ssiz 0:fc963e08d580 342 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 343 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 344 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 345
b0ssiz 0:fc963e08d580 346 *x_max=package.parameter[0];
b0ssiz 0:fc963e08d580 347 *y_max=package.parameter[1];
b0ssiz 0:fc963e08d580 348 *z_max=package.parameter[2];
b0ssiz 0:fc963e08d580 349 *x_min=package.parameter[3];
b0ssiz 0:fc963e08d580 350 *y_min=package.parameter[4];
b0ssiz 0:fc963e08d580 351 *z_min=package.parameter[5];
b0ssiz 0:fc963e08d580 352
b0ssiz 0:fc963e08d580 353
b0ssiz 0:fc963e08d580 354 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 355 printf("X_max : 0x%02X Y_max : 0x%02X Z_max : 0x%02X X_min : 0x%02X Y_min : 0x%02X Z_min : 0x%02X \r\n",x_max,y_max,z_max,x_min,y_min,z_min);
b0ssiz 0:fc963e08d580 356 #endif
b0ssiz 0:fc963e08d580 357 }
b0ssiz 0:fc963e08d580 358 return status;
b0ssiz 0:fc963e08d580 359 }
b0ssiz 0:fc963e08d580 360
b0ssiz 0:fc963e08d580 361
b0ssiz 0:fc963e08d580 362
b0ssiz 0:fc963e08d580 363 uint8_t Bear_Communicate::SetOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 0:fc963e08d580 364 {
b0ssiz 0:fc963e08d580 365 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 366
b0ssiz 0:fc963e08d580 367 package.robotId = id;
b0ssiz 0:fc963e08d580 368 package.length = 5;
b0ssiz 0:fc963e08d580 369 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 370 package.parameter[0]=SET_OFFSET_Y;
b0ssiz 0:fc963e08d580 371 package.parameter[1]=offset_y;
b0ssiz 0:fc963e08d580 372 package.parameter[2]=offset_z;
b0ssiz 0:fc963e08d580 373
b0ssiz 0:fc963e08d580 374 rs485_dirc=1;
b0ssiz 0:fc963e08d580 375 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 376 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 377 }
b0ssiz 0:fc963e08d580 378
b0ssiz 0:fc963e08d580 379
b0ssiz 0:fc963e08d580 380
b0ssiz 0:fc963e08d580 381 uint8_t Bear_Communicate::GetOffset(uint8_t id,float *offset_y,float *offset_z)
b0ssiz 0:fc963e08d580 382 {
b0ssiz 0:fc963e08d580 383 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 384
b0ssiz 0:fc963e08d580 385 package.robotId = id;
b0ssiz 0:fc963e08d580 386 package.length = 4;
b0ssiz 0:fc963e08d580 387 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 388 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 389 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 390
b0ssiz 0:fc963e08d580 391 rs485_dirc=1;
b0ssiz 0:fc963e08d580 392 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 393 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 394
b0ssiz 0:fc963e08d580 395 rs485_dirc=0;
b0ssiz 0:fc963e08d580 396 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 397 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 398 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 399
b0ssiz 0:fc963e08d580 400 *offset_y=package.parameter[0];
b0ssiz 0:fc963e08d580 401 *offset_z=package.parameter[1];
b0ssiz 0:fc963e08d580 402
b0ssiz 0:fc963e08d580 403 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 404 //printf("Offset_Y : 0x%02X Offset_Z : 0x%02X \r\n",*offset_y,*offset_z);
b0ssiz 0:fc963e08d580 405 #endif
b0ssiz 0:fc963e08d580 406 }
b0ssiz 0:fc963e08d580 407 return status;
b0ssiz 0:fc963e08d580 408 }
b0ssiz 0:fc963e08d580 409
b0ssiz 0:fc963e08d580 410
b0ssiz 0:fc963e08d580 411
b0ssiz 0:fc963e08d580 412 uint8_t Bear_Communicate::SetBodyLength(uint8_t id,float body_length)
b0ssiz 0:fc963e08d580 413 {
b0ssiz 0:fc963e08d580 414 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 415
b0ssiz 0:fc963e08d580 416 package.robotId = id;
b0ssiz 0:fc963e08d580 417 package.length = 4;
b0ssiz 0:fc963e08d580 418 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 419 package.parameter[0]=SET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 420 package.parameter[1]=body_length;
b0ssiz 0:fc963e08d580 421
b0ssiz 0:fc963e08d580 422 rs485_dirc=1;
b0ssiz 0:fc963e08d580 423 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 424 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 425 }
b0ssiz 0:fc963e08d580 426
b0ssiz 0:fc963e08d580 427
b0ssiz 0:fc963e08d580 428
b0ssiz 0:fc963e08d580 429
b0ssiz 0:fc963e08d580 430 uint8_t Bear_Communicate::GetBodyLength(uint8_t id,float *body_length)
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 = 4;
b0ssiz 0:fc963e08d580 436 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 437 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 438 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 439
b0ssiz 0:fc963e08d580 440 rs485_dirc=1;
b0ssiz 0:fc963e08d580 441 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 442 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 443
b0ssiz 0:fc963e08d580 444 rs485_dirc=0;
b0ssiz 0:fc963e08d580 445 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 446 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 447 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 448
b0ssiz 0:fc963e08d580 449 *body_length=package.parameter[0];
b0ssiz 0:fc963e08d580 450
b0ssiz 0:fc963e08d580 451 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 452 //printf("Body_Length : 0x%02X \r\n",*body_length);
b0ssiz 0:fc963e08d580 453 #endif
b0ssiz 0:fc963e08d580 454 }
b0ssiz 0:fc963e08d580 455 return status;
b0ssiz 0:fc963e08d580 456 }
b0ssiz 0:fc963e08d580 457
b0ssiz 0:fc963e08d580 458 uint8_t Bear_Communicate::SetAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 0:fc963e08d580 459 {
b0ssiz 0:fc963e08d580 460 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 461
b0ssiz 0:fc963e08d580 462 package.robotId = id;
b0ssiz 0:fc963e08d580 463 package.length = 5;
b0ssiz 0:fc963e08d580 464 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 465 package.parameter[0]=SET_MAX_ANGLE;
b0ssiz 0:fc963e08d580 466 package.parameter[1]=max_angle;
b0ssiz 0:fc963e08d580 467 package.parameter[2]=min_angle;
b0ssiz 0:fc963e08d580 468
b0ssiz 0:fc963e08d580 469 rs485_dirc=1;
b0ssiz 0:fc963e08d580 470 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 471 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 472 }
b0ssiz 0:fc963e08d580 473
b0ssiz 0:fc963e08d580 474
b0ssiz 0:fc963e08d580 475
b0ssiz 0:fc963e08d580 476
b0ssiz 0:fc963e08d580 477 uint8_t Bear_Communicate::GetAngleRange(uint8_t id,float *max_angle,float *min_angle)
b0ssiz 0:fc963e08d580 478 {
b0ssiz 0:fc963e08d580 479
b0ssiz 0:fc963e08d580 480 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 481
b0ssiz 0:fc963e08d580 482 package.robotId = id;
b0ssiz 0:fc963e08d580 483 package.length = 4;
b0ssiz 0:fc963e08d580 484 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 485 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 486 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 487
b0ssiz 0:fc963e08d580 488 rs485_dirc=1;
b0ssiz 0:fc963e08d580 489 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 490 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 491
b0ssiz 0:fc963e08d580 492 rs485_dirc=0;
b0ssiz 0:fc963e08d580 493 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 494 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 495 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 496
b0ssiz 0:fc963e08d580 497 *max_angle=package.parameter[0];
b0ssiz 0:fc963e08d580 498 *min_angle=package.parameter[1];
b0ssiz 0:fc963e08d580 499
b0ssiz 0:fc963e08d580 500 #ifdef SERIAL_DEBUG
b0ssiz 0:fc963e08d580 501 //printf("Max_angle : 0x%02X Min_angle : 0x%02X \r\n",*max_angle,*min,angle);
b0ssiz 0:fc963e08d580 502 #endif
b0ssiz 0:fc963e08d580 503 }
b0ssiz 0:fc963e08d580 504 return status;
b0ssiz 0:fc963e08d580 505 }