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

Committer:
b0ssiz
Date:
Sat Jan 16 12:01:43 2016 +0000
Revision:
7:a0ea8c127611
Parent:
6:f6529ad2a70d
Child:
8:2d34916ac178
Update; - Add function

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