BE@R lab / BEAR_Reciever

Communication for reciever

Dependencies:   Fork_Boss_Communication_Robot

Dependents:   BEAR_Motion BEAR_Motion_Editing4Betago

Fork of BEAR_Protocol by BE@R lab

Receiver.cpp@6:f6529ad2a70d, 2016-01-16 (annotated)

Committer:
b0ssiz
Date:
Sat Jan 16 03:20:27 2016 +0000
Revision:
6:f6529ad2a70d
Parent:
5:baf5576a14bd
Child:
7:a0ea8c127611
Update function in "Receiver.h" and "Receiver.cpp"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
b0ssiz 5:baf5576a14bd 1 #include "mbed.h"
b0ssiz 5:baf5576a14bd 2 #include "Receiver.h"
b0ssiz 5:baf5576a14bd 3 #define FLOAT_CONVERTER 10000
b0ssiz 5:baf5576a14bd 4
b0ssiz 5:baf5576a14bd 5 DigitalOut rs485_dirc(RS485_DIRC);
b0ssiz 5:baf5576a14bd 6
b0ssiz 5:baf5576a14bd 7 Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate)
b0ssiz 5:baf5576a14bd 8 {
b0ssiz 5:baf5576a14bd 9 com = new COMMUNICATION(tx,rx,baudrate);
b0ssiz 5:baf5576a14bd 10 }
b0ssiz 5:baf5576a14bd 11
b0ssiz 6:f6529ad2a70d 12 uint8_t Bear_Receiver::ReceiveCommand(uint8_t* ID,uint8_t* Instruction,uint8_t* data_array)
b0ssiz 5:baf5576a14bd 13 {
b0ssiz 5:baf5576a14bd 14 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:baf5576a14bd 15
b0ssiz 5:baf5576a14bd 16 rs485_dirc=0;
b0ssiz 5:baf5576a14bd 17 wait_us(RS485_DELAY);
b0ssiz 5:baf5576a14bd 18 uint8_t status = com->receiveCommunicatePacket(&package);
b0ssiz 5:baf5576a14bd 19
b0ssiz 5:baf5576a14bd 20 if(status == ANDANTE_ERRBIT_NONE) {
b0ssiz 5:baf5576a14bd 21
b0ssiz 5:baf5576a14bd 22 *ID = package.robotId;
b0ssiz 5:baf5576a14bd 23 *Instruction = package.instructionErrorId;
b0ssiz 6:f6529ad2a70d 24 data_array = package.parameter;
b0ssiz 5:baf5576a14bd 25
b0ssiz 5:baf5576a14bd 26 }
b0ssiz 5:baf5576a14bd 27 return status;
b0ssiz 5:baf5576a14bd 28 }
b0ssiz 5:baf5576a14bd 29
b0ssiz 5:baf5576a14bd 30
b0ssiz 5:baf5576a14bd 31
b0ssiz 5:baf5576a14bd 32 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
b0ssiz 5:baf5576a14bd 33 {
b0ssiz 5:baf5576a14bd 34 float float_buffer;
b0ssiz 5:baf5576a14bd 35 float int_buffer;
b0ssiz 5:baf5576a14bd 36 int16_t integer;
b0ssiz 5:baf5576a14bd 37 int16_t floating_point;
b0ssiz 5:baf5576a14bd 38
b0ssiz 5:baf5576a14bd 39 float_buffer=modf(input_float,&int_buffer);
b0ssiz 5:baf5576a14bd 40 float_buffer*=FLOAT_CONVERTER;
b0ssiz 5:baf5576a14bd 41 integer=(int16_t)int_buffer;
b0ssiz 5:baf5576a14bd 42 floating_point=(int16_t)float_buffer;
b0ssiz 5:baf5576a14bd 43 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
b0ssiz 5:baf5576a14bd 44 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
b0ssiz 5:baf5576a14bd 45 }
b0ssiz 5:baf5576a14bd 46
b0ssiz 5:baf5576a14bd 47
b0ssiz 5:baf5576a14bd 48
b0ssiz 5:baf5576a14bd 49
b0ssiz 5:baf5576a14bd 50 uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
b0ssiz 5:baf5576a14bd 51 {
b0ssiz 5:baf5576a14bd 52 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 5:baf5576a14bd 53 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 5:baf5576a14bd 54 FloatSep(up_angle,IntUpAngle,FloatUpAngle);
b0ssiz 5:baf5576a14bd 55 FloatSep(low_angle,IntLowAngle,FloatLowAngle);
b0ssiz 5:baf5576a14bd 56
b0ssiz 5:baf5576a14bd 57 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 5:baf5576a14bd 58
b0ssiz 5:baf5576a14bd 59 package.robotId = id;
b0ssiz 5:baf5576a14bd 60 package.length = 10;
b0ssiz 5:baf5576a14bd 61 package.instructionErrorId = WRITE_DATA;
b0ssiz 5:baf5576a14bd 62 package.parameter[0]=IntUpAngle[0];
b0ssiz 5:baf5576a14bd 63 package.parameter[1]=IntUpAngle[1];
b0ssiz 5:baf5576a14bd 64 package.parameter[2]=FloatUpAngle[0];
b0ssiz 5:baf5576a14bd 65 package.parameter[3]=FloatUpAngle[1];
b0ssiz 5:baf5576a14bd 66 package.parameter[4]=IntLowAngle[0];
b0ssiz 5:baf5576a14bd 67 package.parameter[5]=IntLowAngle[1];
b0ssiz 5:baf5576a14bd 68 package.parameter[6]=FloatLowAngle[0];
b0ssiz 5:baf5576a14bd 69 package.parameter[7]=FloatLowAngle[1];
b0ssiz 5:baf5576a14bd 70
b0ssiz 5:baf5576a14bd 71 rs485_dirc=1;
b0ssiz 5:baf5576a14bd 72 wait_us(RS485_DELAY);
b0ssiz 5:baf5576a14bd 73 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 74
b0ssiz 6:f6529ad2a70d 75
b0ssiz 6:f6529ad2a70d 76 }
b0ssiz 6:f6529ad2a70d 77
b0ssiz 6:f6529ad2a70d 78
b0ssiz 6:f6529ad2a70d 79
b0ssiz 6:f6529ad2a70d 80 uint8_t Bear_Receiver::sendUpMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 81 {
b0ssiz 6:f6529ad2a70d 82 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 83 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 84 uint8_t IntKd[2],FloatKd[2];
b0ssiz 6:f6529ad2a70d 85
b0ssiz 6:f6529ad2a70d 86 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 87 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 88 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 5:baf5576a14bd 89
b0ssiz 5:baf5576a14bd 90
b0ssiz 6:f6529ad2a70d 91 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 92
b0ssiz 6:f6529ad2a70d 93 package.robotId = id;
b0ssiz 6:f6529ad2a70d 94 package.length = 14;
b0ssiz 6:f6529ad2a70d 95 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 96 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 97 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 98 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 99 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 100 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 101 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 102 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 103 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 104 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 105 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 106 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 107 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 108
b0ssiz 6:f6529ad2a70d 109 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 110 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 111 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 112 }
b0ssiz 6:f6529ad2a70d 113
b0ssiz 6:f6529ad2a70d 114
b0ssiz 6:f6529ad2a70d 115
b0ssiz 6:f6529ad2a70d 116
b0ssiz 6:f6529ad2a70d 117 uint8_t Bear_Receiver::sendLowMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
b0ssiz 6:f6529ad2a70d 118 {
b0ssiz 6:f6529ad2a70d 119 uint8_t IntKp[2],FloatKp[2];
b0ssiz 6:f6529ad2a70d 120 uint8_t IntKi[2],FloatKi[2];
b0ssiz 6:f6529ad2a70d 121 uint8_t IntKd[2],FloatKd[2];
b0ssiz 6:f6529ad2a70d 122
b0ssiz 6:f6529ad2a70d 123 FloatSep(Kp,IntKp,FloatKp);
b0ssiz 6:f6529ad2a70d 124 FloatSep(Ki,IntKi,FloatKi);
b0ssiz 6:f6529ad2a70d 125 FloatSep(Kd,IntKd,FloatKd);
b0ssiz 6:f6529ad2a70d 126
b0ssiz 6:f6529ad2a70d 127
b0ssiz 6:f6529ad2a70d 128 ANDANTE_PROTOCOL_PACKET package;
b0ssiz 6:f6529ad2a70d 129
b0ssiz 6:f6529ad2a70d 130 package.robotId = id;
b0ssiz 6:f6529ad2a70d 131 package.length = 14;
b0ssiz 6:f6529ad2a70d 132 package.instructionErrorId = WRITE_DATA;
b0ssiz 6:f6529ad2a70d 133 package.parameter[0]=IntKp[0];
b0ssiz 6:f6529ad2a70d 134 package.parameter[1]=IntKp[1];
b0ssiz 6:f6529ad2a70d 135 package.parameter[2]=FloatKp[0];
b0ssiz 6:f6529ad2a70d 136 package.parameter[3]=FloatKp[1];
b0ssiz 6:f6529ad2a70d 137 package.parameter[4]=IntKi[0];
b0ssiz 6:f6529ad2a70d 138 package.parameter[5]=IntKi[1];
b0ssiz 6:f6529ad2a70d 139 package.parameter[6]=FloatKi[0];
b0ssiz 6:f6529ad2a70d 140 package.parameter[7]=FloatKi[1];
b0ssiz 6:f6529ad2a70d 141 package.parameter[8]=IntKd[0];
b0ssiz 6:f6529ad2a70d 142 package.parameter[9]=IntKd[1];
b0ssiz 6:f6529ad2a70d 143 package.parameter[10]=FloatKd[0];
b0ssiz 6:f6529ad2a70d 144 package.parameter[11]=FloatKd[1];
b0ssiz 6:f6529ad2a70d 145
b0ssiz 6:f6529ad2a70d 146 rs485_dirc=1;
b0ssiz 6:f6529ad2a70d 147 wait_us(RS485_DELAY);
b0ssiz 6:f6529ad2a70d 148 return com->sendCommunicatePacket(&package);
b0ssiz 6:f6529ad2a70d 149 }