Secret_Betago / BEAR_Reciever

update protocol.h

Dependencies:   Communication_Robot

Fork of BEAR_Reciever by BE@R lab

Receiver.cpp@8:2d34916ac178, 2016-01-21 (annotated)

Committer:
b0ssiz
Date:
Thu Jan 21 14:59:21 2016 +0000
Revision:
8:2d34916ac178
Parent:
7:a0ea8c127611
Child:
11:b9337e23782e
Update function at Receiver.cpp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
b0ssiz 5:baf5576a14bd 1 #include "mbed.h"
b0ssiz 5:baf5576a14bd 2 #include "Receiver.h"
b0ssiz 7:a0ea8c127611 3
b0ssiz 5:baf5576a14bd 4
b0ssiz 5:baf5576a14bd 5 DigitalOut rs485_dirc(RS485_DIRC);
b0ssiz 5:baf5576a14bd 6
b0ssiz 5:baf5576a14bd 7 Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate)
b0ssiz 5:baf5576a14bd 8 {
b0ssiz 5:baf5576a14bd 9 com = new COMMUNICATION(tx,rx,baudrate);
b0ssiz 5:baf5576a14bd 10 }
b0ssiz 5:baf5576a14bd 11
b0ssiz 8:2d34916ac178 12 uint8_t Bear_Receiver::ReceiveCommand(uint8_t *id,uint8_t *data_array,uint8_t *ins)
b0ssiz 5:baf5576a14bd 13 {
b0ssiz 5:baf5576a14bd 14 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:baf5576a14bd 15
b0ssiz 5:baf5576a14bd 16 rs485_dirc=0;
b0ssiz 5:baf5576a14bd 17 wait_us(RS485_DELAY);
b0ssiz 5:baf5576a14bd 18 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 5:baf5576a14bd 19
b0ssiz 5:baf5576a14bd 20 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 5:baf5576a14bd 21
b0ssiz 8:2d34916ac178 22 *id = package.robotId;
b0ssiz 6:f6529ad2a70d 23 data_array = package.parameter;
b0ssiz 8:2d34916ac178 24 *ins=package.instructionErrorId;
b0ssiz 5:baf5576a14bd 25
b0ssiz 5:baf5576a14bd 26 }
b0ssiz 5:baf5576a14bd 27 return status;
b0ssiz 5:baf5576a14bd 28 }
b0ssiz 5:baf5576a14bd 29
b0ssiz 5:baf5576a14bd 30
b0ssiz 5:baf5576a14bd 31
b0ssiz 7:a0ea8c127611 32
b0ssiz 7:a0ea8c127611 33
b0ssiz 5:baf5576a14bd 34 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
b0ssiz 5:baf5576a14bd 35 {
b0ssiz 5:baf5576a14bd 36 float float_buffer;
b0ssiz 5:baf5576a14bd 37 float int_buffer;
b0ssiz 5:baf5576a14bd 38 int16_t integer;
b0ssiz 5:baf5576a14bd 39 int16_t floating_point;
b0ssiz 5:baf5576a14bd 40
b0ssiz 5:baf5576a14bd 41 float_buffer=modf(input_float,&int_buffer);
b0ssiz 5:baf5576a14bd 42 float_buffer*=FLOAT_CONVERTER;
b0ssiz 5:baf5576a14bd 43 integer=(int16_t)int_buffer;
b0ssiz 5:baf5576a14bd 44 floating_point=(int16_t)float_buffer;
b0ssiz 5:baf5576a14bd 45 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
b0ssiz 5:baf5576a14bd 46 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
b0ssiz 5:baf5576a14bd 47 }
b0ssiz 5:baf5576a14bd 48
b0ssiz 5:baf5576a14bd 49
b0ssiz 5:baf5576a14bd 50
b0ssiz 5:baf5576a14bd 51
b0ssiz 5:baf5576a14bd 52 uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
b0ssiz 5:baf5576a14bd 53 {
b0ssiz 5:baf5576a14bd 54 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 5:baf5576a14bd 55 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 5:baf5576a14bd 56 FloatSep(up_angle,IntUpAngle,FloatUpAngle);
b0ssiz 5:baf5576a14bd 57 FloatSep(low_angle,IntLowAngle,FloatLowAngle);
b0ssiz 5:baf5576a14bd 58
b0ssiz 5:baf5576a14bd 59 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:baf5576a14bd 60
b0ssiz 5:baf5576a14bd 61 package.robotId = id;
b0ssiz 5:baf5576a14bd 62 package.length = 10;
b0ssiz 5:baf5576a14bd 63 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:baf5576a14bd 64 package.parameter[0]=IntUpAngle[0];
b0ssiz 5:baf5576a14bd 65 package.parameter[1]=IntUpAngle[1];
b0ssiz 5:baf5576a14bd 66 package.parameter[2]=FloatUpAngle[0];
b0ssiz 5:baf5576a14bd 67 package.parameter[3]=FloatUpAngle[1];
b0ssiz 5:baf5576a14bd 68 package.parameter[4]=IntLowAngle[0];
b0ssiz 5:baf5576a14bd 69 package.parameter[5]=IntLowAngle[1];
b0ssiz 5:baf5576a14bd 70 package.parameter[6]=FloatLowAngle[0];
b0ssiz 5:baf5576a14bd 71 package.parameter[7]=FloatLowAngle[1];
b0ssiz 5:baf5576a14bd 72
b0ssiz 5:baf5576a14bd 73 rs485_dirc=1;
b0ssiz 5:baf5576a14bd 74 wait_us(RS485_DELAY);
b0ssiz 5:baf5576a14bd 75 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 76
b0ssiz 6:f6529ad2a70d 77
b0ssiz 6:f6529ad2a70d 78 }
b0ssiz 6:f6529ad2a70d 79
b0ssiz 6:f6529ad2a70d 80
b0ssiz 6:f6529ad2a70d 81
b0ssiz 6:f6529ad2a70d 82 uint8_t Bear_Receiver::sendUpMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 83 {
b0ssiz 6:f6529ad2a70d 84 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 85 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 86 uint8_t IntKd[2],FloatKd[2];
b0ssiz 7:a0ea8c127611 87
b0ssiz 6:f6529ad2a70d 88 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 89 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 90 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 7:a0ea8c127611 91
b0ssiz 7:a0ea8c127611 92
b0ssiz 6:f6529ad2a70d 93 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 94
b0ssiz 6:f6529ad2a70d 95 package.robotId = id;
b0ssiz 6:f6529ad2a70d 96 package.length = 14;
b0ssiz 6:f6529ad2a70d 97 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 98 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 99 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 100 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 101 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 102 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 103 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 104 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 105 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 106 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 107 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 108 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 109 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 110
b0ssiz 6:f6529ad2a70d 111 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 112 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 113 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 114 }
b0ssiz 6:f6529ad2a70d 115
b0ssiz 6:f6529ad2a70d 116
b0ssiz 6:f6529ad2a70d 117
b0ssiz 6:f6529ad2a70d 118
b0ssiz 6:f6529ad2a70d 119 uint8_t Bear_Receiver::sendLowMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 120 {
b0ssiz 6:f6529ad2a70d 121 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 122 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 123 uint8_t IntKd[2],FloatKd[2];
b0ssiz 7:a0ea8c127611 124
b0ssiz 6:f6529ad2a70d 125 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 126 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 127 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 7:a0ea8c127611 128
b0ssiz 7:a0ea8c127611 129
b0ssiz 6:f6529ad2a70d 130 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 131
b0ssiz 6:f6529ad2a70d 132 package.robotId = id;
b0ssiz 6:f6529ad2a70d 133 package.length = 14;
b0ssiz 6:f6529ad2a70d 134 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 135 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 136 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 137 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 138 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 139 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 140 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 141 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 142 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 143 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 144 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 145 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 146 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 147
b0ssiz 6:f6529ad2a70d 148 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 149 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 150 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 151 }
b0ssiz 8:2d34916ac178 152
b0ssiz 8:2d34916ac178 153
b0ssiz 8:2d34916ac178 154
b0ssiz 8:2d34916ac178 155 uint8_t Bear_Receiver::sendMargin(uint8_t id,float margin)
b0ssiz 8:2d34916ac178 156 {
b0ssiz 8:2d34916ac178 157 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 8:2d34916ac178 158 FloatSep(margin,IntMargin,FloatMargin);
b0ssiz 8:2d34916ac178 159
b0ssiz 8:2d34916ac178 160 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 161
b0ssiz 8:2d34916ac178 162 package.robotId = id;
b0ssiz 8:2d34916ac178 163 package.length = 6;
b0ssiz 8:2d34916ac178 164 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 165 package.parameter[0]=IntMargin[0];
b0ssiz 8:2d34916ac178 166 package.parameter[1]=IntMargin[1];
b0ssiz 8:2d34916ac178 167 package.parameter[2]=FloatMargin[0];
b0ssiz 8:2d34916ac178 168 package.parameter[3]=FloatMargin[1];
b0ssiz 8:2d34916ac178 169
b0ssiz 8:2d34916ac178 170 rs485_dirc=1;
b0ssiz 8:2d34916ac178 171 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 172 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 173
b0ssiz 8:2d34916ac178 174 }
b0ssiz 8:2d34916ac178 175
b0ssiz 8:2d34916ac178 176 uint8_t Bear_Receiver::sendHeight(uint8_t id,float height)
b0ssiz 8:2d34916ac178 177 {
b0ssiz 8:2d34916ac178 178 uint8_t IntHeight[2],FloatHeight[2];
b0ssiz 8:2d34916ac178 179 FloatSep(height,IntHeight,FloatHeight);
b0ssiz 8:2d34916ac178 180
b0ssiz 8:2d34916ac178 181 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 182
b0ssiz 8:2d34916ac178 183 package.robotId = id;
b0ssiz 8:2d34916ac178 184 package.length = 6;
b0ssiz 8:2d34916ac178 185 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 186 package.parameter[0]=IntHeight[0];
b0ssiz 8:2d34916ac178 187 package.parameter[1]=IntHeight[1];
b0ssiz 8:2d34916ac178 188 package.parameter[2]=FloatHeight[0];
b0ssiz 8:2d34916ac178 189 package.parameter[3]=FloatHeight[1];
b0ssiz 8:2d34916ac178 190
b0ssiz 8:2d34916ac178 191 rs485_dirc=1;
b0ssiz 8:2d34916ac178 192 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 193 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 194 }
b0ssiz 8:2d34916ac178 195
b0ssiz 8:2d34916ac178 196 uint8_t Bear_Receiver::sendWheelPos(uint8_t id,float WheelPos)
b0ssiz 8:2d34916ac178 197 {
b0ssiz 8:2d34916ac178 198 uint8_t IntWheelPos[2],FloatWheelPos[2];
b0ssiz 8:2d34916ac178 199 FloatSep(WheelPos,IntWheelPos,FloatWheelPos);
b0ssiz 8:2d34916ac178 200
b0ssiz 8:2d34916ac178 201 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 202
b0ssiz 8:2d34916ac178 203 package.robotId = id;
b0ssiz 8:2d34916ac178 204 package.length = 6;
b0ssiz 8:2d34916ac178 205 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 206 package.parameter[0]=IntWheelPos[0];
b0ssiz 8:2d34916ac178 207 package.parameter[1]=IntWheelPos[1];
b0ssiz 8:2d34916ac178 208 package.parameter[2]=FloatWheelPos[0];
b0ssiz 8:2d34916ac178 209 package.parameter[3]=FloatWheelPos[1];
b0ssiz 8:2d34916ac178 210
b0ssiz 8:2d34916ac178 211 rs485_dirc=1;
b0ssiz 8:2d34916ac178 212 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 213 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 214 }
b0ssiz 8:2d34916ac178 215
b0ssiz 8:2d34916ac178 216 uint8_t Bear_Receiver::sendMagData(uint8_t id,float x_max,float x_min,float y_max,float y_min,float z_max,float z_min)
b0ssiz 8:2d34916ac178 217 {
b0ssiz 8:2d34916ac178 218 uint8_t IntXmax[2],FloatXmax[2];
b0ssiz 8:2d34916ac178 219 uint8_t IntXmin[2],FloatXmin[2];
b0ssiz 8:2d34916ac178 220 uint8_t IntYmax[2],FloatYmax[2];
b0ssiz 8:2d34916ac178 221 uint8_t IntYmin[2],FloatYmin[2];
b0ssiz 8:2d34916ac178 222 uint8_t IntZmax[2],FloatZmax[2];
b0ssiz 8:2d34916ac178 223 uint8_t IntZmin[2],FloatZmin[2];
b0ssiz 8:2d34916ac178 224 FloatSep(x_max,IntXmax,FloatXmax);
b0ssiz 8:2d34916ac178 225 FloatSep(x_min,IntXmin,FloatXmin);
b0ssiz 8:2d34916ac178 226 FloatSep(y_max,IntYmax,FloatYmax);
b0ssiz 8:2d34916ac178 227 FloatSep(y_min,IntYmin,FloatYmin);
b0ssiz 8:2d34916ac178 228 FloatSep(z_max,IntZmax,FloatZmax);
b0ssiz 8:2d34916ac178 229 FloatSep(z_min,IntZmin,FloatZmin);
b0ssiz 8:2d34916ac178 230
b0ssiz 8:2d34916ac178 231 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 232
b0ssiz 8:2d34916ac178 233 package.robotId = id;
b0ssiz 8:2d34916ac178 234 package.length = 26;
b0ssiz 8:2d34916ac178 235 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 236 package.parameter[0]=IntXmax[0];
b0ssiz 8:2d34916ac178 237 package.parameter[1]=IntXmax[1];
b0ssiz 8:2d34916ac178 238 package.parameter[2]=FloatXmax[0];
b0ssiz 8:2d34916ac178 239 package.parameter[3]=FloatXmax[1];
b0ssiz 8:2d34916ac178 240 package.parameter[4]=IntXmin[0];
b0ssiz 8:2d34916ac178 241 package.parameter[5]=IntXmin[1];
b0ssiz 8:2d34916ac178 242 package.parameter[6]=FloatXmin[0];
b0ssiz 8:2d34916ac178 243 package.parameter[7]=FloatXmin[1];
b0ssiz 8:2d34916ac178 244 package.parameter[8]=IntYmax[0];
b0ssiz 8:2d34916ac178 245 package.parameter[9]=IntYmax[1];
b0ssiz 8:2d34916ac178 246 package.parameter[10]=FloatYmax[0];
b0ssiz 8:2d34916ac178 247 package.parameter[11]=FloatYmax[1];
b0ssiz 8:2d34916ac178 248 package.parameter[12]=IntYmin[0];
b0ssiz 8:2d34916ac178 249 package.parameter[13]=IntYmin[1];
b0ssiz 8:2d34916ac178 250 package.parameter[14]=FloatYmin[0];
b0ssiz 8:2d34916ac178 251 package.parameter[15]=FloatYmin[1];
b0ssiz 8:2d34916ac178 252 package.parameter[16]=IntZmax[0];
b0ssiz 8:2d34916ac178 253 package.parameter[17]=IntZmax[1];
b0ssiz 8:2d34916ac178 254 package.parameter[18]=FloatZmax[0];
b0ssiz 8:2d34916ac178 255 package.parameter[19]=FloatZmax[1];
b0ssiz 8:2d34916ac178 256 package.parameter[20]=IntZmin[0];
b0ssiz 8:2d34916ac178 257 package.parameter[21]=IntZmin[1];
b0ssiz 8:2d34916ac178 258 package.parameter[22]=FloatZmin[0];
b0ssiz 8:2d34916ac178 259 package.parameter[23]=FloatZmin[1];
b0ssiz 8:2d34916ac178 260
b0ssiz 8:2d34916ac178 261 rs485_dirc=1;
b0ssiz 8:2d34916ac178 262 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 263 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 264 }
b0ssiz 8:2d34916ac178 265
b0ssiz 8:2d34916ac178 266 uint8_t Bear_Receiver::sendOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 8:2d34916ac178 267 {
b0ssiz 8:2d34916ac178 268 uint8_t IntOffset_Y[2],FloatOffset_Y[2];
b0ssiz 8:2d34916ac178 269 uint8_t IntOffset_Z[2],FloatOffset_Z[2];
b0ssiz 8:2d34916ac178 270 FloatSep(offset_y,IntOffset_Y,FloatOffset_Y);
b0ssiz 8:2d34916ac178 271 FloatSep(offset_z,IntOffset_Z,FloatOffset_Z);
b0ssiz 8:2d34916ac178 272
b0ssiz 8:2d34916ac178 273 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 274
b0ssiz 8:2d34916ac178 275 package.robotId = id;
b0ssiz 8:2d34916ac178 276 package.length = 10;
b0ssiz 8:2d34916ac178 277 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 278 package.parameter[0]=IntOffset_Y[0];
b0ssiz 8:2d34916ac178 279 package.parameter[1]=IntOffset_Y[1];
b0ssiz 8:2d34916ac178 280 package.parameter[2]=FloatOffset_Y[0];
b0ssiz 8:2d34916ac178 281 package.parameter[3]=FloatOffset_Y[1];
b0ssiz 8:2d34916ac178 282 package.parameter[4]=IntOffset_Z[0];
b0ssiz 8:2d34916ac178 283 package.parameter[5]=IntOffset_Z[1];
b0ssiz 8:2d34916ac178 284 package.parameter[6]=FloatOffset_Z[0];
b0ssiz 8:2d34916ac178 285 package.parameter[7]=FloatOffset_Z[1];
b0ssiz 8:2d34916ac178 286
b0ssiz 8:2d34916ac178 287 rs485_dirc=1;
b0ssiz 8:2d34916ac178 288 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 289 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 290 }
b0ssiz 8:2d34916ac178 291
b0ssiz 8:2d34916ac178 292 uint8_t Bear_Receiver::sendBodyLength(uint8_t id,float body_length)
b0ssiz 8:2d34916ac178 293 {
b0ssiz 8:2d34916ac178 294 uint8_t IntBodyLength[2],FloatBodyLength[2];
b0ssiz 8:2d34916ac178 295 FloatSep(body_length,IntBodyLength,FloatBodyLength);
b0ssiz 8:2d34916ac178 296
b0ssiz 8:2d34916ac178 297 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 298
b0ssiz 8:2d34916ac178 299 package.robotId = id;
b0ssiz 8:2d34916ac178 300 package.length = 6;
b0ssiz 8:2d34916ac178 301 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 302 package.parameter[0]=IntBodyLength[0];
b0ssiz 8:2d34916ac178 303 package.parameter[1]=IntBodyLength[1];
b0ssiz 8:2d34916ac178 304 package.parameter[2]=FloatBodyLength[0];
b0ssiz 8:2d34916ac178 305 package.parameter[3]=FloatBodyLength[1];
b0ssiz 8:2d34916ac178 306
b0ssiz 8:2d34916ac178 307 rs485_dirc=1;
b0ssiz 8:2d34916ac178 308 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 309 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 310 }
b0ssiz 8:2d34916ac178 311
b0ssiz 8:2d34916ac178 312 uint8_t Bear_Receiver::sendAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 8:2d34916ac178 313 {
b0ssiz 8:2d34916ac178 314 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 8:2d34916ac178 315 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 8:2d34916ac178 316 FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
b0ssiz 8:2d34916ac178 317 FloatSep(min_angle,IntMinAngle,FloatMinAngle);
b0ssiz 8:2d34916ac178 318
b0ssiz 8:2d34916ac178 319 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 320
b0ssiz 8:2d34916ac178 321 package.robotId = id;
b0ssiz 8:2d34916ac178 322 package.length = 10;
b0ssiz 8:2d34916ac178 323 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 324 package.parameter[0]=IntMaxAngle[0];
b0ssiz 8:2d34916ac178 325 package.parameter[1]=IntMaxAngle[1];
b0ssiz 8:2d34916ac178 326 package.parameter[2]=FloatMaxAngle[0];
b0ssiz 8:2d34916ac178 327 package.parameter[3]=FloatMaxAngle[1];
b0ssiz 8:2d34916ac178 328 package.parameter[4]=IntMinAngle[0];
b0ssiz 8:2d34916ac178 329 package.parameter[5]=IntMinAngle[1];
b0ssiz 8:2d34916ac178 330 package.parameter[6]=FloatMinAngle[0];
b0ssiz 8:2d34916ac178 331 package.parameter[7]=FloatMinAngle[1];
b0ssiz 8:2d34916ac178 332
b0ssiz 8:2d34916ac178 333 rs485_dirc=1;
b0ssiz 8:2d34916ac178 334 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 335 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 336 }