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@7:ce4234c56410, 2016-01-21 (annotated)

Committer:
b0ssiz
Date:
Thu Jan 21 16:03:47 2016 +0000
Revision:
7:ce4234c56410
Parent:
6:07749b50d600
Child:
8:e1f43b1df0b5
Change structure of function in Bear_Protocol.cpp

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