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@13:45286c47ca0d, 2016-01-28 (annotated)

Committer:
soulx
Date:
Thu Jan 28 15:23:15 2016 +0000
Revision:
13:45286c47ca0d
Parent:
12:6296cb35f853 
boz edit

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