Secret_Betago
สัสชิน
Dependencies: BEAR_Protocol_Edited_V22 BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Revision 8:fc70c78a443b, committed 2016-06-08
- Comitter:
- icyzkungz
- Date:
- Wed Jun 08 17:19:21 2016 +0000
- Parent:
- 7:ffd6959444ae
- Commit message:
- ??????
Changed in this revision
--- a/BEAR_Protocol_Edited.lib Tue Jun 07 17:25:18 2016 +0000 +++ b/BEAR_Protocol_Edited.lib Wed Jun 08 17:19:21 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/Betago/code/BEAR_Protocol_Edited/#c851f0ab826c +https://developer.mbed.org/teams/Secret_Betago/code/BEAR_Protocol_Edited_V22/#65e0cf1b1844
--- a/main.cpp Tue Jun 07 17:25:18 2016 +0000
+++ b/main.cpp Wed Jun 08 17:19:21 2016 +0000
@@ -31,17 +31,20 @@
Timeout time_distance;
Timeout shutdown;
move m1;
+
+//Serial pc(SERIAL_TX,SERIAL_RX);
+BufferedSerial pc(SERIAL_TX,SERIAL_RX);
//*****************************************************/
// Global //
//timer
- int timer_now=0,timer_later=0;
- int times=0,timer_buffer=0;
-
- //encoder
- int Encoderpos = 0;
- int real_d=0;
- float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
+int timer_now=0,timer_later=0;
+int times=0,timer_buffer=0;
+
+//encoder
+int Encoderpos = 0;
+int real_d=0;
+float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
//pid
double setp1=0,setp2=0;
@@ -61,12 +64,12 @@
EEPROM memory(PB_4,PA_8,0);
float KP_LEFT_BUFF=0,KI_LEFT_BUFF=0,KD_LEFT_BUFF=0,KP_RIGHT_BUFF=0,KI_RIGHT_BUFF =0,KD_RIGHT_BUFF=0;
- void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
- void RC();
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+void RC();
//rplidar
- //float distances = 0;
- //float angle = 0;
+//float distances = 0;
+//float angle = 0;
//ool startBit = 0;
//char quality =0 ;
@@ -82,43 +85,46 @@
//s1.get_motor();รับค่ามอเตอร์
RC();
timer_later= timer_now;
-
+
}
void EncoderA_1()//ซ้าย
-{ if(encoderB_1==0)
- { Encoderpos = Encoderpos + 1;}
- else
- { Encoderpos = Encoderpos -1;}
- pulse_1+=1;
- //Encoderpos = Encoderpos + 1;
- //valocity+=1;
- //pc.printf("%d \n",Encoderpos);
- //pc.printf("pulse=%f \n",pulse);
- //if(pulse==128)
- //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
+{
+ if(encoderB_1==0) {
+ Encoderpos = Encoderpos + 1;
+ } else {
+ Encoderpos = Encoderpos -1;
+ }
+ pulse_1+=1;
+ //Encoderpos = Encoderpos + 1;
+ //valocity+=1;
+ //pc.printf("%d \n",Encoderpos);
+ //pc.printf("pulse=%f \n",pulse);
+ //if(pulse==128)
+ //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
}
- void EncoderA_2()//ขวา
-{
- if(encoderB_2==0)
- { Encoderpos = Encoderpos + 1;}
- else
- { Encoderpos = Encoderpos -1;}
+void EncoderA_2()//ขวา
+{
+ if(encoderB_2==0) {
+ Encoderpos = Encoderpos + 1;
+ } else {
+ Encoderpos = Encoderpos -1;
+ }
pulse_2+=1;
//pc.printf("%d",Encoderpos);
}
void EncoderA_D()
-{
- if(encoderB_d==0)
- { Encoderpos = Encoderpos + 1;}
- else
- { Encoderpos = Encoderpos -1;}
+{
+ if(encoderB_d==0) {
+ Encoderpos = Encoderpos + 1;
+ } else {
+ Encoderpos = Encoderpos -1;
+ }
pulse_d+=1;
- if(pulse_d==128)
- {
+ if(pulse_d==128) {
Z_d+=1;
pulse_d=0;
}
-
+
}
void getvelo1()//จาก encoder
{
@@ -138,60 +144,49 @@
{
real_d=Z_d*(2*3.14*r);
//ส่งข้อมูล
-
+
}
void get_rplidar()
{
- if (IS_OK(lidar.waitPoint()))
- {
-
- }
- else
- {
-
- lidar.startScan();
-
- }
-
+ if (!IS_OK(lidar.waitPoint())) lidar.startScan();
}
+
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-
+
}
void PID_m1()//left
{
setp1=map(1.0,0.0,1.094,0.0,1.0);
P1.setSetPoint(setp1);
- times=timerStart.read();
- if(times==1)// m/s
- {
- getvelo1();
- //pc.printf("TIME \n");
- times=0;
- pulse_1=0;
- }
+ times=timerStart.read();
+ if(times==1) { // m/s
+ getvelo1();
+ //pc.printf("TIME \n");
+ times=0;
+ pulse_1=0;
+ }
P1.setProcessValue(valocity1);
outPID=P1.compute();
- //pc.printf("outPID=%f \n",outPID);
- m1.movespeed_1(setp1,outPID);
+ //pc.printf("outPID=%f \n",outPID);
+ m1.movespeed_1(setp1,outPID);
}
void PID_m2()//right
{
setp2=map(1.0,0.0,1.094,0.0,1.0);
P2.setSetPoint(setp2);
- times=timerStart.read();
- if(times==1)// m/s
- {
- getvelo2();
- //pc.printf("TIME \n");
- times=0;
- pulse_2=0;
- }
+ times=timerStart.read();
+ if(times==1) { // m/s
+ getvelo2();
+ //pc.printf("TIME \n");
+ times=0;
+ pulse_2=0;
+ }
P2.setProcessValue(valocity2);
outPID=P2.compute();
- //pc.printf("outPID=%f \n",outPID);
- m1.movespeed_2(setp2,outPID);
+ //pc.printf("outPID=%f \n",outPID);
+ m1.movespeed_2(setp2,outPID);
}
@@ -215,32 +210,69 @@
}
/*******************************************************/
int buf=0;
+
+DigitalIn button(USER_BUTTON);
+
int main()
{
PC.baud(115200);
lidar.begin(se_lidar);
+ int buff =0;
tim.start();
//com1 = new COMMUNICATION(PA_9,PA_10,115200);
- encoderA_1.rise(&EncoderA_1);
- timerStart.start();
- P1.setMode(1);
- P1.setBias(0);
- // pc.printf("READY \n");
+ encoderA_1.rise(&EncoderA_1);
+ timerStart.start();
+ P1.setMode(1);
+ P1.setBias(0);
+ // pc.printf("READY \n");
//pc.attach(&Rx_interrupt, Serial::RxIrq);
lidar.setAngle(0,360);
+ VMO=1;
+
+ pc.baud(115200);
+ int LidarData[360];
+ for(int i=0; i<360; i++)
+ LidarData[i] = 9999;
+
while(1) {
- VMO=1;
get_rplidar();
+ LidarData[lidar.ang] = lidar.dis;
+ //if(tim.read_ms()-buff>=1000){
+ for(int i=0;i<360;i++)
+ pc.printf("[%d]-%d\t",i,LidarData[i]);
+ //buff=tim.read_ms();
+ //}
+ //pc.printf("while loop\n");
+ /*if(button==0)
+ {
+ while(button==0);
+ for(int i=0;i<360;i++)
+ pc.printf("[%d]-%d\t",i,LidarData[i]);
+ }
+ pc.printf("while loop\n");*/
+
+
+
+ }
+
+
+//pc.printf("Ang[Dis] : %d[%d]\n",lidar.ang,lidar.dis);
+
+//wait_ms(1000);
+ /*if(t.read_ms()>=1){
+ for(int i=0;i<360;i++)
+ printf("%d,",lidar_data[i]);
+ t.reset();
+ }*/
/* if(tim.read_ms()-buf>=1000){
for(int x=0;x<=359;x++){
printf("%d,",lidar.Data[x]);
}
buf = tim.read_ms();
}*/
- RC();
- //wait_ms(1);
- }
+//com.UpdateLidar();
+// RC();
}
@@ -251,10 +283,11 @@
+
void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
- PC.printf("cmdcheck\n");
- if(id==MY_ID) {
+ PC.printf("cmdcheck\n");
+ if(id==MY_ID) {
switch (ins) {
case PING: {
break;
@@ -317,10 +350,10 @@
float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
- flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
VRmax=Int+flo;
PC.printf("VRmax = %f",VRmax);
- // PC.printf("*****************************");
+ // PC.printf("*****************************");
break;
}
//save to rom
@@ -329,35 +362,35 @@
float Int,flo;
int_buffer[0]=command[1];
int_buffer[1]=command[2];
- float_buffer[0]=command[3];
+ float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KP_LEFT=Int+flo;
- PC.printf("KP_LEFT=%f /n",KP_LEFT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_LEFT_KP,data_buff);
- wait_ms(EEPROM_DELAY);
+ KP_LEFT=Int+flo;
+ PC.printf("KP_LEFT=%f /n",KP_LEFT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KP,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KI_LEFT: {
- uint8_t int_buffer[2],float_buffer[2];
+ uint8_t int_buffer[2],float_buffer[2];
float Int,flo;
int_buffer[0]=command[1];
int_buffer[1]=command[2];
- float_buffer[0]=command[3];
+ float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KI_LEFT=Int+flo;
- PC.printf("KI_LEFT=%f /n",KI_LEFT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_LEFT_KI ,data_buff);
- wait_ms(EEPROM_DELAY);
+ KI_LEFT=Int+flo;
+ PC.printf("KI_LEFT=%f /n",KI_LEFT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KI ,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KD_LEFT: {
@@ -365,16 +398,16 @@
float Int,flo;
int_buffer[0]=command[1];
int_buffer[1]=command[2];
- float_buffer[0]=command[3];
+ float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
- flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KD_LEFT=Int+flo;
- PC.printf("KD_LEFT=%f /n",KD_LEFT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_LEFT_KD,data_buff);
- wait_ms(EEPROM_DELAY);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ KD_LEFT=Int+flo;
+ PC.printf("KD_LEFT=%f /n",KD_LEFT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KD,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KP_RIGHT: {
@@ -382,33 +415,33 @@
float Int,flo;
int_buffer[0]=command[1];
int_buffer[1]=command[2];
- float_buffer[0]=command[3];
+ float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
- flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KP_RIGHT=Int+flo;
- PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_RIGHT_KP,data_buff);
- wait_ms(EEPROM_DELAY);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ KP_RIGHT=Int+flo;
+ PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KP,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KI_RIGHT: {
- uint8_t int_buffer[2],float_buffer[2];
+ uint8_t int_buffer[2],float_buffer[2];
float Int,flo;
int_buffer[0]=command[1];
int_buffer[1]=command[2];
- float_buffer[0]=command[3];
+ float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
- flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KI_RIGHT=Int+flo;
- PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_RIGHT_KI,data_buff);
- wait_ms(EEPROM_DELAY);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ KI_RIGHT=Int+flo;
+ PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KI,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KD_RIGHT: {
@@ -419,237 +452,240 @@
float_buffer[0]=command[3];
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
- flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KD_RIGHT=Int+flo;
- PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
- int32_t data_buff;
- data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
- memory.write(ADDRESS_RIGHT_KD,data_buff);
- wait_ms(EEPROM_DELAY);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ KD_RIGHT=Int+flo;
+ PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KD,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
- }
- } break;
- case READ_DATA: { PC.printf("READ_DATA\n");
+ }
+ }
+ break;
+ case READ_DATA: {
+ PC.printf("READ_DATA\n");
switch (command[0]) {
case GET_LIDAR: {
-
- com.sendlidar();
+
+ com.sendlidar(lidar);
PC.printf("SEND1\n");
break;
- }
- case GET_LIDAR2: {
-
- com.sendlidar2();
+ }
+ case GET_LIDAR2: {
+
+ com.sendlidar2(lidar);
PC.printf("SEND2\n");
break;
- }
- case GET_LIDAR3: {
-
- com.sendlidar3();
+ }
+ case GET_LIDAR3: {
+
+ com.sendlidar3(lidar);
PC.printf("SEND3\n");
- break;
- }
- case GET_LIDAR4: {
-
- com.sendlidar4();
+ break;
+ }
+ case GET_LIDAR4: {
+
+ com.sendlidar4(lidar);
PC.printf("SEND4\n");
- break;
- }
- case GET_LIDAR5: {
-
-
- com.sendlidar5();
+ break;
+ }
+ case GET_LIDAR5: {
+
+
+ com.sendlidar5(lidar);
PC.printf("SEND5\n");
- break;
- }
- case GET_LIDAR6: {
- com.sendlidar6();
- PC.printf("SEND6\n");
-
- break;
- }
-
+ break;
+ }
+ case GET_LIDAR6: {
+ com.sendlidar6(lidar);
+ PC.printf("SEND6\n");
+
+ break;
+ }
+
case GET_BATTERY: {
-
+
break;
- }
+ }
case GET_VELOCITY_LEFT: {
uint8_t intVelo_L[2],floatVelo_L[2];
com.FloatSep(valocity1,intVelo_L,floatVelo_L);
- ANDANTE_PROTOCOL_PACKET package;
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intVelo_L[0];
package.parameter[1]=intVelo_L[1];
- package.parameter[2]=floatVelo_L[0];
+ package.parameter[2]=floatVelo_L[0];
package.parameter[3]=floatVelo_L[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+
break;
- }
+ }
case GET_VELOCITY_RIGHT : {
uint8_t intVelo_R[2],floatVelo_R[2];
com.FloatSep(valocity2,intVelo_R,floatVelo_R);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intVelo_R[0];
package.parameter[1]=intVelo_R[1];
- package.parameter[2]=floatVelo_R[0];
+ package.parameter[2]=floatVelo_R[0];
package.parameter[3]=floatVelo_R[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KP_LEFT: {
memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF);
uint8_t intKPL[2],floatKPL[2];
com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKPL[0];
package.parameter[1]=intKPL[1];
- package.parameter[2]=floatKPL[0];
+ package.parameter[2]=floatKPL[0];
package.parameter[3]=floatKPL[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KI_LEFT: {
memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
uint8_t intKIL[2],floatKIL[2];
com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKIL[0];
package.parameter[1]=intKIL[1];
- package.parameter[2]=floatKIL[0];
+ package.parameter[2]=floatKIL[0];
package.parameter[3]=floatKIL[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KD_LEFT: {
memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
uint8_t intKDL[2],floatKDL[2];
com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKDL[0];
package.parameter[1]=intKDL[1];
- package.parameter[2]=floatKDL[0];
+ package.parameter[2]=floatKDL[0];
package.parameter[3]=floatKDL[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KP_RIGHT: {
memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
uint8_t intKDR[2],floatKDR[2];
com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKDR[0];
package.parameter[1]=intKDR[1];
- package.parameter[2]=floatKDR[0];
+ package.parameter[2]=floatKDR[0];
package.parameter[3]=floatKDR[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KI_RIGHT: {
memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
uint8_t intKIR[2],floatKIR[2];
com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKIR[0];
package.parameter[1]=intKIR[1];
- package.parameter[2]=floatKIR[0];
+ package.parameter[2]=floatKIR[0];
package.parameter[3]=floatKIR[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
+ }
case GET_KD_RIGHT: {
memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
uint8_t intKDR[2],floatKDR[2];
com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
-
-
- ANDANTE_PROTOCOL_PACKET package;
+
+
+ ANDANTE_PROTOCOL_PACKET package;
package.robotId = MY_ID;
- package.length = 6;
+ package.length = 6;
package.instructionErrorId = WRITE_DATA;
package.parameter[0]=intKDR[0];
package.parameter[1]=intKDR[1];
- package.parameter[2]=floatKDR[0];
+ package.parameter[2]=floatKDR[0];
package.parameter[3]=floatKDR[1];
rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
- }
- }break;
-
+ }
+ }
+ }
+ break;
+
+ }
}
- }
}
--- a/rplidar/RPlidar.cpp Tue Jun 07 17:25:18 2016 +0000
+++ b/rplidar/RPlidar.cpp Wed Jun 08 17:19:21 2016 +0000
@@ -68,11 +68,12 @@
}
// close the currently opened serial interface
void RPLidar::end()
-{/*
- if (isOpen()) {
- _bined_serialdev->end();
- _bined_serialdev = NULL;
- }*/
+{
+ /*
+ if (isOpen()) {
+ _bined_serialdev->end();
+ _bined_serialdev = NULL;
+ }*/
}
@@ -97,7 +98,7 @@
u_result ans;
- // if (!isOpen()) return RESULT_OPERATION_FAIL;
+ // if (!isOpen()) return RESULT_OPERATION_FAIL;
{
if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
@@ -140,7 +141,7 @@
rplidar_ans_header_t response_header;
u_result ans;
- // if (!isOpen()) return RESULT_OPERATION_FAIL;
+ // if (!isOpen()) return RESULT_OPERATION_FAIL;
{
if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
@@ -192,23 +193,23 @@
stop(); //force the previous operation to stop
- ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
- if (IS_FAIL(ans)) return ans;
+ ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
+ if (IS_FAIL(ans)) return ans;
- // waiting for confirmation
- rplidar_ans_header_t response_header;
- if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
- return ans;
- }
+ // waiting for confirmation
+ rplidar_ans_header_t response_header;
+ if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+ return ans;
+ }
- // verify whether we got a correct header
- if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
- return RESULT_INVALID_DATA;
- }
+ // verify whether we got a correct header
+ if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
+ return RESULT_INVALID_DATA;
+ }
- if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
- return RESULT_INVALID_DATA;
- }
+ if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
+ return RESULT_INVALID_DATA;
+ }
return RESULT_OK;
}
@@ -216,69 +217,70 @@
// wait for one sample point to arrive
u_result RPLidar::waitPoint(_u32 timeout)
{
- _u32 currentTs = timers.read_ms();
- _u32 remainingtime;
- rplidar_response_measurement_node_t node;
- _u8 *nodebuf = (_u8*)&node;
+ _u32 currentTs = timers.read_ms();
+ _u32 remainingtime;
+ rplidar_response_measurement_node_t node;
+ _u8 *nodebuf = (_u8*)&node;
- _u8 recvPos = 0;
+ _u8 recvPos = 0;
- while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+ while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
int currentbyte = _bined_serialdev->getc();
if (currentbyte<0) continue;
//Serial.println(currentbyte);
switch (recvPos) {
- case 0: // expect the sync bit and its reverse in this byte {
- {
- _u8 tmp = (currentbyte>>1);
- if ( (tmp ^ currentbyte) & 0x1 ) {
- // pass
- } else {
- continue;
- }
+ case 0: { // expect the sync bit and its reverse in this byte {
+ _u8 tmp = (currentbyte>>1);
+ if ( (tmp ^ currentbyte) & 0x1 ) {
+ // pass
+ } else {
+ continue;
+ }
+ }
+ break;
+ case 1: { // expect the highest bit to be 1
+ if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+ // pass
+ } else {
+ recvPos = 0;
+ continue;
}
- break;
- case 1: // expect the highest bit to be 1
- {
- if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
- // pass
- } else {
- recvPos = 0;
- continue;
- }
- }
- break;
- }
- nodebuf[recvPos++] = currentbyte;
- if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
- // store the data ...
- _currentMeasurement.distance = node.distance_q2/4.0f;
- _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
- _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
- _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
- ang = _currentMeasurement.angle;
- dis = _currentMeasurement.distance;
+ }
+ break;
+ }
+ nodebuf[recvPos++] = currentbyte;
+ if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
+ // store the data ...
+ _currentMeasurement.distance = node.distance_q2/4.0f;
+ _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
+ _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+ _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
+ ang = _currentMeasurement.angle;
+ dis = _currentMeasurement.distance;
+
+ //Data[ang] = dis;
+ if(ang>=angMin&&ang<=angMax) Data[ang] = dis;
+
+ return RESULT_OK;
+ }
- if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
-
- return RESULT_OK;
- }
+ }
-
- }
-
- return RESULT_OPERATION_TIMEOUT;
+ return RESULT_OPERATION_TIMEOUT;
}
-void RPLidar::setAngle(int min,int max){
- angMin = min;
- angMax = max;
+void RPLidar::setAngle(int min,int max)
+{
+ angMin = min;
+ angMax = max;
}
-void RPLidar::get_lidar(){
- if (!IS_OK(waitPoint())) startScan();
+void RPLidar::get_lidar(int* data)
+{
+ if (!IS_OK(waitPoint())) startScan();
+ // data = Data;
}
u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
{
@@ -298,7 +300,7 @@
_bined_serialdev->putc(header->syncByte );
_bined_serialdev->putc(header->cmd_flag );
- // _bined_serialdev->write( (uint8_t *)header, 2);
+ // _bined_serialdev->write( (uint8_t *)header, 2);
if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
checksum ^= RPLIDAR_CMD_SYNC_BYTE;
@@ -313,15 +315,15 @@
// send size
_u8 sizebyte = payloadsize;
_bined_serialdev->putc(sizebyte);
- // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
+ // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
// send payload
- // _bined_serialdev.putc((uint8_t )payload);
- // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
+ // _bined_serialdev.putc((uint8_t )payload);
+ // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
// send checksum
_bined_serialdev->putc(checksum);
- // _bined_serialdev->write((uint8_t *)&checksum, 1);
+ // _bined_serialdev->write((uint8_t *)&checksum, 1);
}
@@ -339,17 +341,17 @@
int currentbyte = _bined_serialdev->getc();
if (currentbyte<0) continue;
switch (recvPos) {
- case 0:
- if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
- continue;
- }
- break;
- case 1:
- if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
- recvPos = 0;
- continue;
- }
- break;
+ case 0:
+ if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
+ continue;
+ }
+ break;
+ case 1:
+ if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
+ recvPos = 0;
+ continue;
+ }
+ break;
}
headerbuf[recvPos++] = currentbyte;
--- a/rplidar/rplidar.h Tue Jun 07 17:25:18 2016 +0000
+++ b/rplidar/rplidar.h Wed Jun 08 17:19:21 2016 +0000
@@ -76,15 +76,17 @@
u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
// retrieve currently received sample point
- int Data[360];
+ uint8_t Data[360];
int ang,dis;
int angMin,angMax;
void setAngle(int min,int max);
- void get_lidar();
+ void get_lidar(int*);
+
const RPLidarMeasurement & getCurrentPoint()
{
return _currentMeasurement;
}
+
protected:
// u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);