Secret_Betago / Communication_Robot_V22

สัสชิน

Fork of Communication_Robot by phirawat rattanachote

Receiver.cpp@15:6c5d44c34264, 2016-06-08 (annotated)

Committer:
icyzkungz
Date:
Wed Jun 08 17:17:59 2016 +0000
Revision:
15:6c5d44c34264
Parent:
14:cf43df0ddb93 
??????

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 11:3c11a0355a3e 3
palmdotax 11:3c11a0355a3e 4 DigitalOut rs485_dirc(RS485_DIRC);
palmdotax 11:3c11a0355a3e 5
palmdotax 11:3c11a0355a3e 6 Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate)
palmdotax 11:3c11a0355a3e 7 {
palmdotax 11:3c11a0355a3e 8 com = new COMMUNICATION(tx,rx,baudrate);
palmdotax 11:3c11a0355a3e 9 }
palmdotax 11:3c11a0355a3e 10
palmdotax 11:3c11a0355a3e 11 uint8_t Bear_Receiver::ReceiveCommand(uint8_t *id,uint8_t *data_array,uint8_t *ins)
palmdotax 11:3c11a0355a3e 12 {
palmdotax 11:3c11a0355a3e 13 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 14
palmdotax 11:3c11a0355a3e 15 rs485_dirc=0;
palmdotax 11:3c11a0355a3e 16 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 17 uint8_t status = com->receiveCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 18
palmdotax 11:3c11a0355a3e 19 if(status == ANDANTE_ERRBIT_NONE) {
palmdotax 11:3c11a0355a3e 20
palmdotax 11:3c11a0355a3e 21 *id = package.robotId;
palmdotax 11:3c11a0355a3e 22 for(int i=0; i<30; i++) {
palmdotax 11:3c11a0355a3e 23 data_array[i] = package.parameter[i];
palmdotax 11:3c11a0355a3e 24 }
palmdotax 11:3c11a0355a3e 25 *ins=package.instructionErrorId;
palmdotax 11:3c11a0355a3e 26
palmdotax 11:3c11a0355a3e 27 }
palmdotax 11:3c11a0355a3e 28 return status;
palmdotax 11:3c11a0355a3e 29 }
palmdotax 11:3c11a0355a3e 30
palmdotax 11:3c11a0355a3e 31
palmdotax 11:3c11a0355a3e 32
palmdotax 11:3c11a0355a3e 33
palmdotax 11:3c11a0355a3e 34
palmdotax 11:3c11a0355a3e 35 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
palmdotax 11:3c11a0355a3e 36 {
palmdotax 11:3c11a0355a3e 37 float float_buffer;
palmdotax 11:3c11a0355a3e 38 float int_buffer;
palmdotax 11:3c11a0355a3e 39 int16_t integer;
palmdotax 11:3c11a0355a3e 40 int16_t floating_point;
palmdotax 11:3c11a0355a3e 41
palmdotax 11:3c11a0355a3e 42 float_buffer=modf(input_float,&int_buffer);
palmdotax 11:3c11a0355a3e 43 float_buffer*=FLOAT_CONVERTER;
palmdotax 11:3c11a0355a3e 44 integer=(int16_t)int_buffer;
palmdotax 11:3c11a0355a3e 45 floating_point=(int16_t)float_buffer;
palmdotax 11:3c11a0355a3e 46 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
palmdotax 11:3c11a0355a3e 47 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
palmdotax 11:3c11a0355a3e 48 }
palmdotax 11:3c11a0355a3e 49
palmdotax 11:3c11a0355a3e 50
palmdotax 11:3c11a0355a3e 51
palmdotax 11:3c11a0355a3e 52
palmdotax 11:3c11a0355a3e 53 uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
palmdotax 11:3c11a0355a3e 54 {
palmdotax 11:3c11a0355a3e 55 uint8_t IntUpAngle[2],FloatUpAngle[2];
palmdotax 11:3c11a0355a3e 56 uint8_t IntLowAngle[2],FloatLowAngle[2];
palmdotax 11:3c11a0355a3e 57 FloatSep(up_angle,IntUpAngle,FloatUpAngle);
palmdotax 11:3c11a0355a3e 58 FloatSep(low_angle,IntLowAngle,FloatLowAngle);
palmdotax 11:3c11a0355a3e 59
palmdotax 11:3c11a0355a3e 60 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 61
palmdotax 11:3c11a0355a3e 62 package.robotId = id;
palmdotax 11:3c11a0355a3e 63 package.length = 10;
palmdotax 11:3c11a0355a3e 64 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 65 package.parameter[0]=IntUpAngle[0];
palmdotax 11:3c11a0355a3e 66 package.parameter[1]=IntUpAngle[1];
palmdotax 11:3c11a0355a3e 67 package.parameter[2]=FloatUpAngle[0];
palmdotax 11:3c11a0355a3e 68 package.parameter[3]=FloatUpAngle[1];
palmdotax 11:3c11a0355a3e 69 package.parameter[4]=IntLowAngle[0];
palmdotax 11:3c11a0355a3e 70 package.parameter[5]=IntLowAngle[1];
palmdotax 11:3c11a0355a3e 71 package.parameter[6]=FloatLowAngle[0];
palmdotax 11:3c11a0355a3e 72 package.parameter[7]=FloatLowAngle[1];
palmdotax 11:3c11a0355a3e 73
palmdotax 11:3c11a0355a3e 74 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 75 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 76 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 77
palmdotax 11:3c11a0355a3e 78
palmdotax 11:3c11a0355a3e 79 }
palmdotax 11:3c11a0355a3e 80
palmdotax 11:3c11a0355a3e 81
palmdotax 11:3c11a0355a3e 82
palmdotax 11:3c11a0355a3e 83 uint8_t Bear_Receiver::sendUpMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
palmdotax 11:3c11a0355a3e 84 {
palmdotax 11:3c11a0355a3e 85 uint8_t IntKp[2],FloatKp[2];
palmdotax 11:3c11a0355a3e 86 uint8_t IntKi[2],FloatKi[2];
palmdotax 11:3c11a0355a3e 87 uint8_t IntKd[2],FloatKd[2];
palmdotax 11:3c11a0355a3e 88
palmdotax 11:3c11a0355a3e 89 FloatSep(Kp,IntKp,FloatKp);
palmdotax 11:3c11a0355a3e 90 FloatSep(Ki,IntKi,FloatKi);
palmdotax 11:3c11a0355a3e 91 FloatSep(Kd,IntKd,FloatKd);
palmdotax 11:3c11a0355a3e 92
palmdotax 11:3c11a0355a3e 93
palmdotax 11:3c11a0355a3e 94 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 95
palmdotax 11:3c11a0355a3e 96 package.robotId = id;
palmdotax 11:3c11a0355a3e 97 package.length = 14;
palmdotax 11:3c11a0355a3e 98 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 99 package.parameter[0]=IntKp[0];
palmdotax 11:3c11a0355a3e 100 package.parameter[1]=IntKp[1];
palmdotax 11:3c11a0355a3e 101 package.parameter[2]=FloatKp[0];
palmdotax 11:3c11a0355a3e 102 package.parameter[3]=FloatKp[1];
palmdotax 11:3c11a0355a3e 103 package.parameter[4]=IntKi[0];
palmdotax 11:3c11a0355a3e 104 package.parameter[5]=IntKi[1];
palmdotax 11:3c11a0355a3e 105 package.parameter[6]=FloatKi[0];
palmdotax 11:3c11a0355a3e 106 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 107 package.parameter[8]=IntKd[0];
palmdotax 11:3c11a0355a3e 108 package.parameter[9]=IntKd[1];
palmdotax 11:3c11a0355a3e 109 package.parameter[10]=FloatKd[0];
palmdotax 11:3c11a0355a3e 110 package.parameter[11]=FloatKd[1];
palmdotax 11:3c11a0355a3e 111
palmdotax 11:3c11a0355a3e 112 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 113 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 114 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 115 }
palmdotax 11:3c11a0355a3e 116
palmdotax 11:3c11a0355a3e 117
palmdotax 11:3c11a0355a3e 118
palmdotax 11:3c11a0355a3e 119
palmdotax 11:3c11a0355a3e 120 uint8_t Bear_Receiver::sendLowMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
palmdotax 11:3c11a0355a3e 121 {
palmdotax 11:3c11a0355a3e 122 uint8_t IntKp[2],FloatKp[2];
palmdotax 11:3c11a0355a3e 123 uint8_t IntKi[2],FloatKi[2];
palmdotax 11:3c11a0355a3e 124 uint8_t IntKd[2],FloatKd[2];
palmdotax 11:3c11a0355a3e 125
palmdotax 11:3c11a0355a3e 126 FloatSep(Kp,IntKp,FloatKp);
palmdotax 11:3c11a0355a3e 127 FloatSep(Ki,IntKi,FloatKi);
palmdotax 11:3c11a0355a3e 128 FloatSep(Kd,IntKd,FloatKd);
palmdotax 11:3c11a0355a3e 129
palmdotax 11:3c11a0355a3e 130
palmdotax 11:3c11a0355a3e 131 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 132
palmdotax 11:3c11a0355a3e 133 package.robotId = id;
palmdotax 11:3c11a0355a3e 134 package.length = 14;
palmdotax 11:3c11a0355a3e 135 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 136 package.parameter[0]=IntKp[0];
palmdotax 11:3c11a0355a3e 137 package.parameter[1]=IntKp[1];
palmdotax 11:3c11a0355a3e 138 package.parameter[2]=FloatKp[0];
palmdotax 11:3c11a0355a3e 139 package.parameter[3]=FloatKp[1];
palmdotax 11:3c11a0355a3e 140 package.parameter[4]=IntKi[0];
palmdotax 11:3c11a0355a3e 141 package.parameter[5]=IntKi[1];
palmdotax 11:3c11a0355a3e 142 package.parameter[6]=FloatKi[0];
palmdotax 11:3c11a0355a3e 143 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 144 package.parameter[8]=IntKd[0];
palmdotax 11:3c11a0355a3e 145 package.parameter[9]=IntKd[1];
palmdotax 11:3c11a0355a3e 146 package.parameter[10]=FloatKd[0];
palmdotax 11:3c11a0355a3e 147 package.parameter[11]=FloatKd[1];
palmdotax 11:3c11a0355a3e 148
palmdotax 11:3c11a0355a3e 149 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 150 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 151 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 152 }
palmdotax 11:3c11a0355a3e 153
palmdotax 11:3c11a0355a3e 154
palmdotax 11:3c11a0355a3e 155
palmdotax 11:3c11a0355a3e 156 uint8_t Bear_Receiver::sendUpMargin(uint8_t id,float Margin)
palmdotax 11:3c11a0355a3e 157 {
palmdotax 11:3c11a0355a3e 158 uint8_t Int[2],Float[2];
palmdotax 11:3c11a0355a3e 159 FloatSep(Margin,Int,Float);
palmdotax 11:3c11a0355a3e 160 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 161
palmdotax 11:3c11a0355a3e 162 package.robotId = id;
palmdotax 11:3c11a0355a3e 163 package.length = 6;
palmdotax 11:3c11a0355a3e 164 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 165 package.parameter[0]=Int[0];
palmdotax 11:3c11a0355a3e 166 package.parameter[1]=Int[1];
palmdotax 11:3c11a0355a3e 167 package.parameter[2]=Float[0];
palmdotax 11:3c11a0355a3e 168 package.parameter[3]=Float[1];
palmdotax 11:3c11a0355a3e 169
palmdotax 11:3c11a0355a3e 170 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 171 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 172 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 173
palmdotax 11:3c11a0355a3e 174 }
palmdotax 11:3c11a0355a3e 175
palmdotax 11:3c11a0355a3e 176 uint8_t Bear_Receiver::sendLowMargin(uint8_t id,float Margin)
palmdotax 11:3c11a0355a3e 177 {
palmdotax 11:3c11a0355a3e 178 uint8_t Int[2],Float[2];
palmdotax 11:3c11a0355a3e 179 FloatSep(Margin,Int,Float);
palmdotax 11:3c11a0355a3e 180 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 181
palmdotax 11:3c11a0355a3e 182 package.robotId = id;
palmdotax 11:3c11a0355a3e 183 package.length = 6;
palmdotax 11:3c11a0355a3e 184 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 185 package.parameter[0]=Int[0];
palmdotax 11:3c11a0355a3e 186 package.parameter[1]=Int[1];
palmdotax 11:3c11a0355a3e 187 package.parameter[2]=Float[0];
palmdotax 11:3c11a0355a3e 188 package.parameter[3]=Float[1];
palmdotax 11:3c11a0355a3e 189
palmdotax 11:3c11a0355a3e 190 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 191 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 192 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 193 }
palmdotax 11:3c11a0355a3e 194
palmdotax 11:3c11a0355a3e 195 uint8_t Bear_Receiver::sendHeight(uint8_t id,uint8_t *Height)
palmdotax 11:3c11a0355a3e 196 {
palmdotax 11:3c11a0355a3e 197 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 198
palmdotax 11:3c11a0355a3e 199 package.robotId = id;
palmdotax 11:3c11a0355a3e 200 package.length = 6;
palmdotax 11:3c11a0355a3e 201 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 202 package.parameter[0]=Height[0];
palmdotax 11:3c11a0355a3e 203 package.parameter[1]=Height[1];
palmdotax 11:3c11a0355a3e 204 package.parameter[2]=Height[2];
palmdotax 11:3c11a0355a3e 205 package.parameter[3]=Height[3];
palmdotax 11:3c11a0355a3e 206
palmdotax 11:3c11a0355a3e 207 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 208 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 209 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 210 }
palmdotax 11:3c11a0355a3e 211
palmdotax 11:3c11a0355a3e 212 uint8_t Bear_Receiver::sendWheelPos(uint8_t id,uint8_t *WheelPos)
palmdotax 11:3c11a0355a3e 213 {
palmdotax 11:3c11a0355a3e 214 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 215
palmdotax 11:3c11a0355a3e 216 package.robotId = id;
palmdotax 11:3c11a0355a3e 217 package.length = 6;
palmdotax 11:3c11a0355a3e 218 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 219 package.parameter[0]=WheelPos[0];
palmdotax 11:3c11a0355a3e 220 package.parameter[1]=WheelPos[1];
palmdotax 11:3c11a0355a3e 221 package.parameter[2]=WheelPos[2];
palmdotax 11:3c11a0355a3e 222 package.parameter[3]=WheelPos[3];
palmdotax 11:3c11a0355a3e 223
palmdotax 11:3c11a0355a3e 224 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 225 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 226 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 227 }
palmdotax 11:3c11a0355a3e 228
palmdotax 11:3c11a0355a3e 229 uint8_t Bear_Receiver::sendMagData(uint8_t id,uint8_t *Mag)
palmdotax 11:3c11a0355a3e 230 {
palmdotax 11:3c11a0355a3e 231 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 232
palmdotax 11:3c11a0355a3e 233 package.robotId = id;
palmdotax 11:3c11a0355a3e 234 package.length = 26;
palmdotax 11:3c11a0355a3e 235 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 236 package.parameter[0]=Mag[0];
palmdotax 11:3c11a0355a3e 237 package.parameter[1]=Mag[1];
palmdotax 11:3c11a0355a3e 238 package.parameter[2]=Mag[2];
palmdotax 11:3c11a0355a3e 239 package.parameter[3]=Mag[3];
palmdotax 11:3c11a0355a3e 240 package.parameter[4]=Mag[4];
palmdotax 11:3c11a0355a3e 241 package.parameter[5]=Mag[5];
palmdotax 11:3c11a0355a3e 242 package.parameter[6]=Mag[6];
palmdotax 11:3c11a0355a3e 243 package.parameter[7]=Mag[7];
palmdotax 11:3c11a0355a3e 244 package.parameter[8]=Mag[8];
palmdotax 11:3c11a0355a3e 245 package.parameter[9]=Mag[9];
palmdotax 11:3c11a0355a3e 246 package.parameter[10]=Mag[10];
palmdotax 11:3c11a0355a3e 247 package.parameter[11]=Mag[11];
palmdotax 11:3c11a0355a3e 248 package.parameter[12]=Mag[12];
palmdotax 11:3c11a0355a3e 249 package.parameter[13]=Mag[13];
palmdotax 11:3c11a0355a3e 250 package.parameter[14]=Mag[14];
palmdotax 11:3c11a0355a3e 251 package.parameter[15]=Mag[15];
palmdotax 11:3c11a0355a3e 252 package.parameter[16]=Mag[16];
palmdotax 11:3c11a0355a3e 253 package.parameter[17]=Mag[17];
palmdotax 11:3c11a0355a3e 254 package.parameter[18]=Mag[18];
palmdotax 11:3c11a0355a3e 255 package.parameter[19]=Mag[19];
palmdotax 11:3c11a0355a3e 256 package.parameter[20]=Mag[20];
palmdotax 11:3c11a0355a3e 257 package.parameter[21]=Mag[21];
palmdotax 11:3c11a0355a3e 258 package.parameter[22]=Mag[22];
palmdotax 11:3c11a0355a3e 259 package.parameter[23]=Mag[23];
palmdotax 11:3c11a0355a3e 260
palmdotax 11:3c11a0355a3e 261 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 262 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 263 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 264 }
palmdotax 11:3c11a0355a3e 265
palmdotax 11:3c11a0355a3e 266 uint8_t Bear_Receiver::sendOffset(uint8_t id,uint8_t *Offset)
palmdotax 11:3c11a0355a3e 267 {
palmdotax 11:3c11a0355a3e 268 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 269
palmdotax 11:3c11a0355a3e 270 package.robotId = id;
palmdotax 11:3c11a0355a3e 271 package.length = 10;
palmdotax 11:3c11a0355a3e 272 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 273 package.parameter[0]=Offset[0];
palmdotax 11:3c11a0355a3e 274 package.parameter[1]=Offset[1];
palmdotax 11:3c11a0355a3e 275 package.parameter[2]=Offset[2];
palmdotax 11:3c11a0355a3e 276 package.parameter[3]=Offset[3];
palmdotax 11:3c11a0355a3e 277 package.parameter[4]=Offset[4];
palmdotax 11:3c11a0355a3e 278 package.parameter[5]=Offset[5];
palmdotax 11:3c11a0355a3e 279 package.parameter[6]=Offset[6];
palmdotax 11:3c11a0355a3e 280 package.parameter[7]=Offset[7];
palmdotax 11:3c11a0355a3e 281
palmdotax 11:3c11a0355a3e 282 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 283 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 284 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 285 }
palmdotax 11:3c11a0355a3e 286
palmdotax 11:3c11a0355a3e 287 uint8_t Bear_Receiver::sendBodyWidth(uint8_t id,uint8_t *BodyWidth)
palmdotax 11:3c11a0355a3e 288 {
palmdotax 11:3c11a0355a3e 289 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 290
palmdotax 11:3c11a0355a3e 291 package.robotId = id;
palmdotax 11:3c11a0355a3e 292 package.length = 6;
palmdotax 11:3c11a0355a3e 293 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 294 package.parameter[0]=BodyWidth[0];
palmdotax 11:3c11a0355a3e 295 package.parameter[1]=BodyWidth[1];
palmdotax 11:3c11a0355a3e 296 package.parameter[2]=BodyWidth[2];
palmdotax 11:3c11a0355a3e 297 package.parameter[3]=BodyWidth[3];
palmdotax 11:3c11a0355a3e 298
palmdotax 11:3c11a0355a3e 299 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 300 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 301 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 302 }
palmdotax 11:3c11a0355a3e 303
palmdotax 11:3c11a0355a3e 304 uint8_t Bear_Receiver::sendUpAngleRange(uint8_t id,uint8_t *Angle)
palmdotax 11:3c11a0355a3e 305 {
palmdotax 11:3c11a0355a3e 306 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 307
palmdotax 11:3c11a0355a3e 308 package.robotId = id;
palmdotax 11:3c11a0355a3e 309 package.length = 10;
palmdotax 11:3c11a0355a3e 310 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 311 package.parameter[0]=Angle[0];
palmdotax 11:3c11a0355a3e 312 package.parameter[1]=Angle[1];
palmdotax 11:3c11a0355a3e 313 package.parameter[2]=Angle[2];
palmdotax 11:3c11a0355a3e 314 package.parameter[3]=Angle[3];
palmdotax 11:3c11a0355a3e 315 package.parameter[4]=Angle[4];
palmdotax 11:3c11a0355a3e 316 package.parameter[5]=Angle[5];
palmdotax 11:3c11a0355a3e 317 package.parameter[6]=Angle[6];
palmdotax 11:3c11a0355a3e 318 package.parameter[7]=Angle[7];
palmdotax 11:3c11a0355a3e 319
palmdotax 11:3c11a0355a3e 320 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 321 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 322 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 323 }
palmdotax 11:3c11a0355a3e 324
palmdotax 11:3c11a0355a3e 325
palmdotax 11:3c11a0355a3e 326 uint8_t Bear_Receiver::sendLowAngleRange(uint8_t id,uint8_t *Angle)
palmdotax 11:3c11a0355a3e 327 {
palmdotax 11:3c11a0355a3e 328 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 329
palmdotax 11:3c11a0355a3e 330 package.robotId = id;
palmdotax 11:3c11a0355a3e 331 package.length = 10;
palmdotax 11:3c11a0355a3e 332 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 333 package.parameter[0]=Angle[0];
palmdotax 11:3c11a0355a3e 334 package.parameter[1]=Angle[1];
palmdotax 11:3c11a0355a3e 335 package.parameter[2]=Angle[2];
palmdotax 11:3c11a0355a3e 336 package.parameter[3]=Angle[3];
palmdotax 11:3c11a0355a3e 337 package.parameter[4]=Angle[4];
palmdotax 11:3c11a0355a3e 338 package.parameter[5]=Angle[5];
palmdotax 11:3c11a0355a3e 339 package.parameter[6]=Angle[6];
palmdotax 11:3c11a0355a3e 340 package.parameter[7]=Angle[7];
palmdotax 11:3c11a0355a3e 341
palmdotax 11:3c11a0355a3e 342 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 343 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 344 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 345 }
palmdotax 11:3c11a0355a3e 346
palmdotax 11:3c11a0355a3e 347 uint8_t Bear_Receiver::sendUpLinkLength(uint8_t id,uint8_t *Length)
palmdotax 11:3c11a0355a3e 348 {
palmdotax 11:3c11a0355a3e 349 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 350
palmdotax 11:3c11a0355a3e 351 package.robotId = id;
palmdotax 11:3c11a0355a3e 352 package.length = 6;
palmdotax 11:3c11a0355a3e 353 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 354 package.parameter[0]=Length[0];
palmdotax 11:3c11a0355a3e 355 package.parameter[1]=Length[1];
palmdotax 11:3c11a0355a3e 356 package.parameter[2]=Length[2];
palmdotax 11:3c11a0355a3e 357 package.parameter[3]=Length[3];
palmdotax 11:3c11a0355a3e 358
palmdotax 11:3c11a0355a3e 359 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 360 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 361 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 362
palmdotax 11:3c11a0355a3e 363 }
palmdotax 11:3c11a0355a3e 364
palmdotax 11:3c11a0355a3e 365 uint8_t Bear_Receiver::sendLowLinkLength(uint8_t id,uint8_t *Length)
palmdotax 11:3c11a0355a3e 366 {
palmdotax 11:3c11a0355a3e 367 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 368
palmdotax 11:3c11a0355a3e 369 package.robotId = id;
palmdotax 11:3c11a0355a3e 370 package.length = 6;
palmdotax 11:3c11a0355a3e 371 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 372 package.parameter[0]=Length[0];
palmdotax 11:3c11a0355a3e 373 package.parameter[1]=Length[1];
palmdotax 11:3c11a0355a3e 374 package.parameter[2]=Length[2];
palmdotax 11:3c11a0355a3e 375 package.parameter[3]=Length[3];
palmdotax 11:3c11a0355a3e 376
palmdotax 11:3c11a0355a3e 377 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 378 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 379 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 380
palmdotax 11:3c11a0355a3e 381 }
icyzkungz 15:6c5d44c34264 382 uint8_t Bear_Receiver::sendlidar(RPLidar &lidar1)
palmdotax 13:bc19774be4df 383 {
icyzkungz 15:6c5d44c34264 384 int i=0,j=1,k=0;
icyzkungz 15:6c5d44c34264 385 uint8_t intData[2]= {0x00,0x01},floatData[2];
icyzkungz 15:6c5d44c34264 386 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 387 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 388 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 389 package.length = 62;//122
icyzkungz 15:6c5d44c34264 390 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 391
icyzkungz 15:6c5d44c34264 392 while(k<60) {
icyzkungz 15:6c5d44c34264 393 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 394 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 395 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 396 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 397 i=i+2;
icyzkungz 15:6c5d44c34264 398 j=j+2;
icyzkungz 15:6c5d44c34264 399 k++;
icyzkungz 15:6c5d44c34264 400 }
icyzkungz 15:6c5d44c34264 401
icyzkungz 15:6c5d44c34264 402 rs485_dirc=1;
icyzkungz 15:6c5d44c34264 403 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 404 return com->sendCommunicatePacket(&package);
palmdotax 13:bc19774be4df 405
palmdotax 13:bc19774be4df 406 }
icyzkungz 15:6c5d44c34264 407 uint8_t Bear_Receiver::sendlidar2(RPLidar &lidar1)
palmdotax 14:cf43df0ddb93 408 {
icyzkungz 15:6c5d44c34264 409 int i=0,j=1,k=60;
icyzkungz 15:6c5d44c34264 410 uint8_t intData[2],floatData[2];
icyzkungz 15:6c5d44c34264 411 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 412 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 413 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 414 package.length = 122;
icyzkungz 15:6c5d44c34264 415 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 416
icyzkungz 15:6c5d44c34264 417 while(k<120) {
icyzkungz 15:6c5d44c34264 418 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 419 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 420 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 421 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 422 i=i+2;
icyzkungz 15:6c5d44c34264 423 j=j+2;
icyzkungz 15:6c5d44c34264 424 k++;
icyzkungz 15:6c5d44c34264 425
icyzkungz 15:6c5d44c34264 426 }
icyzkungz 15:6c5d44c34264 427 //rs485_dirc1=1;
icyzkungz 15:6c5d44c34264 428 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 429 return com->sendCommunicatePacket(&package);
icyzkungz 15:6c5d44c34264 430
icyzkungz 15:6c5d44c34264 431 }
icyzkungz 15:6c5d44c34264 432 uint8_t Bear_Receiver::sendlidar3(RPLidar &lidar1)
icyzkungz 15:6c5d44c34264 433 {
icyzkungz 15:6c5d44c34264 434 int i=0,j=1,k=120;
icyzkungz 15:6c5d44c34264 435 uint8_t intData[2],floatData[2];
icyzkungz 15:6c5d44c34264 436 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 437 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 438 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 439 package.length = 122;
icyzkungz 15:6c5d44c34264 440 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 441 while(k<180) {
icyzkungz 15:6c5d44c34264 442 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 443 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 444 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 445 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 446 i=i+2;
icyzkungz 15:6c5d44c34264 447 j=j+2;
icyzkungz 15:6c5d44c34264 448 k++;
icyzkungz 15:6c5d44c34264 449 }
icyzkungz 15:6c5d44c34264 450 //rs485_dirc1=1;
icyzkungz 15:6c5d44c34264 451 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 452 return com->sendCommunicatePacket(&package);
palmdotax 14:cf43df0ddb93 453
palmdotax 14:cf43df0ddb93 454 }
icyzkungz 15:6c5d44c34264 455 uint8_t Bear_Receiver::sendlidar4(RPLidar &lidar1)
palmdotax 14:cf43df0ddb93 456 {
icyzkungz 15:6c5d44c34264 457 int i=0,j=1,k=180;
icyzkungz 15:6c5d44c34264 458 uint8_t intData[2],floatData[2];
icyzkungz 15:6c5d44c34264 459 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 460 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 461 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 462 package.length = 122;
icyzkungz 15:6c5d44c34264 463 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 464 while(k<240) {
icyzkungz 15:6c5d44c34264 465 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 466 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 467 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 468 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 469 i=i+2;
icyzkungz 15:6c5d44c34264 470 j=j+2;
icyzkungz 15:6c5d44c34264 471 k++;
icyzkungz 15:6c5d44c34264 472 }
icyzkungz 15:6c5d44c34264 473 // rs485_dirc1=1;
icyzkungz 15:6c5d44c34264 474 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 475 return com->sendCommunicatePacket(&package);
palmdotax 14:cf43df0ddb93 476
palmdotax 14:cf43df0ddb93 477 }
icyzkungz 15:6c5d44c34264 478 uint8_t Bear_Receiver::sendlidar5(RPLidar &lidar1)
palmdotax 14:cf43df0ddb93 479 {
icyzkungz 15:6c5d44c34264 480 int i=0,j=1,k=240;
icyzkungz 15:6c5d44c34264 481 uint8_t intData[2],floatData[2];
icyzkungz 15:6c5d44c34264 482 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 483 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 484 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 485 package.length = 122;
icyzkungz 15:6c5d44c34264 486 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 487 while(k<300) {
icyzkungz 15:6c5d44c34264 488 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 489 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 490 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 491 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 492 i=i+2;
icyzkungz 15:6c5d44c34264 493 j=j+2;
icyzkungz 15:6c5d44c34264 494 k++;
icyzkungz 15:6c5d44c34264 495 }
icyzkungz 15:6c5d44c34264 496
icyzkungz 15:6c5d44c34264 497 // rs485_dirc1=1;
icyzkungz 15:6c5d44c34264 498 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 499 return com->sendCommunicatePacket(&package);
palmdotax 14:cf43df0ddb93 500 }
icyzkungz 15:6c5d44c34264 501 uint8_t Bear_Receiver::sendlidar6(RPLidar &lidar1)
palmdotax 14:cf43df0ddb93 502 {
icyzkungz 15:6c5d44c34264 503 int i=0,j=1,k=300;
icyzkungz 15:6c5d44c34264 504 uint8_t intData[2],floatData[2];
icyzkungz 15:6c5d44c34264 505 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 15:6c5d44c34264 506 //BUFFER_SIZE=143
icyzkungz 15:6c5d44c34264 507 package.robotId = 0x00;
icyzkungz 15:6c5d44c34264 508 package.length = 122;
icyzkungz 15:6c5d44c34264 509 package.instructionErrorId = WRITE_DATA;
icyzkungz 15:6c5d44c34264 510 while(k<360) {
icyzkungz 15:6c5d44c34264 511 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 15:6c5d44c34264 512 FloatSep( lidar1.Data[k],intData,floatData);
icyzkungz 15:6c5d44c34264 513 package.parameter[i]=intData[0];
icyzkungz 15:6c5d44c34264 514 package.parameter[j]=intData[1];
icyzkungz 15:6c5d44c34264 515 i=i+2;
icyzkungz 15:6c5d44c34264 516 j=j+2;
icyzkungz 15:6c5d44c34264 517 k++;
icyzkungz 15:6c5d44c34264 518 }
icyzkungz 15:6c5d44c34264 519 // rs485_dirc1=1;
icyzkungz 15:6c5d44c34264 520 wait_us(RS485_DELAY);
icyzkungz 15:6c5d44c34264 521 return com->sendCommunicatePacket(&package);
icyzkungz 15:6c5d44c34264 522
icyzkungz 15:6c5d44c34264 523 }
icyzkungz 15:6c5d44c34264 524
icyzkungz 15:6c5d44c34264 525 void Bear_Receiver::UpdateLidar()
icyzkungz 15:6c5d44c34264 526 {
icyzkungz 15:6c5d44c34264 527 if (!IS_OK(lidar1.waitPoint()))
icyzkungz 15:6c5d44c34264 528 lidar1.startScan();
icyzkungz 15:6c5d44c34264 529
icyzkungz 15:6c5d44c34264 530 /*if(sync_lidar.read_ms()>=1) {
icyzkungz 15:6c5d44c34264 531 lidar1.printData();
icyzkungz 15:6c5d44c34264 532 sync_lidar.reset();
icyzkungz 15:6c5d44c34264 533 }*/
palmdotax 14:cf43df0ddb93 534 }