BE@R lab / BEAR_Protocol

ใช้สื่อสารกันระหว่าง Brain และ Motion

Dependencies:   Fork_Boss_Communication_Robot

Dependents:   Program_BEAR_Protocol SwitchMode MPU9250AHRS-PGear_Stabilizer SwitchMode ... more

BEAR_Protocol.cpp@9:c22410169d9f, 2016-01-21 (annotated)

Committer:
b0ssiz
Date:
Thu Jan 21 18:48:02 2016 +0000
Revision:
9:c22410169d9f
Parent:
8:e1f43b1df0b5
Child:
10:2398eeafa967
fixed semi-colon bug

Who changed what in which revision?

UserRevisionLine numberNew contents of line
b0ssiz 0:fc963e08d580 1 #include "mbed.h"
b0ssiz 0:fc963e08d580 2 #include "BEAR_Protocol.h"
b0ssiz 2:d17ccaf938f6 3 #define RS485_DELAY 90
b0ssiz 4:9fbe67ca2f1b 4 #define FLOAT_CONVERTER 10000
b0ssiz 4:9fbe67ca2f1b 5
b0ssiz 0:fc963e08d580 6 DigitalOut rs485_dirc(RS485_DIRC);
b0ssiz 0:fc963e08d580 7
b0ssiz 0:fc963e08d580 8 Bear_Communicate::Bear_Communicate(PinName tx,PinName rx,int baudrate)
b0ssiz 0:fc963e08d580 9 {
b0ssiz 0:fc963e08d580 10 com = new COMMUNICATION(tx,rx,baudrate);
b0ssiz 0:fc963e08d580 11 }
b0ssiz 0:fc963e08d580 12
b0ssiz 0:fc963e08d580 13
b0ssiz 0:fc963e08d580 14
b0ssiz 4:9fbe67ca2f1b 15 void Bear_Communicate::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
b0ssiz 4:9fbe67ca2f1b 16 {
b0ssiz 4:9fbe67ca2f1b 17 float float_buffer;
b0ssiz 4:9fbe67ca2f1b 18 float int_buffer;
b0ssiz 4:9fbe67ca2f1b 19 int16_t integer;
b0ssiz 4:9fbe67ca2f1b 20 int16_t floating_point;
b0ssiz 4:9fbe67ca2f1b 21
b0ssiz 4:9fbe67ca2f1b 22 float_buffer=modf(input_float,&int_buffer);
b0ssiz 4:9fbe67ca2f1b 23 float_buffer*=FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 24 integer=(int16_t)int_buffer;
b0ssiz 4:9fbe67ca2f1b 25 floating_point=(int16_t)float_buffer;
b0ssiz 4:9fbe67ca2f1b 26 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
b0ssiz 4:9fbe67ca2f1b 27 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
b0ssiz 4:9fbe67ca2f1b 28 }
b0ssiz 4:9fbe67ca2f1b 29
b0ssiz 4:9fbe67ca2f1b 30
b0ssiz 4:9fbe67ca2f1b 31
b0ssiz 8:e1f43b1df0b5 32 uint8_t Bear_Communicate::saveDataToEEPROM(uint8_t id,uint8_t save_data)
b0ssiz 8:e1f43b1df0b5 33 {
b0ssiz 8:e1f43b1df0b5 34 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:e1f43b1df0b5 35
b0ssiz 8:e1f43b1df0b5 36 package.robotId = id;
b0ssiz 8:e1f43b1df0b5 37 package.length = 4;
b0ssiz 8:e1f43b1df0b5 38 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:e1f43b1df0b5 39 package.parameter[0]=SAVE_EEPROM_DATA;
b0ssiz 8:e1f43b1df0b5 40 package.parameter[1]=save_data;
b0ssiz 8:e1f43b1df0b5 41
b0ssiz 8:e1f43b1df0b5 42 rs485_dirc=1;
b0ssiz 8:e1f43b1df0b5 43 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 44 return com->sendCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 45 }
b0ssiz 8:e1f43b1df0b5 46
b0ssiz 8:e1f43b1df0b5 47
b0ssiz 4:9fbe67ca2f1b 48 uint8_t Bear_Communicate::setID(uint8_t id,uint8_t new_id)
b0ssiz 0:fc963e08d580 49 {
b0ssiz 0:fc963e08d580 50 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 51
b0ssiz 0:fc963e08d580 52 package.robotId = id;
b0ssiz 0:fc963e08d580 53 package.length = 4;
b0ssiz 0:fc963e08d580 54 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 55 package.parameter[0]=ID;
b0ssiz 0:fc963e08d580 56 package.parameter[1]=new_id;
b0ssiz 0:fc963e08d580 57
b0ssiz 0:fc963e08d580 58 rs485_dirc=1;
b0ssiz 0:fc963e08d580 59 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 60 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 61 }
b0ssiz 0:fc963e08d580 62
b0ssiz 0:fc963e08d580 63
b0ssiz 0:fc963e08d580 64
b0ssiz 4:9fbe67ca2f1b 65 uint8_t Bear_Communicate::setMotorPos(uint8_t id,float up_angle,float low_angle)
b0ssiz 0:fc963e08d580 66 {
b0ssiz 4:9fbe67ca2f1b 67 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 4:9fbe67ca2f1b 68 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 4:9fbe67ca2f1b 69 FloatSep(up_angle,IntUpAngle,FloatUpAngle);
b0ssiz 4:9fbe67ca2f1b 70 FloatSep(low_angle,IntLowAngle,FloatLowAngle);
b0ssiz 4:9fbe67ca2f1b 71
b0ssiz 0:fc963e08d580 72 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 73
b0ssiz 0:fc963e08d580 74 package.robotId = id;
b0ssiz 4:9fbe67ca2f1b 75 package.length = 11;
b0ssiz 0:fc963e08d580 76 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 77 package.parameter[0]=MOTOR_UPPER_ANG;
b0ssiz 4:9fbe67ca2f1b 78 package.parameter[1]=IntUpAngle[0];
b0ssiz 4:9fbe67ca2f1b 79 package.parameter[2]=IntUpAngle[1];
b0ssiz 4:9fbe67ca2f1b 80 package.parameter[3]=FloatUpAngle[0];
b0ssiz 4:9fbe67ca2f1b 81 package.parameter[4]=FloatUpAngle[1];
b0ssiz 4:9fbe67ca2f1b 82 package.parameter[5]=IntLowAngle[0];
b0ssiz 4:9fbe67ca2f1b 83 package.parameter[6]=IntLowAngle[1];
b0ssiz 4:9fbe67ca2f1b 84 package.parameter[7]=FloatLowAngle[0];
b0ssiz 4:9fbe67ca2f1b 85 package.parameter[8]=FloatLowAngle[1];
b0ssiz 0:fc963e08d580 86
b0ssiz 0:fc963e08d580 87 rs485_dirc=1;
b0ssiz 0:fc963e08d580 88 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 89 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 90 }
b0ssiz 0:fc963e08d580 91
b0ssiz 0:fc963e08d580 92
b0ssiz 0:fc963e08d580 93
b0ssiz 4:9fbe67ca2f1b 94 uint8_t Bear_Communicate::getMotorPos(uint8_t id,float *up_angle,float *low_angle)
b0ssiz 0:fc963e08d580 95 {
b0ssiz 4:9fbe67ca2f1b 96 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 4:9fbe67ca2f1b 97 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 4:9fbe67ca2f1b 98 float int_buffer,float_buffer;
b0ssiz 4:9fbe67ca2f1b 99
b0ssiz 0:fc963e08d580 100 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 101
b0ssiz 0:fc963e08d580 102 package.robotId = id;
b0ssiz 7:ce4234c56410 103 package.length = 3;
b0ssiz 0:fc963e08d580 104 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 105 package.parameter[0] = MOTOR_UPPER_ANG;
b0ssiz 0:fc963e08d580 106
b0ssiz 0:fc963e08d580 107 rs485_dirc=1;
b0ssiz 0:fc963e08d580 108 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 109 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 110
b0ssiz 0:fc963e08d580 111 rs485_dirc=0;
b0ssiz 0:fc963e08d580 112 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 113 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 114
b0ssiz 0:fc963e08d580 115 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 6:07749b50d600 116 IntUpAngle[0]=package.parameter[0];
b0ssiz 6:07749b50d600 117 IntUpAngle[1]=package.parameter[1];
b0ssiz 6:07749b50d600 118 FloatUpAngle[0]=package.parameter[2];
b0ssiz 6:07749b50d600 119 FloatUpAngle[1]=package.parameter[3];
b0ssiz 4:9fbe67ca2f1b 120 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
b0ssiz 4:9fbe67ca2f1b 121 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 122 *up_angle=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 123
b0ssiz 6:07749b50d600 124 IntLowAngle[0]=package.parameter[4];
b0ssiz 6:07749b50d600 125 IntLowAngle[1]=package.parameter[5];
b0ssiz 6:07749b50d600 126 FloatLowAngle[0]=package.parameter[6];
b0ssiz 6:07749b50d600 127 FloatLowAngle[1]=package.parameter[7];
b0ssiz 4:9fbe67ca2f1b 128 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
b0ssiz 4:9fbe67ca2f1b 129 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
b0ssiz 9:c22410169d9f 130 *low_angle=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 131
b0ssiz 0:fc963e08d580 132 }
b0ssiz 0:fc963e08d580 133 return status;
b0ssiz 0:fc963e08d580 134 }
b0ssiz 0:fc963e08d580 135
b0ssiz 0:fc963e08d580 136
b0ssiz 0:fc963e08d580 137
b0ssiz 5:6f30b4ea4020 138 uint8_t Bear_Communicate::setUpMotorKp(uint8_t id,float Kp)
b0ssiz 0:fc963e08d580 139 {
b0ssiz 4:9fbe67ca2f1b 140 uint8_t IntKp[2],FloatKp[2];
b0ssiz 4:9fbe67ca2f1b 141 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 4:9fbe67ca2f1b 142
b0ssiz 0:fc963e08d580 143 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 144
b0ssiz 0:fc963e08d580 145 package.robotId = id;
b0ssiz 4:9fbe67ca2f1b 146 package.length = 7;
b0ssiz 0:fc963e08d580 147 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 148 package.parameter[0]=KP_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 149 package.parameter[1]=IntKp[0];
b0ssiz 4:9fbe67ca2f1b 150 package.parameter[2]=IntKp[1];
b0ssiz 4:9fbe67ca2f1b 151 package.parameter[3]=FloatKp[0];
b0ssiz 4:9fbe67ca2f1b 152 package.parameter[4]=FloatKp[1];
b0ssiz 0:fc963e08d580 153
b0ssiz 0:fc963e08d580 154
b0ssiz 0:fc963e08d580 155 rs485_dirc=1;
b0ssiz 0:fc963e08d580 156 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 157 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 158 }
b0ssiz 0:fc963e08d580 159
b0ssiz 0:fc963e08d580 160
b0ssiz 0:fc963e08d580 161
b0ssiz 5:6f30b4ea4020 162
b0ssiz 5:6f30b4ea4020 163 uint8_t Bear_Communicate::setLowMotorKp(uint8_t id,float Kp)
b0ssiz 5:6f30b4ea4020 164 {
b0ssiz 5:6f30b4ea4020 165 uint8_t IntKp[2],FloatKp[2];
b0ssiz 5:6f30b4ea4020 166 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 5:6f30b4ea4020 167
b0ssiz 5:6f30b4ea4020 168 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 169
b0ssiz 5:6f30b4ea4020 170 package.robotId = id;
b0ssiz 5:6f30b4ea4020 171 package.length = 7;
b0ssiz 5:6f30b4ea4020 172 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 173 package.parameter[0]=KP_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 174 package.parameter[1]=IntKp[0];
b0ssiz 5:6f30b4ea4020 175 package.parameter[2]=IntKp[1];
b0ssiz 5:6f30b4ea4020 176 package.parameter[3]=FloatKp[0];
b0ssiz 5:6f30b4ea4020 177 package.parameter[4]=FloatKp[1];
b0ssiz 5:6f30b4ea4020 178
b0ssiz 5:6f30b4ea4020 179
b0ssiz 5:6f30b4ea4020 180 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 181 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 182 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 183 }
b0ssiz 5:6f30b4ea4020 184
b0ssiz 5:6f30b4ea4020 185
b0ssiz 5:6f30b4ea4020 186
b0ssiz 5:6f30b4ea4020 187
b0ssiz 5:6f30b4ea4020 188 uint8_t Bear_Communicate::setUpMotorKi(uint8_t id,float Ki)
b0ssiz 0:fc963e08d580 189 {
b0ssiz 4:9fbe67ca2f1b 190 uint8_t IntKi[2],FloatKi[2];
b0ssiz 4:9fbe67ca2f1b 191 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 4:9fbe67ca2f1b 192
b0ssiz 0:fc963e08d580 193 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 194
b0ssiz 0:fc963e08d580 195 package.robotId = id;
b0ssiz 7:ce4234c56410 196 package.length = 7;
b0ssiz 0:fc963e08d580 197 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 198 package.parameter[0]=KI_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 199 package.parameter[1]=IntKi[0];
b0ssiz 4:9fbe67ca2f1b 200 package.parameter[2]=IntKi[1];
b0ssiz 4:9fbe67ca2f1b 201 package.parameter[3]=FloatKi[0];
b0ssiz 4:9fbe67ca2f1b 202 package.parameter[4]=FloatKi[1];
b0ssiz 4:9fbe67ca2f1b 203
b0ssiz 4:9fbe67ca2f1b 204
b0ssiz 4:9fbe67ca2f1b 205 rs485_dirc=1;
b0ssiz 4:9fbe67ca2f1b 206 wait_us(RS485_DELAY);
b0ssiz 4:9fbe67ca2f1b 207 return com->sendCommunicatePacket(&package);
b0ssiz 4:9fbe67ca2f1b 208
b0ssiz 4:9fbe67ca2f1b 209 }
b0ssiz 4:9fbe67ca2f1b 210
b0ssiz 4:9fbe67ca2f1b 211
b0ssiz 4:9fbe67ca2f1b 212
b0ssiz 5:6f30b4ea4020 213
b0ssiz 5:6f30b4ea4020 214 uint8_t Bear_Communicate::setLowMotorKi(uint8_t id,float Ki)
b0ssiz 5:6f30b4ea4020 215 {
b0ssiz 5:6f30b4ea4020 216 uint8_t IntKi[2],FloatKi[2];
b0ssiz 5:6f30b4ea4020 217 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 5:6f30b4ea4020 218
b0ssiz 5:6f30b4ea4020 219 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 220
b0ssiz 5:6f30b4ea4020 221 package.robotId = id;
b0ssiz 7:ce4234c56410 222 package.length = 7;
b0ssiz 5:6f30b4ea4020 223 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 224 package.parameter[0]=KI_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 225 package.parameter[1]=IntKi[0];
b0ssiz 5:6f30b4ea4020 226 package.parameter[2]=IntKi[1];
b0ssiz 5:6f30b4ea4020 227 package.parameter[3]=FloatKi[0];
b0ssiz 5:6f30b4ea4020 228 package.parameter[4]=FloatKi[1];
b0ssiz 5:6f30b4ea4020 229
b0ssiz 5:6f30b4ea4020 230
b0ssiz 5:6f30b4ea4020 231
b0ssiz 5:6f30b4ea4020 232 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 233 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 234 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 235
b0ssiz 5:6f30b4ea4020 236 }
b0ssiz 5:6f30b4ea4020 237
b0ssiz 5:6f30b4ea4020 238
b0ssiz 5:6f30b4ea4020 239
b0ssiz 5:6f30b4ea4020 240 uint8_t Bear_Communicate::setUpMotorKd(uint8_t id,float Kd)
b0ssiz 4:9fbe67ca2f1b 241 {
b0ssiz 4:9fbe67ca2f1b 242 uint8_t IntKd[2],FloatKd[2];
b0ssiz 4:9fbe67ca2f1b 243 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 4:9fbe67ca2f1b 244
b0ssiz 4:9fbe67ca2f1b 245 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 4:9fbe67ca2f1b 246
b0ssiz 4:9fbe67ca2f1b 247 package.robotId = id;
b0ssiz 4:9fbe67ca2f1b 248 package.length = 7;
b0ssiz 4:9fbe67ca2f1b 249 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 250 package.parameter[0]=KD_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 251 package.parameter[1]=IntKd[0];
b0ssiz 4:9fbe67ca2f1b 252 package.parameter[2]=IntKd[1];
b0ssiz 4:9fbe67ca2f1b 253 package.parameter[3]=FloatKd[0];
b0ssiz 4:9fbe67ca2f1b 254 package.parameter[4]=FloatKd[1];
b0ssiz 0:fc963e08d580 255
b0ssiz 0:fc963e08d580 256
b0ssiz 0:fc963e08d580 257 rs485_dirc=1;
b0ssiz 0:fc963e08d580 258 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 259 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 260 }
b0ssiz 0:fc963e08d580 261
b0ssiz 0:fc963e08d580 262
b0ssiz 0:fc963e08d580 263
b0ssiz 5:6f30b4ea4020 264 uint8_t Bear_Communicate::setLowMotorKd(uint8_t id,float Kd)
b0ssiz 5:6f30b4ea4020 265 {
b0ssiz 5:6f30b4ea4020 266 uint8_t IntKd[2],FloatKd[2];
b0ssiz 5:6f30b4ea4020 267 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 5:6f30b4ea4020 268
b0ssiz 5:6f30b4ea4020 269 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 270
b0ssiz 5:6f30b4ea4020 271 package.robotId = id;
b0ssiz 5:6f30b4ea4020 272 package.length = 7;
b0ssiz 5:6f30b4ea4020 273 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 274 package.parameter[0]=KD_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 275 package.parameter[1]=IntKd[0];
b0ssiz 5:6f30b4ea4020 276 package.parameter[2]=IntKd[1];
b0ssiz 5:6f30b4ea4020 277 package.parameter[3]=FloatKd[0];
b0ssiz 5:6f30b4ea4020 278 package.parameter[4]=FloatKd[1];
b0ssiz 5:6f30b4ea4020 279
b0ssiz 5:6f30b4ea4020 280
b0ssiz 5:6f30b4ea4020 281 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 282 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 283 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 284 }
b0ssiz 5:6f30b4ea4020 285
b0ssiz 5:6f30b4ea4020 286
b0ssiz 5:6f30b4ea4020 287
b0ssiz 5:6f30b4ea4020 288 uint8_t Bear_Communicate::getUpMotorKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 0:fc963e08d580 289 {
b0ssiz 4:9fbe67ca2f1b 290 uint8_t IntKp[2],FloatKp[2];
b0ssiz 4:9fbe67ca2f1b 291 uint8_t IntKi[2],FloatKi[2];
b0ssiz 4:9fbe67ca2f1b 292 uint8_t IntKd[2],FloatKd[2];
b0ssiz 4:9fbe67ca2f1b 293 float int_buffer,float_buffer;
b0ssiz 4:9fbe67ca2f1b 294
b0ssiz 0:fc963e08d580 295 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 296
b0ssiz 0:fc963e08d580 297 package.robotId = id;
b0ssiz 7:ce4234c56410 298 package.length = 3;
b0ssiz 0:fc963e08d580 299 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 300 package.parameter[0]=PID_UPPER_MOTOR;
b0ssiz 0:fc963e08d580 301
b0ssiz 0:fc963e08d580 302 rs485_dirc=1;
b0ssiz 0:fc963e08d580 303 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 304 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 305
b0ssiz 0:fc963e08d580 306 rs485_dirc=0;
b0ssiz 0:fc963e08d580 307 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 308 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 309 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 4:9fbe67ca2f1b 310 IntKp[0]=package.parameter[0];
b0ssiz 4:9fbe67ca2f1b 311 IntKp[1]=package.parameter[1];
b0ssiz 4:9fbe67ca2f1b 312 FloatKp[0]=package.parameter[2];
b0ssiz 4:9fbe67ca2f1b 313 FloatKp[1]=package.parameter[3];
b0ssiz 4:9fbe67ca2f1b 314 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp);
b0ssiz 4:9fbe67ca2f1b 315 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 316 *Kp=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 317
b0ssiz 4:9fbe67ca2f1b 318 IntKi[0]=package.parameter[4];
b0ssiz 4:9fbe67ca2f1b 319 IntKi[1]=package.parameter[5];
b0ssiz 4:9fbe67ca2f1b 320 FloatKi[0]=package.parameter[6];
b0ssiz 4:9fbe67ca2f1b 321 FloatKi[1]=package.parameter[7];
b0ssiz 4:9fbe67ca2f1b 322 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi);
b0ssiz 4:9fbe67ca2f1b 323 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 324 *Ki=int_buffer+float_buffer;
b0ssiz 4:9fbe67ca2f1b 325
b0ssiz 4:9fbe67ca2f1b 326 IntKi[0]=package.parameter[8];
b0ssiz 4:9fbe67ca2f1b 327 IntKi[1]=package.parameter[9];
b0ssiz 4:9fbe67ca2f1b 328 FloatKi[0]=package.parameter[10];
b0ssiz 4:9fbe67ca2f1b 329 FloatKi[1]=package.parameter[11];
b0ssiz 4:9fbe67ca2f1b 330 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd);
b0ssiz 4:9fbe67ca2f1b 331 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 332 *Kd=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 333 }
b0ssiz 0:fc963e08d580 334 return status;
b0ssiz 0:fc963e08d580 335 }
b0ssiz 0:fc963e08d580 336
b0ssiz 0:fc963e08d580 337
b0ssiz 5:6f30b4ea4020 338 uint8_t Bear_Communicate::getLowMotorKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 5:6f30b4ea4020 339 {
b0ssiz 5:6f30b4ea4020 340 uint8_t IntKp[2],FloatKp[2];
b0ssiz 5:6f30b4ea4020 341 uint8_t IntKi[2],FloatKi[2];
b0ssiz 5:6f30b4ea4020 342 uint8_t IntKd[2],FloatKd[2];
b0ssiz 5:6f30b4ea4020 343 float int_buffer,float_buffer;
b0ssiz 5:6f30b4ea4020 344
b0ssiz 5:6f30b4ea4020 345 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 346
b0ssiz 5:6f30b4ea4020 347 package.robotId = id;
b0ssiz 7:ce4234c56410 348 package.length = 3;
b0ssiz 5:6f30b4ea4020 349 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 350 package.parameter[0]=PID_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 351
b0ssiz 5:6f30b4ea4020 352 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 353 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 354 com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 355
b0ssiz 5:6f30b4ea4020 356 rs485_dirc=0;
b0ssiz 5:6f30b4ea4020 357 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 358 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 359 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 5:6f30b4ea4020 360 IntKp[0]=package.parameter[0];
b0ssiz 5:6f30b4ea4020 361 IntKp[1]=package.parameter[1];
b0ssiz 5:6f30b4ea4020 362 FloatKp[0]=package.parameter[2];
b0ssiz 5:6f30b4ea4020 363 FloatKp[1]=package.parameter[3];
b0ssiz 5:6f30b4ea4020 364 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp);
b0ssiz 5:6f30b4ea4020 365 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 366 *Kp=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 367
b0ssiz 5:6f30b4ea4020 368 IntKi[0]=package.parameter[4];
b0ssiz 5:6f30b4ea4020 369 IntKi[1]=package.parameter[5];
b0ssiz 5:6f30b4ea4020 370 FloatKi[0]=package.parameter[6];
b0ssiz 5:6f30b4ea4020 371 FloatKi[1]=package.parameter[7];
b0ssiz 5:6f30b4ea4020 372 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi);
b0ssiz 5:6f30b4ea4020 373 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 374 *Ki=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 375
b0ssiz 5:6f30b4ea4020 376 IntKi[0]=package.parameter[8];
b0ssiz 5:6f30b4ea4020 377 IntKi[1]=package.parameter[9];
b0ssiz 5:6f30b4ea4020 378 FloatKi[0]=package.parameter[10];
b0ssiz 5:6f30b4ea4020 379 FloatKi[1]=package.parameter[11];
b0ssiz 5:6f30b4ea4020 380 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd);
b0ssiz 5:6f30b4ea4020 381 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 382 *Kd=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 383 }
b0ssiz 5:6f30b4ea4020 384 return status;
b0ssiz 5:6f30b4ea4020 385 }
b0ssiz 5:6f30b4ea4020 386
b0ssiz 5:6f30b4ea4020 387
b0ssiz 5:6f30b4ea4020 388
b0ssiz 0:fc963e08d580 389
b0ssiz 0:fc963e08d580 390 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
b0ssiz 0:fc963e08d580 391
b0ssiz 0:fc963e08d580 392
b0ssiz 8:e1f43b1df0b5 393 uint8_t Bear_Communicate::setUpMargin(uint8_t id,float margin)
b0ssiz 0:fc963e08d580 394 {
b0ssiz 7:ce4234c56410 395 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 7:ce4234c56410 396 FloatSep(margin,IntMargin,FloatMargin);
b0ssiz 7:ce4234c56410 397
b0ssiz 0:fc963e08d580 398 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 399
b0ssiz 0:fc963e08d580 400 package.robotId = id;
b0ssiz 7:ce4234c56410 401 package.length = 7;
b0ssiz 0:fc963e08d580 402 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:e1f43b1df0b5 403 package.parameter[0]=UP_MARGIN;
b0ssiz 8:e1f43b1df0b5 404 package.parameter[1]=IntMargin[0];
b0ssiz 8:e1f43b1df0b5 405 package.parameter[2]=IntMargin[1];
b0ssiz 8:e1f43b1df0b5 406 package.parameter[3]=FloatMargin[0];
b0ssiz 8:e1f43b1df0b5 407 package.parameter[4]=FloatMargin[1];
b0ssiz 8:e1f43b1df0b5 408
b0ssiz 8:e1f43b1df0b5 409 rs485_dirc=1;
b0ssiz 8:e1f43b1df0b5 410 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 411 return com->sendCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 412 }
b0ssiz 8:e1f43b1df0b5 413
b0ssiz 8:e1f43b1df0b5 414
b0ssiz 8:e1f43b1df0b5 415 uint8_t Bear_Communicate::setLowMargin(uint8_t id,float margin)
b0ssiz 8:e1f43b1df0b5 416 {
b0ssiz 8:e1f43b1df0b5 417 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 8:e1f43b1df0b5 418 FloatSep(margin,IntMargin,FloatMargin);
b0ssiz 8:e1f43b1df0b5 419
b0ssiz 8:e1f43b1df0b5 420 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:e1f43b1df0b5 421
b0ssiz 8:e1f43b1df0b5 422 package.robotId = id;
b0ssiz 8:e1f43b1df0b5 423 package.length = 7;
b0ssiz 8:e1f43b1df0b5 424 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:e1f43b1df0b5 425 package.parameter[0]=LOW_MARGIN;
b0ssiz 7:ce4234c56410 426 package.parameter[1]=IntMargin[0];
b0ssiz 7:ce4234c56410 427 package.parameter[2]=IntMargin[1];
b0ssiz 7:ce4234c56410 428 package.parameter[3]=FloatMargin[0];
b0ssiz 7:ce4234c56410 429 package.parameter[4]=FloatMargin[1];
b0ssiz 0:fc963e08d580 430
b0ssiz 0:fc963e08d580 431 rs485_dirc=1;
b0ssiz 0:fc963e08d580 432 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 433 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 434 }
b0ssiz 0:fc963e08d580 435
b0ssiz 0:fc963e08d580 436
b0ssiz 0:fc963e08d580 437
b0ssiz 8:e1f43b1df0b5 438 uint8_t Bear_Communicate::getUpMargin(uint8_t id,float *margin)
b0ssiz 0:fc963e08d580 439 {
b0ssiz 7:ce4234c56410 440 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 7:ce4234c56410 441 float int_buffer,float_buffer;
b0ssiz 0:fc963e08d580 442 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 443
b0ssiz 0:fc963e08d580 444 package.robotId = id;
b0ssiz 7:ce4234c56410 445 package.length = 3;
b0ssiz 0:fc963e08d580 446 package.instructionErrorId = READ_DATA;
b0ssiz 8:e1f43b1df0b5 447 package.parameter[0]=UP_MARGIN;
b0ssiz 8:e1f43b1df0b5 448
b0ssiz 8:e1f43b1df0b5 449
b0ssiz 8:e1f43b1df0b5 450 rs485_dirc=1;
b0ssiz 8:e1f43b1df0b5 451 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 452 com->sendCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 453
b0ssiz 8:e1f43b1df0b5 454 rs485_dirc=0;
b0ssiz 8:e1f43b1df0b5 455 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 456 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 457 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 8:e1f43b1df0b5 458 IntMargin[0]=package.parameter[0];
b0ssiz 8:e1f43b1df0b5 459 IntMargin[1]=package.parameter[1];
b0ssiz 8:e1f43b1df0b5 460 FloatMargin[0]=package.parameter[2];
b0ssiz 8:e1f43b1df0b5 461 FloatMargin[1]=package.parameter[3];
b0ssiz 8:e1f43b1df0b5 462 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMargin);
b0ssiz 8:e1f43b1df0b5 463 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMargin)/FLOAT_CONVERTER;
b0ssiz 8:e1f43b1df0b5 464 *margin=int_buffer+float_buffer;
b0ssiz 8:e1f43b1df0b5 465 }
b0ssiz 8:e1f43b1df0b5 466 return status;
b0ssiz 8:e1f43b1df0b5 467 }
b0ssiz 8:e1f43b1df0b5 468
b0ssiz 8:e1f43b1df0b5 469
b0ssiz 8:e1f43b1df0b5 470
b0ssiz 8:e1f43b1df0b5 471 uint8_t Bear_Communicate::getLowMargin(uint8_t id,float *margin)
b0ssiz 8:e1f43b1df0b5 472 {
b0ssiz 8:e1f43b1df0b5 473 uint8_t IntMargin[2],FloatMargin[2];
b0ssiz 8:e1f43b1df0b5 474 float int_buffer,float_buffer;
b0ssiz 8:e1f43b1df0b5 475 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:e1f43b1df0b5 476
b0ssiz 8:e1f43b1df0b5 477 package.robotId = id;
b0ssiz 8:e1f43b1df0b5 478 package.length = 3;
b0ssiz 8:e1f43b1df0b5 479 package.instructionErrorId = READ_DATA;
b0ssiz 8:e1f43b1df0b5 480 package.parameter[0]=LOW_MARGIN;
b0ssiz 0:fc963e08d580 481
b0ssiz 0:fc963e08d580 482
b0ssiz 0:fc963e08d580 483 rs485_dirc=1;
b0ssiz 0:fc963e08d580 484 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 485 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 486
b0ssiz 0:fc963e08d580 487 rs485_dirc=0;
b0ssiz 0:fc963e08d580 488 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 489 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 490 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 491 IntMargin[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 492 IntMargin[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 493 FloatMargin[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 494 FloatMargin[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 495 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMargin);
b0ssiz 7:ce4234c56410 496 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMargin)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 497 *margin=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 498 }
b0ssiz 0:fc963e08d580 499 return status;
b0ssiz 0:fc963e08d580 500 }
b0ssiz 0:fc963e08d580 501
b0ssiz 0:fc963e08d580 502
b0ssiz 0:fc963e08d580 503
b0ssiz 4:9fbe67ca2f1b 504 uint8_t Bear_Communicate::setHeight(uint8_t id,float height)
b0ssiz 0:fc963e08d580 505 {
b0ssiz 7:ce4234c56410 506 uint8_t IntHeight[2],FloatHeight[2];
b0ssiz 7:ce4234c56410 507 FloatSep(height,IntHeight,FloatHeight);
b0ssiz 7:ce4234c56410 508
b0ssiz 0:fc963e08d580 509 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 510
b0ssiz 0:fc963e08d580 511 package.robotId = id;
b0ssiz 7:ce4234c56410 512 package.length = 7;
b0ssiz 0:fc963e08d580 513 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 514 package.parameter[0]=HEIGHT;
b0ssiz 7:ce4234c56410 515 package.parameter[1]=IntHeight[0];
b0ssiz 7:ce4234c56410 516 package.parameter[2]=IntHeight[1];
b0ssiz 7:ce4234c56410 517 package.parameter[3]=FloatHeight[0];
b0ssiz 7:ce4234c56410 518 package.parameter[4]=FloatHeight[1];
b0ssiz 0:fc963e08d580 519
b0ssiz 0:fc963e08d580 520 rs485_dirc=1;
b0ssiz 0:fc963e08d580 521 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 522 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 523 }
b0ssiz 0:fc963e08d580 524
b0ssiz 0:fc963e08d580 525
b0ssiz 0:fc963e08d580 526
b0ssiz 4:9fbe67ca2f1b 527 uint8_t Bear_Communicate::getHeight(uint8_t id,float *height)
b0ssiz 0:fc963e08d580 528 {
b0ssiz 7:ce4234c56410 529 uint8_t IntHeight[2],FloatHeight[2];
b0ssiz 7:ce4234c56410 530 float int_buffer,float_buffer;
b0ssiz 7:ce4234c56410 531
b0ssiz 0:fc963e08d580 532 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 533
b0ssiz 0:fc963e08d580 534 package.robotId = id;
b0ssiz 7:ce4234c56410 535 package.length = 3;
b0ssiz 0:fc963e08d580 536 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 537 package.parameter[0]=HEIGHT;
b0ssiz 7:ce4234c56410 538
b0ssiz 0:fc963e08d580 539
b0ssiz 0:fc963e08d580 540 rs485_dirc=1;
b0ssiz 0:fc963e08d580 541 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 542 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 543
b0ssiz 0:fc963e08d580 544 rs485_dirc=0;
b0ssiz 0:fc963e08d580 545 wait_us(RS485_DELAY);
b0ssiz 7:ce4234c56410 546 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 547 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 548 IntHeight[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 549 IntHeight[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 550 FloatHeight[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 551 FloatHeight[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 552 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntHeight);
b0ssiz 7:ce4234c56410 553 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatHeight)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 554 *height=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 555 }
b0ssiz 0:fc963e08d580 556 return status;
b0ssiz 0:fc963e08d580 557 }
b0ssiz 0:fc963e08d580 558
b0ssiz 0:fc963e08d580 559
b0ssiz 0:fc963e08d580 560
b0ssiz 4:9fbe67ca2f1b 561 uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos)
b0ssiz 0:fc963e08d580 562 {
b0ssiz 7:ce4234c56410 563 uint8_t IntWheelPos[2],FloatWheelPos[2];
b0ssiz 7:ce4234c56410 564 FloatSep(WheelPos,IntWheelPos,FloatWheelPos);
b0ssiz 7:ce4234c56410 565
b0ssiz 0:fc963e08d580 566 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 567
b0ssiz 0:fc963e08d580 568 package.robotId = id;
b0ssiz 7:ce4234c56410 569 package.length = 7;
b0ssiz 0:fc963e08d580 570 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 571 package.parameter[0]=WHEELPOS;
b0ssiz 7:ce4234c56410 572 package.parameter[1]=IntWheelPos[0];
b0ssiz 7:ce4234c56410 573 package.parameter[2]=IntWheelPos[1];
b0ssiz 7:ce4234c56410 574 package.parameter[3]=FloatWheelPos[0];
b0ssiz 7:ce4234c56410 575 package.parameter[4]=FloatWheelPos[1];
b0ssiz 0:fc963e08d580 576
b0ssiz 0:fc963e08d580 577 rs485_dirc=1;
b0ssiz 0:fc963e08d580 578 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 579 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 580 }
b0ssiz 0:fc963e08d580 581
b0ssiz 0:fc963e08d580 582
b0ssiz 0:fc963e08d580 583
b0ssiz 4:9fbe67ca2f1b 584 uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos)
b0ssiz 0:fc963e08d580 585 {
b0ssiz 7:ce4234c56410 586 uint8_t IntWheelPos[2],FloatWheelPos[2];
b0ssiz 7:ce4234c56410 587 float int_buffer,float_buffer;
b0ssiz 7:ce4234c56410 588
b0ssiz 7:ce4234c56410 589 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 7:ce4234c56410 590
b0ssiz 7:ce4234c56410 591 package.robotId = id;
b0ssiz 7:ce4234c56410 592 package.length = 3;
b0ssiz 7:ce4234c56410 593 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 594 package.parameter[0]=WHEELPOS;
b0ssiz 7:ce4234c56410 595
b0ssiz 7:ce4234c56410 596
b0ssiz 7:ce4234c56410 597 rs485_dirc=1;
b0ssiz 7:ce4234c56410 598 wait_us(RS485_DELAY);
b0ssiz 7:ce4234c56410 599 com->sendCommunicatePacket(&package);
b0ssiz 7:ce4234c56410 600
b0ssiz 7:ce4234c56410 601 rs485_dirc=0;
b0ssiz 7:ce4234c56410 602 wait_us(RS485_DELAY);
b0ssiz 7:ce4234c56410 603 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 7:ce4234c56410 604 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 605 IntWheelPos[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 606 IntWheelPos[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 607 FloatWheelPos[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 608 FloatWheelPos[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 609 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntWheelPos);
b0ssiz 7:ce4234c56410 610 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatWheelPos)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 611 *WheelPos=int_buffer+float_buffer;
b0ssiz 7:ce4234c56410 612 }
b0ssiz 7:ce4234c56410 613 return status;
b0ssiz 7:ce4234c56410 614 }
b0ssiz 7:ce4234c56410 615
b0ssiz 7:ce4234c56410 616 uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float x_min,float y_max,float y_min,float z_max,float z_min)
b0ssiz 7:ce4234c56410 617 {
b0ssiz 7:ce4234c56410 618 uint8_t IntXmax[2],FloatXmax[2];
b0ssiz 7:ce4234c56410 619 uint8_t IntXmin[2],FloatXmin[2];
b0ssiz 7:ce4234c56410 620 uint8_t IntYmax[2],FloatYmax[2];
b0ssiz 7:ce4234c56410 621 uint8_t IntYmin[2],FloatYmin[2];
b0ssiz 7:ce4234c56410 622 uint8_t IntZmax[2],FloatZmax[2];
b0ssiz 7:ce4234c56410 623 uint8_t IntZmin[2],FloatZmin[2];
b0ssiz 7:ce4234c56410 624 FloatSep(x_max,IntXmax,FloatXmax);
b0ssiz 7:ce4234c56410 625 FloatSep(x_min,IntXmin,FloatXmin);
b0ssiz 7:ce4234c56410 626 FloatSep(y_max,IntYmax,FloatYmax);
b0ssiz 7:ce4234c56410 627 FloatSep(y_min,IntYmin,FloatYmin);
b0ssiz 7:ce4234c56410 628 FloatSep(z_max,IntZmax,FloatZmax);
b0ssiz 7:ce4234c56410 629 FloatSep(z_min,IntZmin,FloatZmin);
b0ssiz 7:ce4234c56410 630
b0ssiz 0:fc963e08d580 631 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 632
b0ssiz 0:fc963e08d580 633 package.robotId = id;
b0ssiz 7:ce4234c56410 634 package.length = 27;
b0ssiz 7:ce4234c56410 635 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 636 package.parameter[0]=MOTOR_UPPER_ANG;
b0ssiz 7:ce4234c56410 637 package.parameter[1]=IntXmax[0];
b0ssiz 7:ce4234c56410 638 package.parameter[2]=IntXmax[1];
b0ssiz 7:ce4234c56410 639 package.parameter[3]=FloatXmax[0];
b0ssiz 7:ce4234c56410 640 package.parameter[4]=FloatXmax[1];
b0ssiz 7:ce4234c56410 641 package.parameter[5]=IntXmin[0];
b0ssiz 7:ce4234c56410 642 package.parameter[6]=IntXmin[1];
b0ssiz 7:ce4234c56410 643 package.parameter[7]=FloatXmin[0];
b0ssiz 7:ce4234c56410 644 package.parameter[8]=FloatXmin[1];
b0ssiz 7:ce4234c56410 645 package.parameter[9]=IntYmax[0];
b0ssiz 7:ce4234c56410 646 package.parameter[10]=IntYmax[1];
b0ssiz 7:ce4234c56410 647 package.parameter[11]=FloatYmax[0];
b0ssiz 7:ce4234c56410 648 package.parameter[12]=FloatYmax[1];
b0ssiz 7:ce4234c56410 649 package.parameter[13]=IntYmin[0];
b0ssiz 7:ce4234c56410 650 package.parameter[14]=IntYmin[1];
b0ssiz 7:ce4234c56410 651 package.parameter[15]=FloatYmin[0];
b0ssiz 7:ce4234c56410 652 package.parameter[16]=FloatYmin[1];
b0ssiz 7:ce4234c56410 653 package.parameter[17]=IntZmax[0];
b0ssiz 7:ce4234c56410 654 package.parameter[18]=IntZmax[1];
b0ssiz 7:ce4234c56410 655 package.parameter[19]=FloatZmax[0];
b0ssiz 7:ce4234c56410 656 package.parameter[20]=FloatZmax[1];
b0ssiz 7:ce4234c56410 657 package.parameter[21]=IntZmin[0];
b0ssiz 7:ce4234c56410 658 package.parameter[22]=IntZmin[1];
b0ssiz 7:ce4234c56410 659 package.parameter[23]=FloatZmin[0];
b0ssiz 7:ce4234c56410 660 package.parameter[24]=FloatZmin[1];
b0ssiz 7:ce4234c56410 661
b0ssiz 7:ce4234c56410 662 rs485_dirc=1;
b0ssiz 7:ce4234c56410 663 wait_us(RS485_DELAY);
b0ssiz 7:ce4234c56410 664 return com->sendCommunicatePacket(&package);
b0ssiz 7:ce4234c56410 665
b0ssiz 7:ce4234c56410 666 }
b0ssiz 7:ce4234c56410 667
b0ssiz 7:ce4234c56410 668 uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *x_min,float *y_max,float *y_min,float *z_max,float *z_min)
b0ssiz 7:ce4234c56410 669 {
b0ssiz 7:ce4234c56410 670 uint8_t IntXmax[2],FloatXmax[2];
b0ssiz 7:ce4234c56410 671 uint8_t IntXmin[2],FloatXmin[2];
b0ssiz 7:ce4234c56410 672 uint8_t IntYmax[2],FloatYmax[2];
b0ssiz 7:ce4234c56410 673 uint8_t IntYmin[2],FloatYmin[2];
b0ssiz 7:ce4234c56410 674 uint8_t IntZmax[2],FloatZmax[2];
b0ssiz 7:ce4234c56410 675 uint8_t IntZmin[2],FloatZmin[2];
b0ssiz 7:ce4234c56410 676 float int_buffer,float_buffer;
b0ssiz 7:ce4234c56410 677
b0ssiz 7:ce4234c56410 678 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 7:ce4234c56410 679
b0ssiz 7:ce4234c56410 680 package.robotId = id;
b0ssiz 7:ce4234c56410 681 package.length = 3;
b0ssiz 0:fc963e08d580 682 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 683 package.parameter[0]=MAG_DATA;
b0ssiz 0:fc963e08d580 684
b0ssiz 0:fc963e08d580 685 rs485_dirc=1;
b0ssiz 0:fc963e08d580 686 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 687 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 688
b0ssiz 0:fc963e08d580 689 rs485_dirc=0;
b0ssiz 0:fc963e08d580 690 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 691 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 692 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 693 IntXmax[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 694 IntXmax[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 695 FloatXmax[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 696 FloatXmax[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 697 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmax);
b0ssiz 7:ce4234c56410 698 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmax)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 699 *x_max=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 700
b0ssiz 7:ce4234c56410 701 IntXmin[0]=package.parameter[4];
b0ssiz 7:ce4234c56410 702 IntXmin[1]=package.parameter[5];
b0ssiz 7:ce4234c56410 703 FloatXmin[0]=package.parameter[6];
b0ssiz 7:ce4234c56410 704 FloatXmin[1]=package.parameter[7];
b0ssiz 7:ce4234c56410 705 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntXmin);
b0ssiz 7:ce4234c56410 706 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatXmin)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 707 *x_min=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 708
b0ssiz 7:ce4234c56410 709 IntYmax[0]=package.parameter[8];
b0ssiz 7:ce4234c56410 710 IntYmax[1]=package.parameter[9];
b0ssiz 7:ce4234c56410 711 FloatYmax[0]=package.parameter[10];
b0ssiz 7:ce4234c56410 712 FloatYmax[1]=package.parameter[11];
b0ssiz 7:ce4234c56410 713 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmax);
b0ssiz 7:ce4234c56410 714 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmax)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 715 *y_max=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 716
b0ssiz 7:ce4234c56410 717 IntYmin[0]=package.parameter[12];
b0ssiz 7:ce4234c56410 718 IntYmin[1]=package.parameter[13];
b0ssiz 7:ce4234c56410 719 FloatYmin[0]=package.parameter[14];
b0ssiz 7:ce4234c56410 720 FloatYmin[1]=package.parameter[15];
b0ssiz 7:ce4234c56410 721 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntYmin);
b0ssiz 7:ce4234c56410 722 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatYmin)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 723 *y_min=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 724
b0ssiz 7:ce4234c56410 725 IntZmax[0]=package.parameter[16];
b0ssiz 7:ce4234c56410 726 IntZmax[1]=package.parameter[17];
b0ssiz 7:ce4234c56410 727 FloatZmax[0]=package.parameter[18];
b0ssiz 7:ce4234c56410 728 FloatZmax[1]=package.parameter[19];
b0ssiz 7:ce4234c56410 729 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmax);
b0ssiz 7:ce4234c56410 730 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmax)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 731 *z_max=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 732
b0ssiz 7:ce4234c56410 733 IntZmin[0]=package.parameter[20];
b0ssiz 7:ce4234c56410 734 IntZmin[1]=package.parameter[21];
b0ssiz 7:ce4234c56410 735 FloatZmin[0]=package.parameter[22];
b0ssiz 7:ce4234c56410 736 FloatZmin[1]=package.parameter[23];
b0ssiz 7:ce4234c56410 737 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntZmin);
b0ssiz 7:ce4234c56410 738 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatZmin)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 739 *z_min=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 740
b0ssiz 0:fc963e08d580 741 }
b0ssiz 0:fc963e08d580 742 return status;
b0ssiz 0:fc963e08d580 743 }
b0ssiz 0:fc963e08d580 744
b0ssiz 0:fc963e08d580 745
b0ssiz 0:fc963e08d580 746
b0ssiz 4:9fbe67ca2f1b 747 uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 0:fc963e08d580 748 {
b0ssiz 7:ce4234c56410 749 uint8_t IntOffset_Y[2],FloatOffset_Y[2];
b0ssiz 7:ce4234c56410 750 uint8_t IntOffset_Z[2],FloatOffset_Z[2];
b0ssiz 7:ce4234c56410 751 FloatSep(offset_y,IntOffset_Y,FloatOffset_Y);
b0ssiz 7:ce4234c56410 752 FloatSep(offset_z,IntOffset_Z,FloatOffset_Z);
b0ssiz 7:ce4234c56410 753
b0ssiz 0:fc963e08d580 754 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 755
b0ssiz 0:fc963e08d580 756 package.robotId = id;
b0ssiz 7:ce4234c56410 757 package.length = 11;
b0ssiz 0:fc963e08d580 758 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 759 package.parameter[0]=OFFSET;
b0ssiz 7:ce4234c56410 760 package.parameter[1]=IntOffset_Y[0];
b0ssiz 7:ce4234c56410 761 package.parameter[2]=IntOffset_Y[1];
b0ssiz 7:ce4234c56410 762 package.parameter[3]=FloatOffset_Y[0];
b0ssiz 7:ce4234c56410 763 package.parameter[4]=FloatOffset_Y[1];
b0ssiz 7:ce4234c56410 764 package.parameter[5]=IntOffset_Z[0];
b0ssiz 7:ce4234c56410 765 package.parameter[6]=IntOffset_Z[1];
b0ssiz 7:ce4234c56410 766 package.parameter[7]=FloatOffset_Z[0];
b0ssiz 7:ce4234c56410 767 package.parameter[8]=FloatOffset_Z[1];
b0ssiz 0:fc963e08d580 768
b0ssiz 0:fc963e08d580 769 rs485_dirc=1;
b0ssiz 0:fc963e08d580 770 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 771 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 772 }
b0ssiz 0:fc963e08d580 773
b0ssiz 0:fc963e08d580 774
b0ssiz 0:fc963e08d580 775
b0ssiz 4:9fbe67ca2f1b 776 uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z)
b0ssiz 0:fc963e08d580 777 {
b0ssiz 7:ce4234c56410 778 uint8_t IntOffset_Y[2],FloatOffset_Y[2];
b0ssiz 7:ce4234c56410 779 uint8_t IntOffset_Z[2],FloatOffset_Z[2];
b0ssiz 7:ce4234c56410 780 float int_buffer,float_buffer;
b0ssiz 7:ce4234c56410 781
b0ssiz 0:fc963e08d580 782 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 783
b0ssiz 0:fc963e08d580 784 package.robotId = id;
b0ssiz 7:ce4234c56410 785 package.length = 3;
b0ssiz 0:fc963e08d580 786 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 787 package.parameter[0] = OFFSET;
b0ssiz 0:fc963e08d580 788
b0ssiz 0:fc963e08d580 789 rs485_dirc=1;
b0ssiz 0:fc963e08d580 790 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 791 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 792
b0ssiz 0:fc963e08d580 793 rs485_dirc=0;
b0ssiz 0:fc963e08d580 794 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 795 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 796
b0ssiz 7:ce4234c56410 797 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 798 IntOffset_Y[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 799 IntOffset_Y[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 800 FloatOffset_Y[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 801 FloatOffset_Y[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 802 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Y);
b0ssiz 7:ce4234c56410 803 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Y)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 804 *offset_y=int_buffer+float_buffer;
b0ssiz 7:ce4234c56410 805
b0ssiz 7:ce4234c56410 806 IntOffset_Z[0]=package.parameter[4];
b0ssiz 7:ce4234c56410 807 IntOffset_Z[1]=package.parameter[5];
b0ssiz 7:ce4234c56410 808 FloatOffset_Z[0]=package.parameter[6];
b0ssiz 7:ce4234c56410 809 FloatOffset_Z[1]=package.parameter[7];
b0ssiz 7:ce4234c56410 810 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntOffset_Z);
b0ssiz 7:ce4234c56410 811 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatOffset_Z)/FLOAT_CONVERTER;
b0ssiz 9:c22410169d9f 812 *offset_z=int_buffer+float_buffer;
b0ssiz 7:ce4234c56410 813
b0ssiz 0:fc963e08d580 814 }
b0ssiz 0:fc963e08d580 815 return status;
b0ssiz 0:fc963e08d580 816 }
b0ssiz 0:fc963e08d580 817
b0ssiz 0:fc963e08d580 818
b0ssiz 0:fc963e08d580 819
b0ssiz 4:9fbe67ca2f1b 820 uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length)
b0ssiz 0:fc963e08d580 821 {
b0ssiz 7:ce4234c56410 822 uint8_t IntBodyLength[2],FloatBodyLength[2];
b0ssiz 7:ce4234c56410 823 FloatSep(body_length,IntBodyLength,FloatBodyLength);
b0ssiz 7:ce4234c56410 824
b0ssiz 0:fc963e08d580 825 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 826
b0ssiz 0:fc963e08d580 827 package.robotId = id;
b0ssiz 7:ce4234c56410 828 package.length = 7;
b0ssiz 0:fc963e08d580 829 package.instructionErrorId = WRITE_DATA;
b0ssiz 7:ce4234c56410 830 package.parameter[0]=BODY_LENGTH;
b0ssiz 7:ce4234c56410 831 package.parameter[1]=IntBodyLength[0];
b0ssiz 7:ce4234c56410 832 package.parameter[2]=IntBodyLength[1];
b0ssiz 7:ce4234c56410 833 package.parameter[3]=FloatBodyLength[0];
b0ssiz 7:ce4234c56410 834 package.parameter[4]=FloatBodyLength[1];
b0ssiz 0:fc963e08d580 835
b0ssiz 0:fc963e08d580 836 rs485_dirc=1;
b0ssiz 0:fc963e08d580 837 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 838 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 839 }
b0ssiz 0:fc963e08d580 840
b0ssiz 0:fc963e08d580 841
b0ssiz 0:fc963e08d580 842
b0ssiz 0:fc963e08d580 843
b0ssiz 4:9fbe67ca2f1b 844 uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length)
b0ssiz 0:fc963e08d580 845 {
b0ssiz 7:ce4234c56410 846 uint8_t IntBodyLength[2],FloatBodyLength[2];
b0ssiz 7:ce4234c56410 847 float int_buffer,float_buffer;
b0ssiz 0:fc963e08d580 848 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 849
b0ssiz 0:fc963e08d580 850 package.robotId = id;
b0ssiz 7:ce4234c56410 851 package.length = 3;
b0ssiz 0:fc963e08d580 852 package.instructionErrorId = READ_DATA;
b0ssiz 7:ce4234c56410 853 package.parameter[0]=BODY_LENGTH;
b0ssiz 7:ce4234c56410 854
b0ssiz 0:fc963e08d580 855
b0ssiz 0:fc963e08d580 856 rs485_dirc=1;
b0ssiz 0:fc963e08d580 857 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 858 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 859
b0ssiz 0:fc963e08d580 860 rs485_dirc=0;
b0ssiz 0:fc963e08d580 861 wait_us(RS485_DELAY);
b0ssiz 7:ce4234c56410 862 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 863 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 864 IntBodyLength[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 865 IntBodyLength[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 866 FloatBodyLength[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 867 FloatBodyLength[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 868 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntBodyLength);
b0ssiz 7:ce4234c56410 869 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatBodyLength)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 870 *body_length=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 871 }
b0ssiz 0:fc963e08d580 872 return status;
b0ssiz 0:fc963e08d580 873 }
b0ssiz 0:fc963e08d580 874
b0ssiz 8:e1f43b1df0b5 875
b0ssiz 8:e1f43b1df0b5 876 uint8_t Bear_Communicate::setUpAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 0:fc963e08d580 877 {
b0ssiz 7:ce4234c56410 878 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 7:ce4234c56410 879 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 7:ce4234c56410 880 FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
b0ssiz 7:ce4234c56410 881 FloatSep(min_angle,IntMinAngle,FloatMinAngle);
b0ssiz 7:ce4234c56410 882
b0ssiz 0:fc963e08d580 883 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 884
b0ssiz 0:fc963e08d580 885 package.robotId = id;
b0ssiz 7:ce4234c56410 886 package.length = 11;
b0ssiz 0:fc963e08d580 887 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:e1f43b1df0b5 888 package.parameter[0]=ANGLE_RANGE_UP;
b0ssiz 8:e1f43b1df0b5 889 package.parameter[1]=IntMaxAngle[0];
b0ssiz 8:e1f43b1df0b5 890 package.parameter[2]=IntMaxAngle[1];
b0ssiz 8:e1f43b1df0b5 891 package.parameter[3]=FloatMaxAngle[0];
b0ssiz 8:e1f43b1df0b5 892 package.parameter[4]=FloatMaxAngle[1];
b0ssiz 8:e1f43b1df0b5 893 package.parameter[5]=IntMinAngle[0];
b0ssiz 8:e1f43b1df0b5 894 package.parameter[6]=IntMinAngle[1];
b0ssiz 8:e1f43b1df0b5 895 package.parameter[7]=FloatMinAngle[0];
b0ssiz 8:e1f43b1df0b5 896 package.parameter[8]=FloatMinAngle[1];
b0ssiz 8:e1f43b1df0b5 897
b0ssiz 8:e1f43b1df0b5 898 rs485_dirc=1;
b0ssiz 8:e1f43b1df0b5 899 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 900 return com->sendCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 901 }
b0ssiz 8:e1f43b1df0b5 902
b0ssiz 8:e1f43b1df0b5 903
b0ssiz 8:e1f43b1df0b5 904 uint8_t Bear_Communicate::setLowAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 8:e1f43b1df0b5 905 {
b0ssiz 8:e1f43b1df0b5 906 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 8:e1f43b1df0b5 907 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 8:e1f43b1df0b5 908 FloatSep(max_angle,IntMaxAngle,FloatMaxAngle);
b0ssiz 8:e1f43b1df0b5 909 FloatSep(min_angle,IntMinAngle,FloatMinAngle);
b0ssiz 8:e1f43b1df0b5 910
b0ssiz 8:e1f43b1df0b5 911 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:e1f43b1df0b5 912
b0ssiz 8:e1f43b1df0b5 913 package.robotId = id;
b0ssiz 8:e1f43b1df0b5 914 package.length = 11;
b0ssiz 8:e1f43b1df0b5 915 package.instructionErrorId = WRITE_DATA;
b0ssiz 8:e1f43b1df0b5 916 package.parameter[0]=ANGLE_RANGE_LOW;
b0ssiz 7:ce4234c56410 917 package.parameter[1]=IntMaxAngle[0];
b0ssiz 7:ce4234c56410 918 package.parameter[2]=IntMaxAngle[1];
b0ssiz 7:ce4234c56410 919 package.parameter[3]=FloatMaxAngle[0];
b0ssiz 7:ce4234c56410 920 package.parameter[4]=FloatMaxAngle[1];
b0ssiz 7:ce4234c56410 921 package.parameter[5]=IntMinAngle[0];
b0ssiz 7:ce4234c56410 922 package.parameter[6]=IntMinAngle[1];
b0ssiz 7:ce4234c56410 923 package.parameter[7]=FloatMinAngle[0];
b0ssiz 7:ce4234c56410 924 package.parameter[8]=FloatMinAngle[1];
b0ssiz 0:fc963e08d580 925
b0ssiz 0:fc963e08d580 926 rs485_dirc=1;
b0ssiz 0:fc963e08d580 927 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 928 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 929 }
b0ssiz 0:fc963e08d580 930
b0ssiz 0:fc963e08d580 931
b0ssiz 0:fc963e08d580 932
b0ssiz 0:fc963e08d580 933
b0ssiz 8:e1f43b1df0b5 934 uint8_t Bear_Communicate::getUpAngleRange(uint8_t id,float *max_angle,float *min_angle)
b0ssiz 0:fc963e08d580 935 {
b0ssiz 7:ce4234c56410 936 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 7:ce4234c56410 937 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 7:ce4234c56410 938 float int_buffer,float_buffer;
b0ssiz 0:fc963e08d580 939
b0ssiz 0:fc963e08d580 940 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 941
b0ssiz 0:fc963e08d580 942 package.robotId = id;
b0ssiz 7:ce4234c56410 943 package.length = 3;
b0ssiz 0:fc963e08d580 944 package.instructionErrorId = READ_DATA;
b0ssiz 8:e1f43b1df0b5 945 package.parameter[0] = ANGLE_RANGE_UP;
b0ssiz 0:fc963e08d580 946
b0ssiz 0:fc963e08d580 947 rs485_dirc=1;
b0ssiz 0:fc963e08d580 948 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 949 com->sendCommunicatePacket(&package);
b0ssiz 4:9fbe67ca2f1b 950
b0ssiz 0:fc963e08d580 951 rs485_dirc=0;
b0ssiz 0:fc963e08d580 952 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 953 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 954
b0ssiz 7:ce4234c56410 955 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 7:ce4234c56410 956 IntMaxAngle[0]=package.parameter[0];
b0ssiz 7:ce4234c56410 957 IntMaxAngle[1]=package.parameter[1];
b0ssiz 7:ce4234c56410 958 FloatMaxAngle[0]=package.parameter[2];
b0ssiz 7:ce4234c56410 959 FloatMaxAngle[1]=package.parameter[3];
b0ssiz 7:ce4234c56410 960 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMaxAngle);
b0ssiz 7:ce4234c56410 961 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMaxAngle)/FLOAT_CONVERTER;
b0ssiz 7:ce4234c56410 962 *max_angle=int_buffer+float_buffer;
b0ssiz 7:ce4234c56410 963
b0ssiz 7:ce4234c56410 964 IntMinAngle[0]=package.parameter[4];
b0ssiz 7:ce4234c56410 965 IntMinAngle[1]=package.parameter[5];
b0ssiz 7:ce4234c56410 966 FloatMinAngle[0]=package.parameter[6];
b0ssiz 7:ce4234c56410 967 FloatMinAngle[1]=package.parameter[7];
b0ssiz 7:ce4234c56410 968 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMinAngle);
b0ssiz 7:ce4234c56410 969 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMinAngle)/FLOAT_CONVERTER;
b0ssiz 9:c22410169d9f 970 *min_angle=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 971
b0ssiz 0:fc963e08d580 972 }
b0ssiz 0:fc963e08d580 973 return status;
b0ssiz 8:e1f43b1df0b5 974 }
b0ssiz 8:e1f43b1df0b5 975
b0ssiz 8:e1f43b1df0b5 976
b0ssiz 8:e1f43b1df0b5 977
b0ssiz 8:e1f43b1df0b5 978 uint8_t Bear_Communicate::getLowAngleRange(uint8_t id,float *max_angle,float *min_angle)
b0ssiz 8:e1f43b1df0b5 979 {
b0ssiz 8:e1f43b1df0b5 980 uint8_t IntMaxAngle[2],FloatMaxAngle[2];
b0ssiz 8:e1f43b1df0b5 981 uint8_t IntMinAngle[2],FloatMinAngle[2];
b0ssiz 8:e1f43b1df0b5 982 float int_buffer,float_buffer;
b0ssiz 8:e1f43b1df0b5 983
b0ssiz 8:e1f43b1df0b5 984 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 8:e1f43b1df0b5 985
b0ssiz 8:e1f43b1df0b5 986 package.robotId = id;
b0ssiz 8:e1f43b1df0b5 987 package.length = 3;
b0ssiz 8:e1f43b1df0b5 988 package.instructionErrorId = READ_DATA;
b0ssiz 8:e1f43b1df0b5 989 package.parameter[0] = ANGLE_RANGE_LOW;
b0ssiz 8:e1f43b1df0b5 990
b0ssiz 8:e1f43b1df0b5 991 rs485_dirc=1;
b0ssiz 8:e1f43b1df0b5 992 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 993 com->sendCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 994
b0ssiz 8:e1f43b1df0b5 995 rs485_dirc=0;
b0ssiz 8:e1f43b1df0b5 996 wait_us(RS485_DELAY);
b0ssiz 8:e1f43b1df0b5 997 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 8:e1f43b1df0b5 998
b0ssiz 8:e1f43b1df0b5 999 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 8:e1f43b1df0b5 1000 IntMaxAngle[0]=package.parameter[0];
b0ssiz 8:e1f43b1df0b5 1001 IntMaxAngle[1]=package.parameter[1];
b0ssiz 8:e1f43b1df0b5 1002 FloatMaxAngle[0]=package.parameter[2];
b0ssiz 8:e1f43b1df0b5 1003 FloatMaxAngle[1]=package.parameter[3];
b0ssiz 8:e1f43b1df0b5 1004 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMaxAngle);
b0ssiz 8:e1f43b1df0b5 1005 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMaxAngle)/FLOAT_CONVERTER;
b0ssiz 8:e1f43b1df0b5 1006 *max_angle=int_buffer+float_buffer;
b0ssiz 8:e1f43b1df0b5 1007
b0ssiz 8:e1f43b1df0b5 1008 IntMinAngle[0]=package.parameter[4];
b0ssiz 8:e1f43b1df0b5 1009 IntMinAngle[1]=package.parameter[5];
b0ssiz 8:e1f43b1df0b5 1010 FloatMinAngle[0]=package.parameter[6];
b0ssiz 8:e1f43b1df0b5 1011 FloatMinAngle[1]=package.parameter[7];
b0ssiz 8:e1f43b1df0b5 1012 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntMinAngle);
b0ssiz 8:e1f43b1df0b5 1013 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatMinAngle)/FLOAT_CONVERTER;
b0ssiz 9:c22410169d9f 1014 *min_angle=int_buffer+float_buffer;
b0ssiz 8:e1f43b1df0b5 1015
b0ssiz 8:e1f43b1df0b5 1016 }
b0ssiz 8:e1f43b1df0b5 1017 return status;
b0ssiz 8:e1f43b1df0b5 1018 }
b0ssiz 8:e1f43b1df0b5 1019