Secret_Betago / BEAR_Reciever

update protocol.h

Dependencies:   Communication_Robot

Fork of BEAR_Reciever by BE@R lab

Receiver.cpp@24:2dca9d071f9e, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 06:59:48 2016 +0000
Revision:
24:2dca9d071f9e
Parent:
22:894bca54ae1a 
update protocol.h

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 15:fe0cc017c66d 23 for(int i=0; i<30; i++) {
b0ssiz 15:fe0cc017c66d 24 data_array[i] = package.parameter[i];
b0ssiz 15:fe0cc017c66d 25 }
b0ssiz 8:2d34916ac178 26 *ins=package.instructionErrorId;
b0ssiz 5:baf5576a14bd 27
b0ssiz 5:baf5576a14bd 28 }
b0ssiz 5:baf5576a14bd 29 return status;
b0ssiz 5:baf5576a14bd 30 }
b0ssiz 5:baf5576a14bd 31
b0ssiz 5:baf5576a14bd 32
b0ssiz 5:baf5576a14bd 33
b0ssiz 7:a0ea8c127611 34
b0ssiz 7:a0ea8c127611 35
b0ssiz 5:baf5576a14bd 36 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
b0ssiz 5:baf5576a14bd 37 {
b0ssiz 5:baf5576a14bd 38 float float_buffer;
b0ssiz 5:baf5576a14bd 39 float int_buffer;
b0ssiz 5:baf5576a14bd 40 int16_t integer;
b0ssiz 5:baf5576a14bd 41 int16_t floating_point;
b0ssiz 5:baf5576a14bd 42
b0ssiz 5:baf5576a14bd 43 float_buffer=modf(input_float,&int_buffer);
b0ssiz 5:baf5576a14bd 44 float_buffer*=FLOAT_CONVERTER;
b0ssiz 5:baf5576a14bd 45 integer=(int16_t)int_buffer;
b0ssiz 5:baf5576a14bd 46 floating_point=(int16_t)float_buffer;
b0ssiz 5:baf5576a14bd 47 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
b0ssiz 5:baf5576a14bd 48 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
b0ssiz 5:baf5576a14bd 49 }
b0ssiz 5:baf5576a14bd 50
b0ssiz 5:baf5576a14bd 51
b0ssiz 5:baf5576a14bd 52
b0ssiz 5:baf5576a14bd 53
b0ssiz 5:baf5576a14bd 54 uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
b0ssiz 5:baf5576a14bd 55 {
b0ssiz 5:baf5576a14bd 56 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 5:baf5576a14bd 57 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 5:baf5576a14bd 58 FloatSep(up_angle,IntUpAngle,FloatUpAngle);
b0ssiz 5:baf5576a14bd 59 FloatSep(low_angle,IntLowAngle,FloatLowAngle);
b0ssiz 5:baf5576a14bd 60
b0ssiz 5:baf5576a14bd 61 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:baf5576a14bd 62
b0ssiz 5:baf5576a14bd 63 package.robotId = id;
b0ssiz 5:baf5576a14bd 64 package.length = 10;
b0ssiz 5:baf5576a14bd 65 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:baf5576a14bd 66 package.parameter[0]=IntUpAngle[0];
b0ssiz 5:baf5576a14bd 67 package.parameter[1]=IntUpAngle[1];
b0ssiz 5:baf5576a14bd 68 package.parameter[2]=FloatUpAngle[0];
b0ssiz 5:baf5576a14bd 69 package.parameter[3]=FloatUpAngle[1];
b0ssiz 5:baf5576a14bd 70 package.parameter[4]=IntLowAngle[0];
b0ssiz 5:baf5576a14bd 71 package.parameter[5]=IntLowAngle[1];
b0ssiz 5:baf5576a14bd 72 package.parameter[6]=FloatLowAngle[0];
b0ssiz 5:baf5576a14bd 73 package.parameter[7]=FloatLowAngle[1];
b0ssiz 5:baf5576a14bd 74
b0ssiz 5:baf5576a14bd 75 rs485_dirc=1;
b0ssiz 5:baf5576a14bd 76 wait_us(RS485_DELAY);
b0ssiz 5:baf5576a14bd 77 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 78
b0ssiz 6:f6529ad2a70d 79
b0ssiz 6:f6529ad2a70d 80 }
b0ssiz 6:f6529ad2a70d 81
b0ssiz 6:f6529ad2a70d 82
b0ssiz 6:f6529ad2a70d 83
b0ssiz 6:f6529ad2a70d 84 uint8_t Bear_Receiver::sendUpMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 85 {
b0ssiz 6:f6529ad2a70d 86 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 87 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 88 uint8_t IntKd[2],FloatKd[2];
b0ssiz 7:a0ea8c127611 89
b0ssiz 6:f6529ad2a70d 90 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 91 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 92 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 7:a0ea8c127611 93
b0ssiz 7:a0ea8c127611 94
b0ssiz 6:f6529ad2a70d 95 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 96
b0ssiz 6:f6529ad2a70d 97 package.robotId = id;
b0ssiz 6:f6529ad2a70d 98 package.length = 14;
b0ssiz 6:f6529ad2a70d 99 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 100 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 101 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 102 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 103 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 104 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 105 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 106 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 107 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 108 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 109 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 110 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 111 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 112
b0ssiz 6:f6529ad2a70d 113 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 114 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 115 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 116 }
b0ssiz 6:f6529ad2a70d 117
b0ssiz 6:f6529ad2a70d 118
b0ssiz 6:f6529ad2a70d 119
b0ssiz 6:f6529ad2a70d 120
b0ssiz 6:f6529ad2a70d 121 uint8_t Bear_Receiver::sendLowMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 122 {
b0ssiz 6:f6529ad2a70d 123 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 124 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 125 uint8_t IntKd[2],FloatKd[2];
b0ssiz 7:a0ea8c127611 126
b0ssiz 6:f6529ad2a70d 127 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 128 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 129 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 7:a0ea8c127611 130
b0ssiz 7:a0ea8c127611 131
b0ssiz 6:f6529ad2a70d 132 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 133
b0ssiz 6:f6529ad2a70d 134 package.robotId = id;
b0ssiz 6:f6529ad2a70d 135 package.length = 14;
b0ssiz 6:f6529ad2a70d 136 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 137 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 138 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 139 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 140 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 141 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 142 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 143 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 144 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 145 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 146 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 147 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 148 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 149
b0ssiz 6:f6529ad2a70d 150 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 151 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 152 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 153 }
b0ssiz 8:2d34916ac178 154
b0ssiz 8:2d34916ac178 155
b0ssiz 8:2d34916ac178 156
soulx 22:894bca54ae1a 157 uint8_t Bear_Receiver::sendUpMargin(uint8_t id,float Margin)
b0ssiz 12:be8675c18a58 158 {
soulx 22:894bca54ae1a 159 uint8_t Int[2],Float[2];
soulx 22:894bca54ae1a 160 FloatSep(Margin,Int,Float);
b0ssiz 12:be8675c18a58 161 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 12:be8675c18a58 162
b0ssiz 12:be8675c18a58 163 package.robotId = id;
b0ssiz 12:be8675c18a58 164 package.length = 6;
b0ssiz 12:be8675c18a58 165 package.instructionErrorId = WRITE_DATA;
soulx 22:894bca54ae1a 166 package.parameter[0]=Int[0];
soulx 22:894bca54ae1a 167 package.parameter[1]=Int[1];
soulx 22:894bca54ae1a 168 package.parameter[2]=Float[0];
soulx 22:894bca54ae1a 169 package.parameter[3]=Float[1];
b0ssiz 12:be8675c18a58 170
b0ssiz 12:be8675c18a58 171 rs485_dirc=1;
b0ssiz 12:be8675c18a58 172 wait_us(RS485_DELAY);
b0ssiz 12:be8675c18a58 173 return com->sendCommunicatePacket(&package);
b0ssiz 12:be8675c18a58 174
b0ssiz 12:be8675c18a58 175 }
b0ssiz 12:be8675c18a58 176
soulx 22:894bca54ae1a 177 uint8_t Bear_Receiver::sendLowMargin(uint8_t id,float Margin)
b0ssiz 8:2d34916ac178 178 {
soulx 22:894bca54ae1a 179 uint8_t Int[2],Float[2];
soulx 22:894bca54ae1a 180 FloatSep(Margin,Int,Float);
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;
soulx 22:894bca54ae1a 186 package.parameter[0]=Int[0];
soulx 22:894bca54ae1a 187 package.parameter[1]=Int[1];
soulx 22:894bca54ae1a 188 package.parameter[2]=Float[0];
soulx 22:894bca54ae1a 189 package.parameter[3]=Float[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
ParinyaT 14:1c3180c64272 196 uint8_t Bear_Receiver::sendHeight(uint8_t id,uint8_t *Height)
b0ssiz 8:2d34916ac178 197 {
b0ssiz 8:2d34916ac178 198 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 199
b0ssiz 8:2d34916ac178 200 package.robotId = id;
b0ssiz 8:2d34916ac178 201 package.length = 6;
b0ssiz 8:2d34916ac178 202 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 203 package.parameter[0]=Height[0];
ParinyaT 14:1c3180c64272 204 package.parameter[1]=Height[1];
ParinyaT 14:1c3180c64272 205 package.parameter[2]=Height[2];
ParinyaT 14:1c3180c64272 206 package.parameter[3]=Height[3];
b0ssiz 8:2d34916ac178 207
b0ssiz 8:2d34916ac178 208 rs485_dirc=1;
b0ssiz 8:2d34916ac178 209 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 210 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 211 }
b0ssiz 8:2d34916ac178 212
ParinyaT 14:1c3180c64272 213 uint8_t Bear_Receiver::sendWheelPos(uint8_t id,uint8_t *WheelPos)
b0ssiz 8:2d34916ac178 214 {
b0ssiz 8:2d34916ac178 215 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 216
b0ssiz 8:2d34916ac178 217 package.robotId = id;
b0ssiz 8:2d34916ac178 218 package.length = 6;
b0ssiz 8:2d34916ac178 219 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 220 package.parameter[0]=WheelPos[0];
ParinyaT 14:1c3180c64272 221 package.parameter[1]=WheelPos[1];
ParinyaT 14:1c3180c64272 222 package.parameter[2]=WheelPos[2];
ParinyaT 14:1c3180c64272 223 package.parameter[3]=WheelPos[3];
b0ssiz 8:2d34916ac178 224
b0ssiz 8:2d34916ac178 225 rs485_dirc=1;
b0ssiz 8:2d34916ac178 226 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 227 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 228 }
b0ssiz 8:2d34916ac178 229
ParinyaT 14:1c3180c64272 230 uint8_t Bear_Receiver::sendMagData(uint8_t id,uint8_t *Mag)
b0ssiz 8:2d34916ac178 231 {
b0ssiz 8:2d34916ac178 232 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 233
b0ssiz 8:2d34916ac178 234 package.robotId = id;
b0ssiz 8:2d34916ac178 235 package.length = 26;
b0ssiz 8:2d34916ac178 236 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 237 package.parameter[0]=Mag[0];
ParinyaT 14:1c3180c64272 238 package.parameter[1]=Mag[1];
ParinyaT 14:1c3180c64272 239 package.parameter[2]=Mag[2];
ParinyaT 14:1c3180c64272 240 package.parameter[3]=Mag[3];
ParinyaT 14:1c3180c64272 241 package.parameter[4]=Mag[4];
ParinyaT 14:1c3180c64272 242 package.parameter[5]=Mag[5];
ParinyaT 14:1c3180c64272 243 package.parameter[6]=Mag[6];
ParinyaT 14:1c3180c64272 244 package.parameter[7]=Mag[7];
ParinyaT 14:1c3180c64272 245 package.parameter[8]=Mag[8];
ParinyaT 14:1c3180c64272 246 package.parameter[9]=Mag[9];
ParinyaT 14:1c3180c64272 247 package.parameter[10]=Mag[10];
ParinyaT 14:1c3180c64272 248 package.parameter[11]=Mag[11];
ParinyaT 14:1c3180c64272 249 package.parameter[12]=Mag[12];
ParinyaT 14:1c3180c64272 250 package.parameter[13]=Mag[13];
ParinyaT 14:1c3180c64272 251 package.parameter[14]=Mag[14];
ParinyaT 14:1c3180c64272 252 package.parameter[15]=Mag[15];
ParinyaT 14:1c3180c64272 253 package.parameter[16]=Mag[16];
ParinyaT 14:1c3180c64272 254 package.parameter[17]=Mag[17];
ParinyaT 14:1c3180c64272 255 package.parameter[18]=Mag[18];
ParinyaT 14:1c3180c64272 256 package.parameter[19]=Mag[19];
ParinyaT 14:1c3180c64272 257 package.parameter[20]=Mag[20];
ParinyaT 14:1c3180c64272 258 package.parameter[21]=Mag[21];
ParinyaT 14:1c3180c64272 259 package.parameter[22]=Mag[22];
ParinyaT 14:1c3180c64272 260 package.parameter[23]=Mag[23];
b0ssiz 8:2d34916ac178 261
b0ssiz 8:2d34916ac178 262 rs485_dirc=1;
b0ssiz 8:2d34916ac178 263 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 264 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 265 }
b0ssiz 8:2d34916ac178 266
ParinyaT 14:1c3180c64272 267 uint8_t Bear_Receiver::sendOffset(uint8_t id,uint8_t *Offset)
b0ssiz 8:2d34916ac178 268 {
b0ssiz 8:2d34916ac178 269 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 270
b0ssiz 8:2d34916ac178 271 package.robotId = id;
b0ssiz 8:2d34916ac178 272 package.length = 10;
b0ssiz 8:2d34916ac178 273 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 274 package.parameter[0]=Offset[0];
ParinyaT 14:1c3180c64272 275 package.parameter[1]=Offset[1];
ParinyaT 14:1c3180c64272 276 package.parameter[2]=Offset[2];
ParinyaT 14:1c3180c64272 277 package.parameter[3]=Offset[3];
ParinyaT 14:1c3180c64272 278 package.parameter[4]=Offset[4];
ParinyaT 14:1c3180c64272 279 package.parameter[5]=Offset[5];
ParinyaT 14:1c3180c64272 280 package.parameter[6]=Offset[6];
ParinyaT 14:1c3180c64272 281 package.parameter[7]=Offset[7];
b0ssiz 8:2d34916ac178 282
b0ssiz 8:2d34916ac178 283 rs485_dirc=1;
b0ssiz 8:2d34916ac178 284 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 285 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 286 }
b0ssiz 8:2d34916ac178 287
ParinyaT 14:1c3180c64272 288 uint8_t Bear_Receiver::sendBodyWidth(uint8_t id,uint8_t *BodyWidth)
b0ssiz 8:2d34916ac178 289 {
b0ssiz 8:2d34916ac178 290 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 291
b0ssiz 8:2d34916ac178 292 package.robotId = id;
b0ssiz 8:2d34916ac178 293 package.length = 6;
b0ssiz 8:2d34916ac178 294 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 295 package.parameter[0]=BodyWidth[0];
ParinyaT 14:1c3180c64272 296 package.parameter[1]=BodyWidth[1];
ParinyaT 14:1c3180c64272 297 package.parameter[2]=BodyWidth[2];
ParinyaT 14:1c3180c64272 298 package.parameter[3]=BodyWidth[3];
b0ssiz 8:2d34916ac178 299
b0ssiz 8:2d34916ac178 300 rs485_dirc=1;
b0ssiz 8:2d34916ac178 301 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 302 return com->sendCommunicatePacket(&package);
b0ssiz 8:2d34916ac178 303 }
b0ssiz 8:2d34916ac178 304
ParinyaT 14:1c3180c64272 305 uint8_t Bear_Receiver::sendUpAngleRange(uint8_t id,uint8_t *Angle)
b0ssiz 11:b9337e23782e 306 {
b0ssiz 11:b9337e23782e 307 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 11:b9337e23782e 308
b0ssiz 11:b9337e23782e 309 package.robotId = id;
b0ssiz 11:b9337e23782e 310 package.length = 10;
b0ssiz 11:b9337e23782e 311 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 312 package.parameter[0]=Angle[0];
ParinyaT 14:1c3180c64272 313 package.parameter[1]=Angle[1];
ParinyaT 14:1c3180c64272 314 package.parameter[2]=Angle[2];
ParinyaT 14:1c3180c64272 315 package.parameter[3]=Angle[3];
ParinyaT 14:1c3180c64272 316 package.parameter[4]=Angle[4];
ParinyaT 14:1c3180c64272 317 package.parameter[5]=Angle[5];
ParinyaT 14:1c3180c64272 318 package.parameter[6]=Angle[6];
ParinyaT 14:1c3180c64272 319 package.parameter[7]=Angle[7];
b0ssiz 11:b9337e23782e 320
b0ssiz 11:b9337e23782e 321 rs485_dirc=1;
b0ssiz 11:b9337e23782e 322 wait_us(RS485_DELAY);
b0ssiz 11:b9337e23782e 323 return com->sendCommunicatePacket(&package);
b0ssiz 11:b9337e23782e 324 }
b0ssiz 11:b9337e23782e 325
b0ssiz 11:b9337e23782e 326
ParinyaT 14:1c3180c64272 327 uint8_t Bear_Receiver::sendLowAngleRange(uint8_t id,uint8_t *Angle)
b0ssiz 8:2d34916ac178 328 {
b0ssiz 8:2d34916ac178 329 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:2d34916ac178 330
b0ssiz 8:2d34916ac178 331 package.robotId = id;
b0ssiz 8:2d34916ac178 332 package.length = 10;
b0ssiz 8:2d34916ac178 333 package.instructionErrorId = WRITE_DATA;
ParinyaT 14:1c3180c64272 334 package.parameter[0]=Angle[0];
ParinyaT 14:1c3180c64272 335 package.parameter[1]=Angle[1];
ParinyaT 14:1c3180c64272 336 package.parameter[2]=Angle[2];
ParinyaT 14:1c3180c64272 337 package.parameter[3]=Angle[3];
ParinyaT 14:1c3180c64272 338 package.parameter[4]=Angle[4];
ParinyaT 14:1c3180c64272 339 package.parameter[5]=Angle[5];
ParinyaT 14:1c3180c64272 340 package.parameter[6]=Angle[6];
ParinyaT 14:1c3180c64272 341 package.parameter[7]=Angle[7];
b0ssiz 8:2d34916ac178 342
b0ssiz 8:2d34916ac178 343 rs485_dirc=1;
b0ssiz 8:2d34916ac178 344 wait_us(RS485_DELAY);
b0ssiz 8:2d34916ac178 345 return com->sendCommunicatePacket(&package);
b0ssiz 17:774eec1470d8 346 }
b0ssiz 17:774eec1470d8 347
b0ssiz 17:774eec1470d8 348 uint8_t Bear_Receiver::sendUpLinkLength(uint8_t id,uint8_t *Length)
b0ssiz 17:774eec1470d8 349 {
b0ssiz 17:774eec1470d8 350 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 17:774eec1470d8 351
b0ssiz 17:774eec1470d8 352 package.robotId = id;
b0ssiz 17:774eec1470d8 353 package.length = 6;
b0ssiz 17:774eec1470d8 354 package.instructionErrorId = WRITE_DATA;
b0ssiz 17:774eec1470d8 355 package.parameter[0]=Length[0];
b0ssiz 17:774eec1470d8 356 package.parameter[1]=Length[1];
b0ssiz 17:774eec1470d8 357 package.parameter[2]=Length[2];
b0ssiz 17:774eec1470d8 358 package.parameter[3]=Length[3];
b0ssiz 17:774eec1470d8 359
b0ssiz 17:774eec1470d8 360 rs485_dirc=1;
b0ssiz 17:774eec1470d8 361 wait_us(RS485_DELAY);
b0ssiz 17:774eec1470d8 362 return com->sendCommunicatePacket(&package);
b0ssiz 17:774eec1470d8 363
b0ssiz 17:774eec1470d8 364 }
b0ssiz 17:774eec1470d8 365
b0ssiz 17:774eec1470d8 366 uint8_t Bear_Receiver::sendLowLinkLength(uint8_t id,uint8_t *Length)
b0ssiz 17:774eec1470d8 367 {
b0ssiz 17:774eec1470d8 368 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 17:774eec1470d8 369
b0ssiz 17:774eec1470d8 370 package.robotId = id;
b0ssiz 17:774eec1470d8 371 package.length = 6;
b0ssiz 17:774eec1470d8 372 package.instructionErrorId = WRITE_DATA;
b0ssiz 17:774eec1470d8 373 package.parameter[0]=Length[0];
b0ssiz 17:774eec1470d8 374 package.parameter[1]=Length[1];
b0ssiz 17:774eec1470d8 375 package.parameter[2]=Length[2];
b0ssiz 17:774eec1470d8 376 package.parameter[3]=Length[3];
b0ssiz 17:774eec1470d8 377
b0ssiz 17:774eec1470d8 378 rs485_dirc=1;
b0ssiz 17:774eec1470d8 379 wait_us(RS485_DELAY);
b0ssiz 17:774eec1470d8 380 return com->sendCommunicatePacket(&package);
b0ssiz 17:774eec1470d8 381
b0ssiz 6:f6529ad2a70d 382 }