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@5:6f30b4ea4020, 2016-01-15 (annotated)

Committer:
b0ssiz
Date:
Fri Jan 15 17:02:08 2016 +0000
Revision:
5:6f30b4ea4020
Parent:
4:9fbe67ca2f1b
Child:
6:07749b50d600
Add function set Kp,Ki and Kd for upper and lower joint

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 0:fc963e08d580 39 package.parameter[0]=SET_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 0:fc963e08d580 61 package.parameter[0]=SET_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 0:fc963e08d580 87 package.length = 4;
b0ssiz 0:fc963e08d580 88 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 89 package.parameter[0] = GET_MOTOR_UPPER_ANG;
b0ssiz 0:fc963e08d580 90 package.parameter[1] = 0x02;
b0ssiz 0:fc963e08d580 91
b0ssiz 0:fc963e08d580 92 rs485_dirc=1;
b0ssiz 0:fc963e08d580 93 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 94 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 95
b0ssiz 0:fc963e08d580 96 rs485_dirc=0;
b0ssiz 0:fc963e08d580 97 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 98 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 99
b0ssiz 0:fc963e08d580 100 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 4:9fbe67ca2f1b 101 IntUpAngle[0]=package.parameter[1];
b0ssiz 4:9fbe67ca2f1b 102 IntUpAngle[1]=package.parameter[2];
b0ssiz 4:9fbe67ca2f1b 103 FloatUpAngle[0]=package.parameter[3];
b0ssiz 4:9fbe67ca2f1b 104 FloatUpAngle[1]=package.parameter[4];
b0ssiz 4:9fbe67ca2f1b 105 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
b0ssiz 4:9fbe67ca2f1b 106 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 107 *up_angle=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 108
b0ssiz 4:9fbe67ca2f1b 109 IntLowAngle[0]=package.parameter[5];
b0ssiz 4:9fbe67ca2f1b 110 IntLowAngle[1]=package.parameter[6];
b0ssiz 4:9fbe67ca2f1b 111 FloatLowAngle[0]=package.parameter[7];
b0ssiz 4:9fbe67ca2f1b 112 FloatLowAngle[1]=package.parameter[8];
b0ssiz 4:9fbe67ca2f1b 113 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
b0ssiz 4:9fbe67ca2f1b 114 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 115 *low_angle=int_buffer+float_buffer;;
b0ssiz 0:fc963e08d580 116
b0ssiz 0:fc963e08d580 117 }
b0ssiz 0:fc963e08d580 118 return status;
b0ssiz 0:fc963e08d580 119 }
b0ssiz 0:fc963e08d580 120
b0ssiz 0:fc963e08d580 121
b0ssiz 0:fc963e08d580 122
b0ssiz 5:6f30b4ea4020 123 uint8_t Bear_Communicate::setUpMotorKp(uint8_t id,float Kp)
b0ssiz 0:fc963e08d580 124 {
b0ssiz 4:9fbe67ca2f1b 125 uint8_t IntKp[2],FloatKp[2];
b0ssiz 4:9fbe67ca2f1b 126 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 4:9fbe67ca2f1b 127
b0ssiz 0:fc963e08d580 128 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 129
b0ssiz 0:fc963e08d580 130 package.robotId = id;
b0ssiz 4:9fbe67ca2f1b 131 package.length = 7;
b0ssiz 0:fc963e08d580 132 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 133 package.parameter[0]=SET_KP_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 134 package.parameter[1]=IntKp[0];
b0ssiz 4:9fbe67ca2f1b 135 package.parameter[2]=IntKp[1];
b0ssiz 4:9fbe67ca2f1b 136 package.parameter[3]=FloatKp[0];
b0ssiz 4:9fbe67ca2f1b 137 package.parameter[4]=FloatKp[1];
b0ssiz 0:fc963e08d580 138
b0ssiz 0:fc963e08d580 139
b0ssiz 0:fc963e08d580 140 rs485_dirc=1;
b0ssiz 0:fc963e08d580 141 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 142 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 143 }
b0ssiz 0:fc963e08d580 144
b0ssiz 0:fc963e08d580 145
b0ssiz 0:fc963e08d580 146
b0ssiz 5:6f30b4ea4020 147
b0ssiz 5:6f30b4ea4020 148 uint8_t Bear_Communicate::setLowMotorKp(uint8_t id,float Kp)
b0ssiz 5:6f30b4ea4020 149 {
b0ssiz 5:6f30b4ea4020 150 uint8_t IntKp[2],FloatKp[2];
b0ssiz 5:6f30b4ea4020 151 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 5:6f30b4ea4020 152
b0ssiz 5:6f30b4ea4020 153 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 154
b0ssiz 5:6f30b4ea4020 155 package.robotId = id;
b0ssiz 5:6f30b4ea4020 156 package.length = 7;
b0ssiz 5:6f30b4ea4020 157 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 158 package.parameter[0]=SET_KP_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 159 package.parameter[1]=IntKp[0];
b0ssiz 5:6f30b4ea4020 160 package.parameter[2]=IntKp[1];
b0ssiz 5:6f30b4ea4020 161 package.parameter[3]=FloatKp[0];
b0ssiz 5:6f30b4ea4020 162 package.parameter[4]=FloatKp[1];
b0ssiz 5:6f30b4ea4020 163
b0ssiz 5:6f30b4ea4020 164
b0ssiz 5:6f30b4ea4020 165 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 166 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 167 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 168 }
b0ssiz 5:6f30b4ea4020 169
b0ssiz 5:6f30b4ea4020 170
b0ssiz 5:6f30b4ea4020 171
b0ssiz 5:6f30b4ea4020 172
b0ssiz 5:6f30b4ea4020 173 uint8_t Bear_Communicate::setUpMotorKi(uint8_t id,float Ki)
b0ssiz 0:fc963e08d580 174 {
b0ssiz 4:9fbe67ca2f1b 175 uint8_t IntKi[2],FloatKi[2];
b0ssiz 4:9fbe67ca2f1b 176 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 4:9fbe67ca2f1b 177
b0ssiz 0:fc963e08d580 178 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 179
b0ssiz 0:fc963e08d580 180 package.robotId = id;
b0ssiz 0:fc963e08d580 181 package.length = 4;
b0ssiz 0:fc963e08d580 182 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 183 package.parameter[0]=SET_KI_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 184 package.parameter[1]=IntKi[0];
b0ssiz 4:9fbe67ca2f1b 185 package.parameter[2]=IntKi[1];
b0ssiz 4:9fbe67ca2f1b 186 package.parameter[3]=FloatKi[0];
b0ssiz 4:9fbe67ca2f1b 187 package.parameter[4]=FloatKi[1];
b0ssiz 4:9fbe67ca2f1b 188
b0ssiz 4:9fbe67ca2f1b 189
b0ssiz 4:9fbe67ca2f1b 190
b0ssiz 4:9fbe67ca2f1b 191 rs485_dirc=1;
b0ssiz 4:9fbe67ca2f1b 192 wait_us(RS485_DELAY);
b0ssiz 4:9fbe67ca2f1b 193 return com->sendCommunicatePacket(&package);
b0ssiz 4:9fbe67ca2f1b 194
b0ssiz 4:9fbe67ca2f1b 195 }
b0ssiz 4:9fbe67ca2f1b 196
b0ssiz 4:9fbe67ca2f1b 197
b0ssiz 4:9fbe67ca2f1b 198
b0ssiz 5:6f30b4ea4020 199
b0ssiz 5:6f30b4ea4020 200 uint8_t Bear_Communicate::setLowMotorKi(uint8_t id,float Ki)
b0ssiz 5:6f30b4ea4020 201 {
b0ssiz 5:6f30b4ea4020 202 uint8_t IntKi[2],FloatKi[2];
b0ssiz 5:6f30b4ea4020 203 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 5:6f30b4ea4020 204
b0ssiz 5:6f30b4ea4020 205 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 206
b0ssiz 5:6f30b4ea4020 207 package.robotId = id;
b0ssiz 5:6f30b4ea4020 208 package.length = 4;
b0ssiz 5:6f30b4ea4020 209 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 210 package.parameter[0]=SET_KI_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 211 package.parameter[1]=IntKi[0];
b0ssiz 5:6f30b4ea4020 212 package.parameter[2]=IntKi[1];
b0ssiz 5:6f30b4ea4020 213 package.parameter[3]=FloatKi[0];
b0ssiz 5:6f30b4ea4020 214 package.parameter[4]=FloatKi[1];
b0ssiz 5:6f30b4ea4020 215
b0ssiz 5:6f30b4ea4020 216
b0ssiz 5:6f30b4ea4020 217
b0ssiz 5:6f30b4ea4020 218 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 219 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 220 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 221
b0ssiz 5:6f30b4ea4020 222 }
b0ssiz 5:6f30b4ea4020 223
b0ssiz 5:6f30b4ea4020 224
b0ssiz 5:6f30b4ea4020 225
b0ssiz 5:6f30b4ea4020 226 uint8_t Bear_Communicate::setUpMotorKd(uint8_t id,float Kd)
b0ssiz 4:9fbe67ca2f1b 227 {
b0ssiz 4:9fbe67ca2f1b 228 uint8_t IntKd[2],FloatKd[2];
b0ssiz 4:9fbe67ca2f1b 229 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 4:9fbe67ca2f1b 230
b0ssiz 4:9fbe67ca2f1b 231 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 4:9fbe67ca2f1b 232
b0ssiz 4:9fbe67ca2f1b 233 package.robotId = id;
b0ssiz 4:9fbe67ca2f1b 234 package.length = 7;
b0ssiz 4:9fbe67ca2f1b 235 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 236 package.parameter[0]=SET_KD_UPPER_MOTOR;
b0ssiz 4:9fbe67ca2f1b 237 package.parameter[1]=IntKd[0];
b0ssiz 4:9fbe67ca2f1b 238 package.parameter[2]=IntKd[1];
b0ssiz 4:9fbe67ca2f1b 239 package.parameter[3]=FloatKd[0];
b0ssiz 4:9fbe67ca2f1b 240 package.parameter[4]=FloatKd[1];
b0ssiz 0:fc963e08d580 241
b0ssiz 0:fc963e08d580 242
b0ssiz 0:fc963e08d580 243 rs485_dirc=1;
b0ssiz 0:fc963e08d580 244 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 245 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 246 }
b0ssiz 0:fc963e08d580 247
b0ssiz 0:fc963e08d580 248
b0ssiz 0:fc963e08d580 249
b0ssiz 5:6f30b4ea4020 250 uint8_t Bear_Communicate::setLowMotorKd(uint8_t id,float Kd)
b0ssiz 5:6f30b4ea4020 251 {
b0ssiz 5:6f30b4ea4020 252 uint8_t IntKd[2],FloatKd[2];
b0ssiz 5:6f30b4ea4020 253 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 5:6f30b4ea4020 254
b0ssiz 5:6f30b4ea4020 255 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 256
b0ssiz 5:6f30b4ea4020 257 package.robotId = id;
b0ssiz 5:6f30b4ea4020 258 package.length = 7;
b0ssiz 5:6f30b4ea4020 259 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:6f30b4ea4020 260 package.parameter[0]=SET_KD_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 261 package.parameter[1]=IntKd[0];
b0ssiz 5:6f30b4ea4020 262 package.parameter[2]=IntKd[1];
b0ssiz 5:6f30b4ea4020 263 package.parameter[3]=FloatKd[0];
b0ssiz 5:6f30b4ea4020 264 package.parameter[4]=FloatKd[1];
b0ssiz 5:6f30b4ea4020 265
b0ssiz 5:6f30b4ea4020 266
b0ssiz 5:6f30b4ea4020 267 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 268 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 269 return com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 270 }
b0ssiz 5:6f30b4ea4020 271
b0ssiz 5:6f30b4ea4020 272
b0ssiz 5:6f30b4ea4020 273
b0ssiz 5:6f30b4ea4020 274 uint8_t Bear_Communicate::getUpMotorKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 0:fc963e08d580 275 {
b0ssiz 4:9fbe67ca2f1b 276 uint8_t IntKp[2],FloatKp[2];
b0ssiz 4:9fbe67ca2f1b 277 uint8_t IntKi[2],FloatKi[2];
b0ssiz 4:9fbe67ca2f1b 278 uint8_t IntKd[2],FloatKd[2];
b0ssiz 4:9fbe67ca2f1b 279 float int_buffer,float_buffer;
b0ssiz 4:9fbe67ca2f1b 280
b0ssiz 0:fc963e08d580 281 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 282
b0ssiz 0:fc963e08d580 283 package.robotId = id;
b0ssiz 0:fc963e08d580 284 package.length = 4;
b0ssiz 0:fc963e08d580 285 package.instructionErrorId = READ_DATA;
b0ssiz 5:6f30b4ea4020 286 package.parameter[0]=GET_KP_UPPER_MOTOR;
b0ssiz 0:fc963e08d580 287 package.parameter[1]=0x03;
b0ssiz 0:fc963e08d580 288
b0ssiz 0:fc963e08d580 289
b0ssiz 0:fc963e08d580 290 rs485_dirc=1;
b0ssiz 0:fc963e08d580 291 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 292 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 293
b0ssiz 0:fc963e08d580 294 rs485_dirc=0;
b0ssiz 0:fc963e08d580 295 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 296 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 297 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 4:9fbe67ca2f1b 298 IntKp[0]=package.parameter[0];
b0ssiz 4:9fbe67ca2f1b 299 IntKp[1]=package.parameter[1];
b0ssiz 4:9fbe67ca2f1b 300 FloatKp[0]=package.parameter[2];
b0ssiz 4:9fbe67ca2f1b 301 FloatKp[1]=package.parameter[3];
b0ssiz 4:9fbe67ca2f1b 302 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp);
b0ssiz 4:9fbe67ca2f1b 303 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 304 *Kp=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 305
b0ssiz 4:9fbe67ca2f1b 306 IntKi[0]=package.parameter[4];
b0ssiz 4:9fbe67ca2f1b 307 IntKi[1]=package.parameter[5];
b0ssiz 4:9fbe67ca2f1b 308 FloatKi[0]=package.parameter[6];
b0ssiz 4:9fbe67ca2f1b 309 FloatKi[1]=package.parameter[7];
b0ssiz 4:9fbe67ca2f1b 310 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi);
b0ssiz 4:9fbe67ca2f1b 311 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 312 *Ki=int_buffer+float_buffer;
b0ssiz 4:9fbe67ca2f1b 313
b0ssiz 4:9fbe67ca2f1b 314 IntKi[0]=package.parameter[8];
b0ssiz 4:9fbe67ca2f1b 315 IntKi[1]=package.parameter[9];
b0ssiz 4:9fbe67ca2f1b 316 FloatKi[0]=package.parameter[10];
b0ssiz 4:9fbe67ca2f1b 317 FloatKi[1]=package.parameter[11];
b0ssiz 4:9fbe67ca2f1b 318 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd);
b0ssiz 4:9fbe67ca2f1b 319 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER;
b0ssiz 4:9fbe67ca2f1b 320 *Kd=int_buffer+float_buffer;
b0ssiz 0:fc963e08d580 321 }
b0ssiz 0:fc963e08d580 322 return status;
b0ssiz 0:fc963e08d580 323 }
b0ssiz 0:fc963e08d580 324
b0ssiz 0:fc963e08d580 325
b0ssiz 5:6f30b4ea4020 326 uint8_t Bear_Communicate::getLowMotorKpKiKd(uint8_t id,float *Kp,float *Ki,float *Kd)
b0ssiz 5:6f30b4ea4020 327 {
b0ssiz 5:6f30b4ea4020 328 uint8_t IntKp[2],FloatKp[2];
b0ssiz 5:6f30b4ea4020 329 uint8_t IntKi[2],FloatKi[2];
b0ssiz 5:6f30b4ea4020 330 uint8_t IntKd[2],FloatKd[2];
b0ssiz 5:6f30b4ea4020 331 float int_buffer,float_buffer;
b0ssiz 5:6f30b4ea4020 332
b0ssiz 5:6f30b4ea4020 333 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:6f30b4ea4020 334
b0ssiz 5:6f30b4ea4020 335 package.robotId = id;
b0ssiz 5:6f30b4ea4020 336 package.length = 4;
b0ssiz 5:6f30b4ea4020 337 package.instructionErrorId = READ_DATA;
b0ssiz 5:6f30b4ea4020 338 package.parameter[0]=GET_KP_LOWER_MOTOR;
b0ssiz 5:6f30b4ea4020 339 package.parameter[1]=0x03;
b0ssiz 5:6f30b4ea4020 340
b0ssiz 5:6f30b4ea4020 341
b0ssiz 5:6f30b4ea4020 342 rs485_dirc=1;
b0ssiz 5:6f30b4ea4020 343 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 344 com->sendCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 345
b0ssiz 5:6f30b4ea4020 346 rs485_dirc=0;
b0ssiz 5:6f30b4ea4020 347 wait_us(RS485_DELAY);
b0ssiz 5:6f30b4ea4020 348 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 5:6f30b4ea4020 349 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 5:6f30b4ea4020 350 IntKp[0]=package.parameter[0];
b0ssiz 5:6f30b4ea4020 351 IntKp[1]=package.parameter[1];
b0ssiz 5:6f30b4ea4020 352 FloatKp[0]=package.parameter[2];
b0ssiz 5:6f30b4ea4020 353 FloatKp[1]=package.parameter[3];
b0ssiz 5:6f30b4ea4020 354 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKp);
b0ssiz 5:6f30b4ea4020 355 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKp)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 356 *Kp=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 357
b0ssiz 5:6f30b4ea4020 358 IntKi[0]=package.parameter[4];
b0ssiz 5:6f30b4ea4020 359 IntKi[1]=package.parameter[5];
b0ssiz 5:6f30b4ea4020 360 FloatKi[0]=package.parameter[6];
b0ssiz 5:6f30b4ea4020 361 FloatKi[1]=package.parameter[7];
b0ssiz 5:6f30b4ea4020 362 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKi);
b0ssiz 5:6f30b4ea4020 363 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKi)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 364 *Ki=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 365
b0ssiz 5:6f30b4ea4020 366 IntKi[0]=package.parameter[8];
b0ssiz 5:6f30b4ea4020 367 IntKi[1]=package.parameter[9];
b0ssiz 5:6f30b4ea4020 368 FloatKi[0]=package.parameter[10];
b0ssiz 5:6f30b4ea4020 369 FloatKi[1]=package.parameter[11];
b0ssiz 5:6f30b4ea4020 370 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntKd);
b0ssiz 5:6f30b4ea4020 371 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatKd)/FLOAT_CONVERTER;
b0ssiz 5:6f30b4ea4020 372 *Kd=int_buffer+float_buffer;
b0ssiz 5:6f30b4ea4020 373 }
b0ssiz 5:6f30b4ea4020 374 return status;
b0ssiz 5:6f30b4ea4020 375 }
b0ssiz 5:6f30b4ea4020 376
b0ssiz 5:6f30b4ea4020 377
b0ssiz 5:6f30b4ea4020 378
b0ssiz 0:fc963e08d580 379
b0ssiz 0:fc963e08d580 380 ///////////////////////////////////////////// Save Data to EEPROM \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
b0ssiz 0:fc963e08d580 381
b0ssiz 0:fc963e08d580 382
b0ssiz 4:9fbe67ca2f1b 383 uint8_t Bear_Communicate::setMargin(uint8_t id,float margin)
b0ssiz 0:fc963e08d580 384 {
b0ssiz 0:fc963e08d580 385 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 386
b0ssiz 0:fc963e08d580 387 package.robotId = id;
b0ssiz 0:fc963e08d580 388 package.length = 4;
b0ssiz 0:fc963e08d580 389 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 390 package.parameter[0]=SET_MARGIN;
b0ssiz 0:fc963e08d580 391 package.parameter[1]=margin;
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 0:fc963e08d580 402 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 403
b0ssiz 0:fc963e08d580 404 package.robotId = id;
b0ssiz 0:fc963e08d580 405 package.length = 4;
b0ssiz 0:fc963e08d580 406 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 407 package.parameter[0]=GET_MARGIN;
b0ssiz 0:fc963e08d580 408 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 409
b0ssiz 0:fc963e08d580 410
b0ssiz 0:fc963e08d580 411 rs485_dirc=1;
b0ssiz 0:fc963e08d580 412 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 413 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 414
b0ssiz 0:fc963e08d580 415 rs485_dirc=0;
b0ssiz 0:fc963e08d580 416 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 417 uint8_t status=com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 418 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 419
b0ssiz 0:fc963e08d580 420 *margin=package.parameter[0];
b0ssiz 0:fc963e08d580 421 }
b0ssiz 0:fc963e08d580 422 return status;
b0ssiz 0:fc963e08d580 423 }
b0ssiz 0:fc963e08d580 424
b0ssiz 0:fc963e08d580 425
b0ssiz 0:fc963e08d580 426
b0ssiz 4:9fbe67ca2f1b 427 uint8_t Bear_Communicate::setHeight(uint8_t id,float height)
b0ssiz 0:fc963e08d580 428 {
b0ssiz 0:fc963e08d580 429 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 430
b0ssiz 0:fc963e08d580 431 package.robotId = id;
b0ssiz 0:fc963e08d580 432 package.length = 4;
b0ssiz 0:fc963e08d580 433 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 434 package.parameter[0]=SET_HEIGHT;
b0ssiz 0:fc963e08d580 435 package.parameter[1]=height;
b0ssiz 0:fc963e08d580 436
b0ssiz 0:fc963e08d580 437 rs485_dirc=1;
b0ssiz 0:fc963e08d580 438 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 439 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 440 }
b0ssiz 0:fc963e08d580 441
b0ssiz 0:fc963e08d580 442
b0ssiz 0:fc963e08d580 443
b0ssiz 4:9fbe67ca2f1b 444 uint8_t Bear_Communicate::getHeight(uint8_t id,float *height)
b0ssiz 0:fc963e08d580 445 {
b0ssiz 0:fc963e08d580 446 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 447
b0ssiz 0:fc963e08d580 448 package.robotId = id;
b0ssiz 0:fc963e08d580 449 package.length = 4;
b0ssiz 0:fc963e08d580 450 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 451 package.parameter[0]=GET_HEIGHT;
b0ssiz 0:fc963e08d580 452 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 453
b0ssiz 0:fc963e08d580 454 rs485_dirc=1;
b0ssiz 0:fc963e08d580 455 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 456 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 457
b0ssiz 0:fc963e08d580 458 rs485_dirc=0;
b0ssiz 0:fc963e08d580 459 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 460 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 461 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 462
b0ssiz 0:fc963e08d580 463 *height = package.parameter[0];
b0ssiz 0:fc963e08d580 464
b0ssiz 0:fc963e08d580 465 }
b0ssiz 0:fc963e08d580 466 return status;
b0ssiz 0:fc963e08d580 467 }
b0ssiz 0:fc963e08d580 468
b0ssiz 0:fc963e08d580 469
b0ssiz 0:fc963e08d580 470
b0ssiz 4:9fbe67ca2f1b 471 uint8_t Bear_Communicate::setWheelPos(uint8_t id,float WheelPos)
b0ssiz 0:fc963e08d580 472 {
b0ssiz 0:fc963e08d580 473 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 474
b0ssiz 0:fc963e08d580 475 package.robotId = id;
b0ssiz 0:fc963e08d580 476 package.length = 4;
b0ssiz 0:fc963e08d580 477 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 478 package.parameter[0]=SET_WHEELPOS;
b0ssiz 0:fc963e08d580 479 package.parameter[1]=WheelPos;
b0ssiz 0:fc963e08d580 480
b0ssiz 0:fc963e08d580 481 rs485_dirc=1;
b0ssiz 0:fc963e08d580 482 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 483 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 484 }
b0ssiz 0:fc963e08d580 485
b0ssiz 0:fc963e08d580 486
b0ssiz 0:fc963e08d580 487
b0ssiz 4:9fbe67ca2f1b 488 uint8_t Bear_Communicate::getWheelPos(uint8_t id,float *WheelPos)
b0ssiz 0:fc963e08d580 489 {
b0ssiz 0:fc963e08d580 490 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 491
b0ssiz 0:fc963e08d580 492 package.robotId = id;
b0ssiz 0:fc963e08d580 493 package.length = 4;
b0ssiz 0:fc963e08d580 494 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 495 package.parameter[0]=GET_WHEELPOS;
b0ssiz 0:fc963e08d580 496 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 497
b0ssiz 0:fc963e08d580 498 rs485_dirc=1;
b0ssiz 0:fc963e08d580 499 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 500 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 501
b0ssiz 0:fc963e08d580 502 rs485_dirc=0;
b0ssiz 0:fc963e08d580 503 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 504 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 505 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 506
b0ssiz 0:fc963e08d580 507 *WheelPos = package.parameter[0];
b0ssiz 0:fc963e08d580 508 }
b0ssiz 0:fc963e08d580 509 return status;
b0ssiz 0:fc963e08d580 510 }
b0ssiz 0:fc963e08d580 511
b0ssiz 4:9fbe67ca2f1b 512 uint8_t Bear_Communicate::setMagData(uint8_t id,float x_max,float y_max,float z_max,float x_min,float y_min,float z_min)
b0ssiz 0:fc963e08d580 513 {
b0ssiz 0:fc963e08d580 514 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 515
b0ssiz 0:fc963e08d580 516 package.robotId = id;
b0ssiz 0:fc963e08d580 517 package.length = 9;
b0ssiz 0:fc963e08d580 518 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 519 package.parameter[0]=SET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 520 package.parameter[1]=x_max;
b0ssiz 0:fc963e08d580 521 package.parameter[2]=y_max;
b0ssiz 0:fc963e08d580 522 package.parameter[3]=z_max;
b0ssiz 0:fc963e08d580 523 package.parameter[4]=x_min;
b0ssiz 0:fc963e08d580 524 package.parameter[5]=y_min;
b0ssiz 0:fc963e08d580 525 package.parameter[6]=z_min;
b0ssiz 0:fc963e08d580 526
b0ssiz 0:fc963e08d580 527 rs485_dirc=1;
b0ssiz 0:fc963e08d580 528 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 529 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 530 }
b0ssiz 0:fc963e08d580 531
b0ssiz 4:9fbe67ca2f1b 532 uint8_t Bear_Communicate::getMagData(uint8_t id,float *x_max,float *y_max,float *z_max,float *x_min,float *y_min,float *z_min)
b0ssiz 0:fc963e08d580 533 {
b0ssiz 0:fc963e08d580 534 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 535
b0ssiz 0:fc963e08d580 536 package.robotId = id;
b0ssiz 0:fc963e08d580 537 package.length = 4;
b0ssiz 0:fc963e08d580 538 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 539 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 540 package.parameter[1]=0x06;
b0ssiz 0:fc963e08d580 541
b0ssiz 0:fc963e08d580 542 rs485_dirc=1;
b0ssiz 0:fc963e08d580 543 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 544 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 545
b0ssiz 0:fc963e08d580 546 rs485_dirc=0;
b0ssiz 0:fc963e08d580 547 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 548 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 549 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 550
b0ssiz 0:fc963e08d580 551 *x_max=package.parameter[0];
b0ssiz 0:fc963e08d580 552 *y_max=package.parameter[1];
b0ssiz 0:fc963e08d580 553 *z_max=package.parameter[2];
b0ssiz 0:fc963e08d580 554 *x_min=package.parameter[3];
b0ssiz 0:fc963e08d580 555 *y_min=package.parameter[4];
b0ssiz 0:fc963e08d580 556 *z_min=package.parameter[5];
b0ssiz 0:fc963e08d580 557
b0ssiz 0:fc963e08d580 558 }
b0ssiz 0:fc963e08d580 559 return status;
b0ssiz 0:fc963e08d580 560 }
b0ssiz 0:fc963e08d580 561
b0ssiz 0:fc963e08d580 562
b0ssiz 0:fc963e08d580 563
b0ssiz 4:9fbe67ca2f1b 564 uint8_t Bear_Communicate::setOffset(uint8_t id,float offset_y,float offset_z)
b0ssiz 0:fc963e08d580 565 {
b0ssiz 0:fc963e08d580 566 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 567
b0ssiz 0:fc963e08d580 568 package.robotId = id;
b0ssiz 0:fc963e08d580 569 package.length = 5;
b0ssiz 0:fc963e08d580 570 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 571 package.parameter[0]=SET_OFFSET_Y;
b0ssiz 0:fc963e08d580 572 package.parameter[1]=offset_y;
b0ssiz 0:fc963e08d580 573 package.parameter[2]=offset_z;
b0ssiz 0:fc963e08d580 574
b0ssiz 0:fc963e08d580 575 rs485_dirc=1;
b0ssiz 0:fc963e08d580 576 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 577 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 578 }
b0ssiz 0:fc963e08d580 579
b0ssiz 0:fc963e08d580 580
b0ssiz 0:fc963e08d580 581
b0ssiz 4:9fbe67ca2f1b 582 uint8_t Bear_Communicate::getOffset(uint8_t id,float *offset_y,float *offset_z)
b0ssiz 0:fc963e08d580 583 {
b0ssiz 0:fc963e08d580 584 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 585
b0ssiz 0:fc963e08d580 586 package.robotId = id;
b0ssiz 0:fc963e08d580 587 package.length = 4;
b0ssiz 0:fc963e08d580 588 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 589 package.parameter[0]=GET_MAG_X_MAX;
b0ssiz 0:fc963e08d580 590 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 591
b0ssiz 0:fc963e08d580 592 rs485_dirc=1;
b0ssiz 0:fc963e08d580 593 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 594 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 595
b0ssiz 0:fc963e08d580 596 rs485_dirc=0;
b0ssiz 0:fc963e08d580 597 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 598 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 599 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 600
b0ssiz 0:fc963e08d580 601 *offset_y=package.parameter[0];
b0ssiz 0:fc963e08d580 602 *offset_z=package.parameter[1];
b0ssiz 0:fc963e08d580 603 }
b0ssiz 0:fc963e08d580 604 return status;
b0ssiz 0:fc963e08d580 605 }
b0ssiz 0:fc963e08d580 606
b0ssiz 0:fc963e08d580 607
b0ssiz 0:fc963e08d580 608
b0ssiz 4:9fbe67ca2f1b 609 uint8_t Bear_Communicate::setBodyLength(uint8_t id,float body_length)
b0ssiz 0:fc963e08d580 610 {
b0ssiz 0:fc963e08d580 611 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 612
b0ssiz 0:fc963e08d580 613 package.robotId = id;
b0ssiz 0:fc963e08d580 614 package.length = 4;
b0ssiz 0:fc963e08d580 615 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 616 package.parameter[0]=SET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 617 package.parameter[1]=body_length;
b0ssiz 0:fc963e08d580 618
b0ssiz 0:fc963e08d580 619 rs485_dirc=1;
b0ssiz 0:fc963e08d580 620 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 621 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 622 }
b0ssiz 0:fc963e08d580 623
b0ssiz 0:fc963e08d580 624
b0ssiz 0:fc963e08d580 625
b0ssiz 0:fc963e08d580 626
b0ssiz 4:9fbe67ca2f1b 627 uint8_t Bear_Communicate::getBodyLength(uint8_t id,float *body_length)
b0ssiz 0:fc963e08d580 628 {
b0ssiz 0:fc963e08d580 629 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 630
b0ssiz 0:fc963e08d580 631 package.robotId = id;
b0ssiz 0:fc963e08d580 632 package.length = 4;
b0ssiz 0:fc963e08d580 633 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 634 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 635 package.parameter[1]=0x01;
b0ssiz 0:fc963e08d580 636
b0ssiz 0:fc963e08d580 637 rs485_dirc=1;
b0ssiz 0:fc963e08d580 638 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 639 com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 640
b0ssiz 0:fc963e08d580 641 rs485_dirc=0;
b0ssiz 0:fc963e08d580 642 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 643 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 644 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 645
b0ssiz 0:fc963e08d580 646 *body_length=package.parameter[0];
b0ssiz 0:fc963e08d580 647 }
b0ssiz 0:fc963e08d580 648 return status;
b0ssiz 0:fc963e08d580 649 }
b0ssiz 0:fc963e08d580 650
b0ssiz 4:9fbe67ca2f1b 651 uint8_t Bear_Communicate::setAngleRange(uint8_t id,float max_angle,float min_angle)
b0ssiz 0:fc963e08d580 652 {
b0ssiz 0:fc963e08d580 653 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 654
b0ssiz 0:fc963e08d580 655 package.robotId = id;
b0ssiz 0:fc963e08d580 656 package.length = 5;
b0ssiz 0:fc963e08d580 657 package.instructionErrorId = WRITE_DATA;
b0ssiz 0:fc963e08d580 658 package.parameter[0]=SET_MAX_ANGLE;
b0ssiz 0:fc963e08d580 659 package.parameter[1]=max_angle;
b0ssiz 0:fc963e08d580 660 package.parameter[2]=min_angle;
b0ssiz 0:fc963e08d580 661
b0ssiz 0:fc963e08d580 662 rs485_dirc=1;
b0ssiz 0:fc963e08d580 663 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 664 return com->sendCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 665 }
b0ssiz 0:fc963e08d580 666
b0ssiz 0:fc963e08d580 667
b0ssiz 0:fc963e08d580 668
b0ssiz 0:fc963e08d580 669
b0ssiz 4:9fbe67ca2f1b 670 uint8_t Bear_Communicate::getAngleRange(uint8_t id,float *max_angle,float *min_angle)
b0ssiz 0:fc963e08d580 671 {
b0ssiz 0:fc963e08d580 672
b0ssiz 0:fc963e08d580 673 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 0:fc963e08d580 674
b0ssiz 0:fc963e08d580 675 package.robotId = id;
b0ssiz 0:fc963e08d580 676 package.length = 4;
b0ssiz 0:fc963e08d580 677 package.instructionErrorId = READ_DATA;
b0ssiz 0:fc963e08d580 678 package.parameter[0]=GET_BODY_LENGTH;
b0ssiz 0:fc963e08d580 679 package.parameter[1]=0x02;
b0ssiz 0:fc963e08d580 680
b0ssiz 0:fc963e08d580 681 rs485_dirc=1;
b0ssiz 0:fc963e08d580 682 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 683 com->sendCommunicatePacket(&package);
b0ssiz 4:9fbe67ca2f1b 684
b0ssiz 0:fc963e08d580 685 rs485_dirc=0;
b0ssiz 0:fc963e08d580 686 wait_us(RS485_DELAY);
b0ssiz 0:fc963e08d580 687 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 0:fc963e08d580 688 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 0:fc963e08d580 689
b0ssiz 0:fc963e08d580 690 *max_angle=package.parameter[0];
b0ssiz 0:fc963e08d580 691 *min_angle=package.parameter[1];
b0ssiz 0:fc963e08d580 692
b0ssiz 0:fc963e08d580 693 }
b0ssiz 0:fc963e08d580 694 return status;
b0ssiz 0:fc963e08d580 695 }