สัสชิน
Dependencies: BEAR_Protocol_Edited_V22 BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V2 by
Revision 4:de5a65c17664, committed 2016-06-05
- Comitter:
- palmdotax
- Date:
- Sun Jun 05 09:43:40 2016 +0000
- Parent:
- 3:edaab92dbd2f
- Child:
- 5:fe76f3dae81e
- Commit message:
- v1;
Changed in this revision
--- a/BEAR_Protocol_Edited.lib Tue May 24 10:33:21 2016 +0000 +++ b/BEAR_Protocol_Edited.lib Sun Jun 05 09:43:40 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/palm-and-chin/code/BEAR_Protocol_Edited/#13640152de69 +https://developer.mbed.org/teams/palm-and-chin/code/BEAR_Protocol_Edited/#168de1acec53
--- a/main.cpp Tue May 24 10:33:21 2016 +0000 +++ b/main.cpp Sun Jun 05 09:43:40 2016 +0000 @@ -51,9 +51,9 @@ //Ticker Recieve; //-- Communication -- COMMUNICATION *com1; -BufferedSerial PC(SERIAL_TX,SERIAL_RX); -//Serial PC(SERIAL_TX,SERIAL_RX); -Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200); +//BufferedSerial PC(SERIAL_TX,SERIAL_RX); +Serial PC(SERIAL_TX,SERIAL_RX); +Bear_Receiver com(PA_9,PA_10,115200); int16_t MY_ID = 0x00; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); @@ -63,10 +63,10 @@ void RC(); //rplidar - float distances = 0; - float angle = 0; -bool startBit = 0; -char quality =0 ; + //float distances = 0; + //float angle = 0; +//ool startBit = 0; +//char quality =0 ; void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); @@ -140,23 +140,15 @@ } void get_rplidar() { - if (IS_OK(lidar.waitPoint())) { - distances = lidar.getCurrentPoint().distance; //distance value in mm unit - angle = lidar.getCurrentPoint().angle; //anglue value in degree - startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan - quality = lidar.getCurrentPoint().quality; //quality of the current measurement - PC.printf("Distance = %.2f cm\n",distances/10); - wait_ms(100); - } else { - // analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor -// rplidar_response_device_info_t info; - // if (IS_OK(lidar.getDeviceInfo(info, 100))) { + if (IS_OK(lidar.waitPoint())) + { + + } + else + { + lidar.startScan(); - // motor=1; - // start motor rotating at max allowed speed - // analogWrite(RPLIDAR_MOTOR, 255); - //delay(1000); - // } + } } @@ -237,7 +229,7 @@ while(1) { - get_rplidar(); + // get_rplidar(); RC(); //wait_ms(1); } @@ -268,55 +260,74 @@ } case SET_VELOCITY_LEFT: { // - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - VL=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + VL=Int+(flo/10000); PC.printf("VL=%f /n",VL); break; } case SET_VELOCITY_RIGHT: { // - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - VR=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + VR=Int+flo; + PC.printf("VL=%f /n",VR); break; } case SET_VELOCITY_MAX_LEFT: { // - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - VLmax=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + VLmax=Int+flo; + PC.printf("VL=%f /n",VLmax); break; } case SET_VELOCITY_MAX_RIGHT: { // - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - VRmax=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + VRmax=Int+flo; PC.printf("VRmax = %f",VRmax); - PC.printf("*****************************"); + // PC.printf("*****************************"); break; } //save to rom case SET_KP_LEFT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KP_LEFT=Int/1000; + 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); @@ -324,12 +335,17 @@ break; } case SET_KI_LEFT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KI_LEFT=Int/1000; + 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); @@ -337,12 +353,16 @@ break; } case SET_KD_LEFT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KD_LEFT=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + KD_LEFT=Int; + 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); @@ -350,12 +370,16 @@ break; } case SET_KP_RIGHT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KP_RIGHT=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + KP_RIGHT=Int; + 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); @@ -363,12 +387,16 @@ break; } case SET_KI_RIGHT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KI_RIGHT=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + KI_RIGHT=Int; + 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); @@ -376,12 +404,16 @@ break; } case SET_KD_RIGHT: { - uint8_t int_buffer[2]; - float Int; + 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[1]=command[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); - KD_RIGHT=Int/1000; + flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer); + KD_RIGHT=Int; + 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); @@ -393,10 +425,152 @@ case READ_DATA: { switch (command[0]) { case GET_LIDAR: { + int i=0,j=1,k=0; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + while(k<60) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); + + + + + + break; + } + case GET_LIDAR2: { + int i=0,j=1,k=60; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + + while(k<120) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); break; } + case GET_LIDAR3: { + int i=0,j=1,k=120; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + while(k<180) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); + + break; + } + case GET_LIDAR4: { + int i=0,j=1,k=180; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + while(k<240) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); + + break; + } + case GET_LIDAR5: { + int i=0,j=1,k=240; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + while(k<300) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); + + break; + } + case GET_LIDAR6: { + int i=0,j=1,k=300; + uint8_t intData[2],floatData[2]; + ANDANTE_PROTOCOL_PACKET package; + //BUFFER_SIZE=143 + package.robotId = MY_ID; + package.length = 122; + package.instructionErrorId = WRITE_DATA; + while(k<360) + { + com.FloatSep( lidar.Data[k],intData,floatData); + package.parameter[i]=intData[0]; + package.parameter[j]=intData[1]; + i=i+2; + j=j+2; + k++; + } + rs485_dirc1=1; + wait_us(RS485_DELAY); + com1->sendCommunicatePacket(&package); + + break; + } + case GET_BATTERY: { break; @@ -404,8 +578,6 @@ case GET_VELOCITY_LEFT: { uint8_t intVelo_L[2],floatVelo_L[2]; com.FloatSep(valocity1,intVelo_L,floatVelo_L); - - ANDANTE_PROTOCOL_PACKET package; package.robotId = MY_ID;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/recive.h Sun Jun 05 09:43:40 2016 +0000 @@ -0,0 +1,2 @@ +#ifndef RECIVE_H +#define RECIVE_H \ No newline at end of file
--- a/rplidar/RPlidar.cpp Tue May 24 10:33:21 2016 +0000 +++ b/rplidar/RPlidar.cpp Sun Jun 05 09:43:40 2016 +0000 @@ -269,6 +269,12 @@ _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; + + + if(ang>=angMin&&ang<=angMax)Data[ang] = dis; + return RESULT_OK; } @@ -279,7 +285,10 @@ } - +void RPLidar::setAngle(int min,int max){ + angMin = min; + angMax = max; +} u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) {
--- a/rplidar/rplidar.h Tue May 24 10:33:21 2016 +0000 +++ b/rplidar/rplidar.h Sun Jun 05 09:43:40 2016 +0000 @@ -79,6 +79,10 @@ u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize); // retrieve currently received sample point + int Data[360]; + int ang,dis; + int angMin,angMax; + void setAngle(int min,int max); const RPLidarMeasurement & getCurrentPoint() { return _currentMeasurement;