Betago / Communication_Robot

v1

Fork of Communication_Robot by palm and chin

Receiver.cpp@13:bc19774be4df, 2016-06-07 (annotated)

Committer:
palmdotax
Date:
Tue Jun 07 03:14:19 2016 +0000
Revision:
13:bc19774be4df
Parent:
11:3c11a0355a3e 
55+

Who changed what in which revision?

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