Secret_Betago
สัสชิน
Dependencies: BEAR_Protocol_Edited_V22 BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Revision 5:fe76f3dae81e, committed 2016-06-07
- Comitter:
- palmdotax
- Date:
- Tue Jun 07 03:26:08 2016 +0000
- Parent:
- 4:de5a65c17664
- Child:
- 6:adf1f4351f9f
- Commit message:
- v2;
Changed in this revision
--- a/BEAR_Protocol_Edited.lib Sun Jun 05 09:43:40 2016 +0000 +++ b/BEAR_Protocol_Edited.lib Tue Jun 07 03:26:08 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/palm-and-chin/code/BEAR_Protocol_Edited/#168de1acec53 +https://developer.mbed.org/users/palmdotax/code/BEAR_Protocol_Edited/#1b64ff047f68
--- a/iSerial.lib Sun Jun 05 09:43:40 2016 +0000 +++ b/iSerial.lib Tue Jun 07 03:26:08 2016 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/BEaR-lab/code/iSerial/#1188a5024611 +http://developer.mbed.org/teams/BEaR-lab/code/iSerial/#3ffc21af1595
--- a/main.cpp Sun Jun 05 09:43:40 2016 +0000
+++ b/main.cpp Tue Jun 07 03:26:08 2016 +0000
@@ -13,12 +13,12 @@
#include "rplidar.h"
RPLidar lidar;
//#include "pidcontrol.h"
-
+BufferedSerial se_lidar(PA_11,PA_12);
#define EEPROM_DELAY 2
DigitalOut rs485_dirc1(RS485_DIRC);
//#define DEBUG_UP
//#define DEBUG_LOW
-
+PwmOut VMO(PC_8);
InterruptIn encoderA_d(PB_12);
DigitalIn encoderB_d(PB_13);
InterruptIn encoderA_1(PB_1);
@@ -26,6 +26,7 @@
InterruptIn encoderA_2(PB_14);
DigitalIn encoderB_2(PB_15);
Timer timerStart;
+Timer tim;
Timeout time_getsensor;
Timeout time_distance;
Timeout shutdown;
@@ -50,7 +51,8 @@
PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
//Ticker Recieve;
//-- Communication --
-COMMUNICATION *com1;
+COMMUNICATION *com1;
+
//BufferedSerial PC(SERIAL_TX,SERIAL_RX);
Serial PC(SERIAL_TX,SERIAL_RX);
Bear_Receiver com(PA_9,PA_10,115200);
@@ -207,29 +209,35 @@
PC.printf("status = 0x%02x\n\r",status);
if(status == ANDANTE_ERRBIT_NONE) {
CmdCheck((int16_t)id,data_array,ins);
- PC.printf("s******************************");
+ PC.printf("RC******************************");
}
}
/*******************************************************/
+int buf=0;
int main()
{
PC.baud(115200);
- lidar.begin();
- printf("******************");
-
-
-
+ lidar.begin(se_lidar);
+ 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");
//pc.attach(&Rx_interrupt, Serial::RxIrq);
+ lidar.setAngle(0,360);
while(1) {
-
- // get_rplidar();
+ VMO=1;
+ get_rplidar();
+ /* 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);
}
@@ -361,7 +369,7 @@
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KD_LEFT=Int;
+ KD_LEFT=Int+flo;
PC.printf("KD_LEFT=%f /n",KD_LEFT);
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
@@ -378,7 +386,7 @@
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KP_RIGHT=Int;
+ KP_RIGHT=Int+flo;
PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
@@ -395,7 +403,7 @@
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KI_RIGHT=Int;
+ KI_RIGHT=Int+flo;
PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
@@ -412,7 +420,7 @@
float_buffer[1]=command[4];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
- KD_RIGHT=Int;
+ KD_RIGHT=Int+flo;
PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
@@ -422,19 +430,19 @@
}
}
} break;
- case READ_DATA: {
+ case READ_DATA: { PC.printf("READ_DATA\n");
switch (command[0]) {
case GET_LIDAR: {
- int i=0,j=1,k=0;
- uint8_t intData[2],floatData[2];
+ /* int i=0,j=1,k=0;
+ uint8_t intData[2]={0x00,0x01},floatData[2];
ANDANTE_PROTOCOL_PACKET package;
//BUFFER_SIZE=143
package.robotId = MY_ID;
- package.length = 122;
+ package.length = 4;//122
package.instructionErrorId = WRITE_DATA;
-
+ PC.printf("GETDATA\n");
while(k<60)
- {
+ { PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
@@ -443,14 +451,19 @@
k++;
}
- rs485_dirc1=1;
+ // PC.printf("SEND\n");
+ //rs485_dirc1=1;
wait_us(RS485_DELAY);
+ PC.printf("SEND2\n");
com1->sendCommunicatePacket(&package);
+ PC.printf("SEND3\n");
-
+ */
+ com.sendlidar();
+ PC.printf("SEND2\n");
break;
}
case GET_LIDAR2: {
@@ -463,7 +476,7 @@
package.instructionErrorId = WRITE_DATA;
while(k<120)
- {
+ {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
@@ -487,7 +500,7 @@
package.length = 122;
package.instructionErrorId = WRITE_DATA;
while(k<180)
- {
+ {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
@@ -510,7 +523,7 @@
package.length = 122;
package.instructionErrorId = WRITE_DATA;
while(k<240)
- {
+ {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
@@ -533,7 +546,7 @@
package.length = 122;
package.instructionErrorId = WRITE_DATA;
while(k<300)
- {
+ {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
@@ -541,6 +554,7 @@
j=j+2;
k++;
}
+
rs485_dirc1=1;
wait_us(RS485_DELAY);
com1->sendCommunicatePacket(&package);
@@ -556,7 +570,7 @@
package.length = 122;
package.instructionErrorId = WRITE_DATA;
while(k<360)
- {
+ {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
com.FloatSep( lidar.Data[k],intData,floatData);
package.parameter[i]=intData[0];
package.parameter[j]=intData[1];
--- a/rplidar/RPlidar.cpp Sun Jun 05 09:43:40 2016 +0000
+++ b/rplidar/RPlidar.cpp Tue Jun 07 03:26:08 2016 +0000
@@ -28,7 +28,7 @@
*/
#include "rplidar.h"
-BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx
+//BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx
Timer timers;
RPLidar::RPLidar()
{
@@ -60,12 +60,11 @@
_bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
}
*/
-void RPLidar::begin()
+void RPLidar::begin(BufferedSerial &serialobj)
{
-//BufferedSerial lidar_serial(PinName PA_11, PinName PA_12);
- //Serial.begin(115200);
+ _bined_serialdev = &serialobj;
timers.start();
- _bined_serialdev.baud(115200);
+ _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
}
// close the currently opened serial interface
void RPLidar::end()
@@ -119,7 +118,7 @@
}
while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
- int currentbyte = _bined_serialdev.getc();
+ int currentbyte = _bined_serialdev->getc();
if (currentbyte < 0) continue;
infobuf[recvPos++] = currentbyte;
@@ -162,7 +161,7 @@
}
while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
- int currentbyte = _bined_serialdev.getc();
+ int currentbyte = _bined_serialdev->getc();
if (currentbyte<0) continue;
infobuf[recvPos++] = currentbyte;
@@ -186,17 +185,6 @@
// start the measurement operation
u_result RPLidar::startScan(bool force, _u32 timeout)
{
- /*
- rplidar_cmd_packet_t pkt_header;
- rplidar_cmd_packet_t * header = &pkt_header;
- header->syncByte = 0xA5;
- header->cmd_flag = 0x21;
- //se.write(12);
-
- _bined_serialdev.putc(header->syncByte );
- _bined_serialdev.putc(header->cmd_flag );
- */
-
u_result ans;
// if (!isOpen()) return RESULT_OPERATION_FAIL;
@@ -236,7 +224,7 @@
_u8 recvPos = 0;
while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
- int currentbyte = _bined_serialdev.getc();
+ int currentbyte = _bined_serialdev->getc();
if (currentbyte<0) continue;
//Serial.println(currentbyte);
switch (recvPos) {
@@ -269,7 +257,7 @@
_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;
+ ang = _currentMeasurement.angle;
dis = _currentMeasurement.distance;
@@ -289,6 +277,9 @@
angMin = min;
angMax = max;
}
+void RPLidar::get_lidar(){
+ if (!IS_OK(waitPoint())) startScan();
+}
u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
{
@@ -304,8 +295,8 @@
header->cmd_flag = cmd;
// send header first
- _bined_serialdev.putc(header->syncByte );
- _bined_serialdev.putc(header->cmd_flag );
+ _bined_serialdev->putc(header->syncByte );
+ _bined_serialdev->putc(header->cmd_flag );
// _bined_serialdev->write( (uint8_t *)header, 2);
@@ -321,7 +312,7 @@
// send size
_u8 sizebyte = payloadsize;
- _bined_serialdev.putc(sizebyte);
+ _bined_serialdev->putc(sizebyte);
// _bined_serialdev->write((uint8_t *)&sizebyte, 1);
// send payload
@@ -329,7 +320,7 @@
// _bined_serialdev->write((uint8_t *)&payload, sizebyte);
// send checksum
- _bined_serialdev.putc(checksum);
+ _bined_serialdev->putc(checksum);
// _bined_serialdev->write((uint8_t *)&checksum, 1);
}
@@ -345,7 +336,7 @@
_u8 *headerbuf = (_u8*)header;
while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
- int currentbyte = _bined_serialdev.getc();
+ int currentbyte = _bined_serialdev->getc();
if (currentbyte<0) continue;
switch (recvPos) {
case 0:
--- a/rplidar/rplidar.h Sun Jun 05 09:43:40 2016 +0000
+++ b/rplidar/rplidar.h Tue Jun 07 03:26:08 2016 +0000
@@ -26,13 +26,11 @@
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
-#ifndef RPLIDAR_H
-#define RPLIDAR_H
#pragma once
#include "mbed.h"
-#include "rptypes.h"
-#include "rplidar_cmd.h"
+#include "inc/rptypes.h"
+#include "inc/rplidar_cmd.h"
#include "../BufferedSerial/BufferedSerial.h"
struct RPLidarMeasurement
{
@@ -55,7 +53,7 @@
// open the given serial interface and try to connect to the RPLIDAR
//bool begin(BufferedSerial &serialobj);
- void begin();
+ void begin(BufferedSerial &serialobj);
// close the currently opened serial interface
void end();
@@ -76,13 +74,13 @@
// wait for one sample point to arrive
u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
-u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
+ 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);
+ void get_lidar();
const RPLidarMeasurement & getCurrentPoint()
{
return _currentMeasurement;
@@ -93,9 +91,6 @@
u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);
protected:
-
- // BufferedSerial * _bined_serialdev;
-
+ BufferedSerial * _bined_serialdev;
RPLidarMeasurement _currentMeasurement;
};
-#endif