สัสชิน
Dependencies: BEAR_Protocol_Edited_V22 BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V2 by
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);