Betago
v1
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of
CleaningMachine_Betago
by 
palm and chin
Diff: main.cpp
- Revision:
- 5:fe76f3dae81e
- Parent:
- 4:de5a65c17664
--- 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];