Arsapol Manamunchaiyaporn / Communication_Robot

before test

Fork of Communication_Robot by Betago

Receiver.cpp@14:2b253d0b4e63, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 16:16:27 2016 +0000
Revision:
14:2b253d0b4e63
Parent:
13:bc19774be4df 
before test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
palmdotax 11:3c11a0355a3e 1 #include "mbed.h"
palmdotax 11:3c11a0355a3e 2 #include "Receiver.h"
palmdotax 13:bc19774be4df 3 #include "rplidar.h"
palmdotax 13:bc19774be4df 4 RPLidar lidar1;
palmdotax 11:3c11a0355a3e 5
palmdotax 11:3c11a0355a3e 6 DigitalOut rs485_dirc(RS485_DIRC);
palmdotax 11:3c11a0355a3e 7
palmdotax 11:3c11a0355a3e 8 Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate)
palmdotax 11:3c11a0355a3e 9 {
palmdotax 11:3c11a0355a3e 10 com = new COMMUNICATION(tx,rx,baudrate);
palmdotax 11:3c11a0355a3e 11 }
palmdotax 11:3c11a0355a3e 12
palmdotax 11:3c11a0355a3e 13 uint8_t Bear_Receiver::ReceiveCommand(uint8_t *id,uint8_t *data_array,uint8_t *ins)
palmdotax 11:3c11a0355a3e 14 {
palmdotax 11:3c11a0355a3e 15 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 16
palmdotax 11:3c11a0355a3e 17 rs485_dirc=0;
palmdotax 11:3c11a0355a3e 18 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 19 uint8_t status = com->receiveCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 20
palmdotax 11:3c11a0355a3e 21 if(status == ANDANTE_ERRBIT_NONE) {
palmdotax 11:3c11a0355a3e 22
palmdotax 11:3c11a0355a3e 23 *id = package.robotId;
palmdotax 11:3c11a0355a3e 24 for(int i=0; i<30; i++) {
palmdotax 11:3c11a0355a3e 25 data_array[i] = package.parameter[i];
palmdotax 11:3c11a0355a3e 26 }
palmdotax 11:3c11a0355a3e 27 *ins=package.instructionErrorId;
palmdotax 11:3c11a0355a3e 28
palmdotax 11:3c11a0355a3e 29 }
palmdotax 11:3c11a0355a3e 30 return status;
palmdotax 11:3c11a0355a3e 31 }
palmdotax 11:3c11a0355a3e 32
palmdotax 11:3c11a0355a3e 33 void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
palmdotax 11:3c11a0355a3e 34 {
palmdotax 11:3c11a0355a3e 35 float float_buffer;
palmdotax 11:3c11a0355a3e 36 float int_buffer;
palmdotax 11:3c11a0355a3e 37 int16_t integer;
palmdotax 11:3c11a0355a3e 38 int16_t floating_point;
palmdotax 11:3c11a0355a3e 39
palmdotax 11:3c11a0355a3e 40 float_buffer=modf(input_float,&int_buffer);
palmdotax 11:3c11a0355a3e 41 float_buffer*=FLOAT_CONVERTER;
palmdotax 11:3c11a0355a3e 42 integer=(int16_t)int_buffer;
palmdotax 11:3c11a0355a3e 43 floating_point=(int16_t)float_buffer;
palmdotax 11:3c11a0355a3e 44 Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
palmdotax 11:3c11a0355a3e 45 Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
palmdotax 11:3c11a0355a3e 46 }
palmdotax 11:3c11a0355a3e 47
icyzkungz 14:2b253d0b4e63 48 uint8_t Bear_Receiver::sendKpKiKdLeft(float Kp,float Ki,float Kd)
palmdotax 11:3c11a0355a3e 49 {
palmdotax 11:3c11a0355a3e 50 uint8_t IntKp[2],FloatKp[2];
palmdotax 11:3c11a0355a3e 51 uint8_t IntKi[2],FloatKi[2];
palmdotax 11:3c11a0355a3e 52 uint8_t IntKd[2],FloatKd[2];
palmdotax 11:3c11a0355a3e 53
palmdotax 11:3c11a0355a3e 54 FloatSep(Kp,IntKp,FloatKp);
palmdotax 11:3c11a0355a3e 55 FloatSep(Ki,IntKi,FloatKi);
palmdotax 11:3c11a0355a3e 56 FloatSep(Kd,IntKd,FloatKd);
palmdotax 11:3c11a0355a3e 57
palmdotax 11:3c11a0355a3e 58
palmdotax 11:3c11a0355a3e 59 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 60
icyzkungz 14:2b253d0b4e63 61 package.robotId = ID;
palmdotax 11:3c11a0355a3e 62 package.length = 14;
palmdotax 11:3c11a0355a3e 63 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 64 package.parameter[0]=IntKp[0];
palmdotax 11:3c11a0355a3e 65 package.parameter[1]=IntKp[1];
palmdotax 11:3c11a0355a3e 66 package.parameter[2]=FloatKp[0];
palmdotax 11:3c11a0355a3e 67 package.parameter[3]=FloatKp[1];
palmdotax 11:3c11a0355a3e 68 package.parameter[4]=IntKi[0];
palmdotax 11:3c11a0355a3e 69 package.parameter[5]=IntKi[1];
palmdotax 11:3c11a0355a3e 70 package.parameter[6]=FloatKi[0];
palmdotax 11:3c11a0355a3e 71 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 72 package.parameter[8]=IntKd[0];
palmdotax 11:3c11a0355a3e 73 package.parameter[9]=IntKd[1];
palmdotax 11:3c11a0355a3e 74 package.parameter[10]=FloatKd[0];
palmdotax 11:3c11a0355a3e 75 package.parameter[11]=FloatKd[1];
palmdotax 11:3c11a0355a3e 76
palmdotax 11:3c11a0355a3e 77 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 78 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 79 }
palmdotax 11:3c11a0355a3e 80
icyzkungz 14:2b253d0b4e63 81 uint8_t Bear_Receiver::sendKpKiKdRight(float Kp,float Ki,float Kd)
palmdotax 11:3c11a0355a3e 82 {
palmdotax 11:3c11a0355a3e 83 uint8_t IntKp[2],FloatKp[2];
palmdotax 11:3c11a0355a3e 84 uint8_t IntKi[2],FloatKi[2];
palmdotax 11:3c11a0355a3e 85 uint8_t IntKd[2],FloatKd[2];
palmdotax 11:3c11a0355a3e 86
palmdotax 11:3c11a0355a3e 87 FloatSep(Kp,IntKp,FloatKp);
palmdotax 11:3c11a0355a3e 88 FloatSep(Ki,IntKi,FloatKi);
palmdotax 11:3c11a0355a3e 89 FloatSep(Kd,IntKd,FloatKd);
palmdotax 11:3c11a0355a3e 90
palmdotax 11:3c11a0355a3e 91
palmdotax 11:3c11a0355a3e 92 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 93
icyzkungz 14:2b253d0b4e63 94 package.robotId = ID;
palmdotax 11:3c11a0355a3e 95 package.length = 14;
palmdotax 11:3c11a0355a3e 96 package.instructionErrorId = WRITE_DATA;
palmdotax 11:3c11a0355a3e 97 package.parameter[0]=IntKp[0];
palmdotax 11:3c11a0355a3e 98 package.parameter[1]=IntKp[1];
palmdotax 11:3c11a0355a3e 99 package.parameter[2]=FloatKp[0];
palmdotax 11:3c11a0355a3e 100 package.parameter[3]=FloatKp[1];
palmdotax 11:3c11a0355a3e 101 package.parameter[4]=IntKi[0];
palmdotax 11:3c11a0355a3e 102 package.parameter[5]=IntKi[1];
palmdotax 11:3c11a0355a3e 103 package.parameter[6]=FloatKi[0];
palmdotax 11:3c11a0355a3e 104 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 105 package.parameter[8]=IntKd[0];
palmdotax 11:3c11a0355a3e 106 package.parameter[9]=IntKd[1];
palmdotax 11:3c11a0355a3e 107 package.parameter[10]=FloatKd[0];
palmdotax 11:3c11a0355a3e 108 package.parameter[11]=FloatKd[1];
palmdotax 11:3c11a0355a3e 109
palmdotax 11:3c11a0355a3e 110 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 111 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 112 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 113 }
palmdotax 11:3c11a0355a3e 114
icyzkungz 14:2b253d0b4e63 115 uint8_t Bear_Receiver::sendMaxMinLidarDegree(float Kp,float Ki)
icyzkungz 14:2b253d0b4e63 116 {
icyzkungz 14:2b253d0b4e63 117 uint8_t IntKp[2],FloatKp[2];
icyzkungz 14:2b253d0b4e63 118 uint8_t IntKi[2],FloatKi[2];
icyzkungz 14:2b253d0b4e63 119
icyzkungz 14:2b253d0b4e63 120 FloatSep(Kp,IntKp,FloatKp);
icyzkungz 14:2b253d0b4e63 121 FloatSep(Ki,IntKi,FloatKi);
palmdotax 11:3c11a0355a3e 122
palmdotax 11:3c11a0355a3e 123
palmdotax 11:3c11a0355a3e 124 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 125
icyzkungz 14:2b253d0b4e63 126 package.robotId = ID;
icyzkungz 14:2b253d0b4e63 127 package.length = 10;
palmdotax 11:3c11a0355a3e 128 package.instructionErrorId = WRITE_DATA;
icyzkungz 14:2b253d0b4e63 129 package.parameter[0]=IntKp[0];
icyzkungz 14:2b253d0b4e63 130 package.parameter[1]=IntKp[1];
icyzkungz 14:2b253d0b4e63 131 package.parameter[2]=FloatKp[0];
icyzkungz 14:2b253d0b4e63 132 package.parameter[3]=FloatKp[1];
icyzkungz 14:2b253d0b4e63 133 package.parameter[4]=IntKi[0];
icyzkungz 14:2b253d0b4e63 134 package.parameter[5]=IntKi[1];
icyzkungz 14:2b253d0b4e63 135 package.parameter[6]=FloatKi[0];
icyzkungz 14:2b253d0b4e63 136 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 137
palmdotax 11:3c11a0355a3e 138 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 139 wait_us(RS485_DELAY);
palmdotax 11:3c11a0355a3e 140 return com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 141 }
palmdotax 11:3c11a0355a3e 142
icyzkungz 14:2b253d0b4e63 143 uint8_t Bear_Receiver::sendSensorDataAll1(float *data)
palmdotax 11:3c11a0355a3e 144 {
icyzkungz 14:2b253d0b4e63 145 uint8_t IntArray[2],FloatArray[2];
palmdotax 11:3c11a0355a3e 146
palmdotax 11:3c11a0355a3e 147 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 148
icyzkungz 14:2b253d0b4e63 149 package.robotId = ID;
icyzkungz 14:2b253d0b4e63 150 package.length = 42;
palmdotax 11:3c11a0355a3e 151 package.instructionErrorId = WRITE_DATA;
icyzkungz 14:2b253d0b4e63 152 int k=0;
icyzkungz 14:2b253d0b4e63 153 for(int i=0; i<40; i++) {
icyzkungz 14:2b253d0b4e63 154 if(i%2==0)
icyzkungz 14:2b253d0b4e63 155 {
icyzkungz 14:2b253d0b4e63 156 FloatSep(data[k],IntArray,FloatArray);
icyzkungz 14:2b253d0b4e63 157 package.parameter[i-2]=IntArray[0];
icyzkungz 14:2b253d0b4e63 158 package.parameter[i-1]=IntArray[1];
icyzkungz 14:2b253d0b4e63 159 k++;
icyzkungz 14:2b253d0b4e63 160 }
icyzkungz 14:2b253d0b4e63 161 if(i==39)
icyzkungz 14:2b253d0b4e63 162 {
icyzkungz 14:2b253d0b4e63 163 FloatSep(data[k],IntArray,FloatArray);
icyzkungz 14:2b253d0b4e63 164 package.parameter[i-2]=IntArray[0];
icyzkungz 14:2b253d0b4e63 165 package.parameter[i-1]=IntArray[1];
icyzkungz 14:2b253d0b4e63 166 }
icyzkungz 14:2b253d0b4e63 167 }
palmdotax 11:3c11a0355a3e 168
icyzkungz 14:2b253d0b4e63 169 uint8_t status = com->sendCommunicatePacket(&package);
palmdotax 11:3c11a0355a3e 170 wait_us(RS485_DELAY);
icyzkungz 14:2b253d0b4e63 171 return status;
palmdotax 11:3c11a0355a3e 172
icyzkungz 14:2b253d0b4e63 173 //xxxxxxxx
icyzkungz 14:2b253d0b4e63 174 /*uint8_t IntKp[2],FloatKp[2];
icyzkungz 14:2b253d0b4e63 175 uint8_t IntKi[2],FloatKi[2];
icyzkungz 14:2b253d0b4e63 176
icyzkungz 14:2b253d0b4e63 177 FloatSep(Kp,IntKp,FloatKp);
icyzkungz 14:2b253d0b4e63 178 FloatSep(Ki,IntKi,FloatKi);
icyzkungz 14:2b253d0b4e63 179
icyzkungz 14:2b253d0b4e63 180
palmdotax 11:3c11a0355a3e 181 ANDANTE_PROTOCOL_PACKET package;
palmdotax 11:3c11a0355a3e 182
icyzkungz 14:2b253d0b4e63 183 package.robotId = ID;
palmdotax 11:3c11a0355a3e 184 package.length = 10;
palmdotax 11:3c11a0355a3e 185 package.instructionErrorId = WRITE_DATA;
icyzkungz 14:2b253d0b4e63 186 package.parameter[0]=IntKp[0];
icyzkungz 14:2b253d0b4e63 187 package.parameter[1]=IntKp[1];
icyzkungz 14:2b253d0b4e63 188 package.parameter[2]=FloatKp[0];
icyzkungz 14:2b253d0b4e63 189 package.parameter[3]=FloatKp[1];
icyzkungz 14:2b253d0b4e63 190 package.parameter[4]=IntKi[0];
icyzkungz 14:2b253d0b4e63 191 package.parameter[5]=IntKi[1];
icyzkungz 14:2b253d0b4e63 192 package.parameter[6]=FloatKi[0];
icyzkungz 14:2b253d0b4e63 193 package.parameter[7]=FloatKi[1];
palmdotax 11:3c11a0355a3e 194
palmdotax 11:3c11a0355a3e 195 rs485_dirc=1;
palmdotax 11:3c11a0355a3e 196 wait_us(RS485_DELAY);
icyzkungz 14:2b253d0b4e63 197 return com->sendCommunicatePacket(&package);*/
icyzkungz 14:2b253d0b4e63 198 }