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@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?

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 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 }