BE@R lab / BEAR_Reciever

Communication for reciever

Dependencies:   Fork_Boss_Communication_Robot

Dependents:   BEAR_Motion BEAR_Motion_Editing4Betago

Fork of BEAR_Protocol by BE@R lab

Receiver.cpp@13:e7404c675611, 2016-01-22 (annotated)

Committer:
b0ssiz
Date:
Fri Jan 22 16:34:11 2016 +0000
Revision:
13:e7404c675611
Parent:
12:be8675c18a58
Child:
14:1c3180c64272
Update; - Seperate function Margin to UpMargin and LowMargin

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 12:be8675c18a58 155 uint8_t Bear_Receiver::sendUpMargin(uint8_t id,float margin)
b0ssiz 12:be8675c18a58 156 {
b0ssiz 12:be8675c18a58 157 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 12:be8675c18a58 158 FloatSep(margin,IntMargin,FloatMargin);
b0ssiz 12:be8675c18a58 159
b0ssiz 12:be8675c18a58 160 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 12:be8675c18a58 161
b0ssiz 12:be8675c18a58 162 package.robotId = id;
b0ssiz 12:be8675c18a58 163 package.length = 6;
b0ssiz 12:be8675c18a58 164 package.instructionErrorId = WRITE_DATA;
b0ssiz 12:be8675c18a58 165 package.parameter[0]=IntMargin[0];
b0ssiz 12:be8675c18a58 166 package.parameter[1]=IntMargin[1];
b0ssiz 12:be8675c18a58 167 package.parameter[2]=FloatMargin[0];
b0ssiz 12:be8675c18a58 168 package.parameter[3]=FloatMargin[1];
b0ssiz 12:be8675c18a58 169
b0ssiz 12:be8675c18a58 170 rs485_dirc=1;
b0ssiz 12:be8675c18a58 171 wait_us(RS485_DELAY);
b0ssiz 12:be8675c18a58 172 return com->sendCommunicatePacket(&package);
b0ssiz 12:be8675c18a58 173
b0ssiz 12:be8675c18a58 174 }
b0ssiz 12:be8675c18a58 175
b0ssiz 12:be8675c18a58 176 uint8_t Bear_Receiver::sendLowMargin(uint8_t id,float margin)
b0ssiz 8:2d34916ac178 177 {
b0ssiz 8:2d34916ac178 178 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 8:2d34916ac178 179 FloatSep(margin,IntMargin,FloatMargin);
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]=IntMargin[0];
b0ssiz 8:2d34916ac178 187 package.parameter[1]=IntMargin[1];
b0ssiz 8:2d34916ac178 188 package.parameter[2]=FloatMargin[0];
b0ssiz 8:2d34916ac178 189 package.parameter[3]=FloatMargin[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
b0ssiz 8:2d34916ac178 197 uint8_t Bear_Receiver::sendHeight(uint8_t id,float height)
b0ssiz 8:2d34916ac178 198 {
b0ssiz 8:2d34916ac178 199 uint8_t IntHeight[2],FloatHeight[2];
b0ssiz 8:2d34916ac178 200 FloatSep(height,IntHeight,FloatHeight);
b0ssiz 8:2d34916ac178 201
b0ssiz 8:2d34916ac178 202 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 203
b0ssiz 8:2d34916ac178 204 package.robotId = id;
b0ssiz 8:2d34916ac178 205 package.length = 6;
b0ssiz 8:2d34916ac178 206 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 207 package.parameter[0]=IntHeight[0];
b0ssiz 8:2d34916ac178 208 package.parameter[1]=IntHeight[1];
b0ssiz 8:2d34916ac178 209 package.parameter[2]=FloatHeight[0];
b0ssiz 8:2d34916ac178 210 package.parameter[3]=FloatHeight[1];
b0ssiz 8:2d34916ac178 211
b0ssiz 8:2d34916ac178 212 rs485_dirc=1;
b0ssiz 8:2d34916ac178 213 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 214 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 215 }
b0ssiz 8:2d34916ac178 216
b0ssiz 8:2d34916ac178 217 uint8_t Bear_Receiver::sendWheelPos(uint8_t id,float WheelPos)
b0ssiz 8:2d34916ac178 218 {
b0ssiz 8:2d34916ac178 219 uint8_t IntWheelPos[2],FloatWheelPos[2];
b0ssiz 8:2d34916ac178 220 FloatSep(WheelPos,IntWheelPos,FloatWheelPos);
b0ssiz 8:2d34916ac178 221
b0ssiz 8:2d34916ac178 222 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 223
b0ssiz 8:2d34916ac178 224 package.robotId = id;
b0ssiz 8:2d34916ac178 225 package.length = 6;
b0ssiz 8:2d34916ac178 226 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 227 package.parameter[0]=IntWheelPos[0];
b0ssiz 8:2d34916ac178 228 package.parameter[1]=IntWheelPos[1];
b0ssiz 8:2d34916ac178 229 package.parameter[2]=FloatWheelPos[0];
b0ssiz 8:2d34916ac178 230 package.parameter[3]=FloatWheelPos[1];
b0ssiz 8:2d34916ac178 231
b0ssiz 8:2d34916ac178 232 rs485_dirc=1;
b0ssiz 8:2d34916ac178 233 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 234 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 235 }
b0ssiz 8:2d34916ac178 236
b0ssiz 8:2d34916ac178 237 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 238 {
b0ssiz 8:2d34916ac178 239 uint8_t IntXmax[2],FloatXmax[2];
b0ssiz 8:2d34916ac178 240 uint8_t IntXmin[2],FloatXmin[2];
b0ssiz 8:2d34916ac178 241 uint8_t IntYmax[2],FloatYmax[2];
b0ssiz 8:2d34916ac178 242 uint8_t IntYmin[2],FloatYmin[2];
b0ssiz 8:2d34916ac178 243 uint8_t IntZmax[2],FloatZmax[2];
b0ssiz 8:2d34916ac178 244 uint8_t IntZmin[2],FloatZmin[2];
b0ssiz 8:2d34916ac178 245 FloatSep(x_max,IntXmax,FloatXmax);
b0ssiz 8:2d34916ac178 246 FloatSep(x_min,IntXmin,FloatXmin);
b0ssiz 8:2d34916ac178 247 FloatSep(y_max,IntYmax,FloatYmax);
b0ssiz 8:2d34916ac178 248 FloatSep(y_min,IntYmin,FloatYmin);
b0ssiz 8:2d34916ac178 249 FloatSep(z_max,IntZmax,FloatZmax);
b0ssiz 8:2d34916ac178 250 FloatSep(z_min,IntZmin,FloatZmin);
b0ssiz 8:2d34916ac178 251
b0ssiz 8:2d34916ac178 252 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 253
b0ssiz 8:2d34916ac178 254 package.robotId = id;
b0ssiz 8:2d34916ac178 255 package.length = 26;
b0ssiz 8:2d34916ac178 256 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 257 package.parameter[0]=IntXmax[0];
b0ssiz 8:2d34916ac178 258 package.parameter[1]=IntXmax[1];
b0ssiz 8:2d34916ac178 259 package.parameter[2]=FloatXmax[0];
b0ssiz 8:2d34916ac178 260 package.parameter[3]=FloatXmax[1];
b0ssiz 8:2d34916ac178 261 package.parameter[4]=IntXmin[0];
b0ssiz 8:2d34916ac178 262 package.parameter[5]=IntXmin[1];
b0ssiz 8:2d34916ac178 263 package.parameter[6]=FloatXmin[0];
b0ssiz 8:2d34916ac178 264 package.parameter[7]=FloatXmin[1];
b0ssiz 8:2d34916ac178 265 package.parameter[8]=IntYmax[0];
b0ssiz 8:2d34916ac178 266 package.parameter[9]=IntYmax[1];
b0ssiz 8:2d34916ac178 267 package.parameter[10]=FloatYmax[0];
b0ssiz 8:2d34916ac178 268 package.parameter[11]=FloatYmax[1];
b0ssiz 8:2d34916ac178 269 package.parameter[12]=IntYmin[0];
b0ssiz 8:2d34916ac178 270 package.parameter[13]=IntYmin[1];
b0ssiz 8:2d34916ac178 271 package.parameter[14]=FloatYmin[0];
b0ssiz 8:2d34916ac178 272 package.parameter[15]=FloatYmin[1];
b0ssiz 8:2d34916ac178 273 package.parameter[16]=IntZmax[0];
b0ssiz 8:2d34916ac178 274 package.parameter[17]=IntZmax[1];
b0ssiz 8:2d34916ac178 275 package.parameter[18]=FloatZmax[0];
b0ssiz 8:2d34916ac178 276 package.parameter[19]=FloatZmax[1];
b0ssiz 8:2d34916ac178 277 package.parameter[20]=IntZmin[0];
b0ssiz 8:2d34916ac178 278 package.parameter[21]=IntZmin[1];
b0ssiz 8:2d34916ac178 279 package.parameter[22]=FloatZmin[0];
b0ssiz 8:2d34916ac178 280 package.parameter[23]=FloatZmin[1];
b0ssiz 8:2d34916ac178 281
b0ssiz 8:2d34916ac178 282 rs485_dirc=1;
b0ssiz 8:2d34916ac178 283 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 284 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 285 }
b0ssiz 8:2d34916ac178 286
b0ssiz 8:2d34916ac178 287 uint8_t Bear_Receiver::sendOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 8:2d34916ac178 288 {
b0ssiz 8:2d34916ac178 289 uint8_t IntOffset_Y[2],FloatOffset_Y[2];
b0ssiz 8:2d34916ac178 290 uint8_t IntOffset_Z[2],FloatOffset_Z[2];
b0ssiz 8:2d34916ac178 291 FloatSep(offset_y,IntOffset_Y,FloatOffset_Y);
b0ssiz 8:2d34916ac178 292 FloatSep(offset_z,IntOffset_Z,FloatOffset_Z);
b0ssiz 8:2d34916ac178 293
b0ssiz 8:2d34916ac178 294 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 295
b0ssiz 8:2d34916ac178 296 package.robotId = id;
b0ssiz 8:2d34916ac178 297 package.length = 10;
b0ssiz 8:2d34916ac178 298 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 299 package.parameter[0]=IntOffset_Y[0];
b0ssiz 8:2d34916ac178 300 package.parameter[1]=IntOffset_Y[1];
b0ssiz 8:2d34916ac178 301 package.parameter[2]=FloatOffset_Y[0];
b0ssiz 8:2d34916ac178 302 package.parameter[3]=FloatOffset_Y[1];
b0ssiz 8:2d34916ac178 303 package.parameter[4]=IntOffset_Z[0];
b0ssiz 8:2d34916ac178 304 package.parameter[5]=IntOffset_Z[1];
b0ssiz 8:2d34916ac178 305 package.parameter[6]=FloatOffset_Z[0];
b0ssiz 8:2d34916ac178 306 package.parameter[7]=FloatOffset_Z[1];
b0ssiz 8:2d34916ac178 307
b0ssiz 8:2d34916ac178 308 rs485_dirc=1;
b0ssiz 8:2d34916ac178 309 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 310 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 311 }
b0ssiz 8:2d34916ac178 312
b0ssiz 13:e7404c675611 313 uint8_t Bear_Receiver::sendBodyWidth(uint8_t id,float body_Width)
b0ssiz 8:2d34916ac178 314 {
b0ssiz 13:e7404c675611 315 uint8_t IntBodyWidth[2],FloatBodyWidth[2];
b0ssiz 13:e7404c675611 316 FloatSep(body_Width,IntBodyWidth,FloatBodyWidth);
b0ssiz 8:2d34916ac178 317
b0ssiz 8:2d34916ac178 318 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 319
b0ssiz 8:2d34916ac178 320 package.robotId = id;
b0ssiz 8:2d34916ac178 321 package.length = 6;
b0ssiz 8:2d34916ac178 322 package.instructionErrorId = WRITE_DATA;
b0ssiz 13:e7404c675611 323 package.parameter[0]=IntBodyWidth[0];
b0ssiz 13:e7404c675611 324 package.parameter[1]=IntBodyWidth[1];
b0ssiz 13:e7404c675611 325 package.parameter[2]=FloatBodyWidth[0];
b0ssiz 13:e7404c675611 326 package.parameter[3]=FloatBodyWidth[1];
b0ssiz 8:2d34916ac178 327
b0ssiz 8:2d34916ac178 328 rs485_dirc=1;
b0ssiz 8:2d34916ac178 329 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 330 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 331 }
b0ssiz 8:2d34916ac178 332
b0ssiz 11:b9337e23782e 333 uint8_t Bear_Receiver::sendUpAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 11:b9337e23782e 334 {
b0ssiz 11:b9337e23782e 335 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 11:b9337e23782e 336 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 11:b9337e23782e 337 FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
b0ssiz 11:b9337e23782e 338 FloatSep(min_angle,IntMinAngle,FloatMinAngle);
b0ssiz 11:b9337e23782e 339
b0ssiz 11:b9337e23782e 340 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 11:b9337e23782e 341
b0ssiz 11:b9337e23782e 342 package.robotId = id;
b0ssiz 11:b9337e23782e 343 package.length = 10;
b0ssiz 11:b9337e23782e 344 package.instructionErrorId = WRITE_DATA;
b0ssiz 11:b9337e23782e 345 package.parameter[0]=IntMaxAngle[0];
b0ssiz 11:b9337e23782e 346 package.parameter[1]=IntMaxAngle[1];
b0ssiz 11:b9337e23782e 347 package.parameter[2]=FloatMaxAngle[0];
b0ssiz 11:b9337e23782e 348 package.parameter[3]=FloatMaxAngle[1];
b0ssiz 11:b9337e23782e 349 package.parameter[4]=IntMinAngle[0];
b0ssiz 11:b9337e23782e 350 package.parameter[5]=IntMinAngle[1];
b0ssiz 11:b9337e23782e 351 package.parameter[6]=FloatMinAngle[0];
b0ssiz 11:b9337e23782e 352 package.parameter[7]=FloatMinAngle[1];
b0ssiz 11:b9337e23782e 353
b0ssiz 11:b9337e23782e 354 rs485_dirc=1;
b0ssiz 11:b9337e23782e 355 wait_us(RS485_DELAY);
b0ssiz 11:b9337e23782e 356 return com->sendCommunicatePacket(&package);
b0ssiz 11:b9337e23782e 357 }
b0ssiz 11:b9337e23782e 358
b0ssiz 11:b9337e23782e 359
b0ssiz 11:b9337e23782e 360 uint8_t Bear_Receiver::sendLowAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 8:2d34916ac178 361 {
b0ssiz 8:2d34916ac178 362 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 8:2d34916ac178 363 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 8:2d34916ac178 364 FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
b0ssiz 8:2d34916ac178 365 FloatSep(min_angle,IntMinAngle,FloatMinAngle);
b0ssiz 8:2d34916ac178 366
b0ssiz 8:2d34916ac178 367 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 368
b0ssiz 8:2d34916ac178 369 package.robotId = id;
b0ssiz 8:2d34916ac178 370 package.length = 10;
b0ssiz 8:2d34916ac178 371 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:2d34916ac178 372 package.parameter[0]=IntMaxAngle[0];
b0ssiz 8:2d34916ac178 373 package.parameter[1]=IntMaxAngle[1];
b0ssiz 8:2d34916ac178 374 package.parameter[2]=FloatMaxAngle[0];
b0ssiz 8:2d34916ac178 375 package.parameter[3]=FloatMaxAngle[1];
b0ssiz 8:2d34916ac178 376 package.parameter[4]=IntMinAngle[0];
b0ssiz 8:2d34916ac178 377 package.parameter[5]=IntMinAngle[1];
b0ssiz 8:2d34916ac178 378 package.parameter[6]=FloatMinAngle[0];
b0ssiz 8:2d34916ac178 379 package.parameter[7]=FloatMinAngle[1];
b0ssiz 8:2d34916ac178 380
b0ssiz 8:2d34916ac178 381 rs485_dirc=1;
b0ssiz 8:2d34916ac178 382 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 383 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 384 }