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:
- 4:de5a65c17664
- Parent:
- 3:edaab92dbd2f
- Child:
- 5:fe76f3dae81e
--- 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;