Arsapol Manamunchaiyaporn
before test
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Diff: main.cpp
- Revision:
- 7:0dac9d4ff04f
- Parent:
- 5:fe76f3dae81e
--- a/main.cpp Tue Jun 07 03:28:39 2016 +0000
+++ b/main.cpp Tue Jun 07 16:17:34 2016 +0000
@@ -35,13 +35,13 @@
// 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 +61,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 +82,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 +141,55 @@
{
real_d=Z_d*(2*3.14*r);
//ส่งข้อมูล
-
+
}
void get_rplidar()
{
- if (IS_OK(lidar.waitPoint()))
- {
-
- }
- else
- {
-
- lidar.startScan();
-
+ if (IS_OK(lidar.waitPoint())) {
+
+ } else {
+
+ 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,29 +213,30 @@
}
/*******************************************************/
int buf=0;
+float lidar_angle_max=360,lidar_angle_min=0;
int main()
{
PC.baud(115200);
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");
+ 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);
+ lidar.setAngle(lidar_angle_min,lidar_angle_max);
while(1) {
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();
- }*/
+ /* 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);
}
@@ -253,21 +252,38 @@
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;
}
case WRITE_DATA: {
switch (command[0]) {
- case ID: {
- ///
- MY_ID = (int16_t)command[1];
+ case SET_MAX_LIDAR_DEGREE: {
+ uint8_t int_buffer[2],float_buffer[2];
+ uint8_t int_buffer2[2],float_buffer2[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_buffer2[0]=command[5];
+ int_buffer2[1]=command[6];
+ float_buffer2[0]=command[7];
+ float_buffer2[1]=command[8];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
+ lidar_angle_max=Int+(flo/10000);
+
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer2);
+ flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer2);
+ lidar_angle_max=Int+(flo/10000);
+ PC.printf("Min,Max Degree of Lidar : %f %f/n",lidar_angle_min,lidar_angle_max);
break;
}
+
case SET_VELOCITY_LEFT: {
- //
uint8_t int_buffer[2],float_buffer[2];
float Int,flo;
int_buffer[0]=command[1];
@@ -317,10 +333,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 +345,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 +381,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 +398,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,352 +435,292 @@
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: {
- /* 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 = 4;//122
+ /*case GET_LIDAR1: {
+ uint8_t IntArray[2],FloatArray[2];
+ int numberK=0;
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = ID;
+ package.length = 362;
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];
- i=i+2;
- j=j+2;
- k++;
-
+
+ for(int i=1; i<=360; i++) {
+ if(i==2) {
+ com.FloatSep(lidar.Data[numberK],IntArray,FloatArray);
+ package.parameter[i-1]=IntArray[0];
+ package.parameter[i]=IntArray[1];
+ numberK++;
+ }
}
- // 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");
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
break;
- }
- case GET_LIDAR2: {
+ }*/
+ 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)
- {//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];
- 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;
+ uint8_t intData[2],floatData[2];
+ ANDANTE_PROTOCOL_PACKET package;
//BUFFER_SIZE=143
package.robotId = MY_ID;
- package.length = 122;
+ 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];
- 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)
- {//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];
- i=i+2;
- j=j+2;
- k++;
- }
- rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
- break;
+
+ 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];
+ i=i+2;
+ j=j+2;
+ k++;
+
}
- 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)
- {//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];
- 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)
- {//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];
- i=i+2;
- j=j+2;
- k++;
- }
- rs485_dirc1=1;
- wait_us(RS485_DELAY);
- com1->sendCommunicatePacket(&package);
-
- break;
- }
-
+ ////rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ 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);
-
-
+ ////rs485_dirc1=1;
+ 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);
-
+ ////rs485_dirc1=1;
+ 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);
+ memory.read(ADDRESS_LEFT_KP ,KP_LEFT);
+ memory.read(ADDRESS_LEFT_KP ,KI_LEFT);
+ memory.read(ADDRESS_LEFT_KP ,KD_LEFT);
+
+ com.sendKpKiKdLeft(KP_LEFT,KI_LEFT,KD_LEFT);
-
- ANDANTE_PROTOCOL_PACKET package;
+ /*uint8_t IntKp[2],FloatKp[2];
+ uint8_t IntKi[2],FloatKi[2];
+ uint8_t IntKd[2],FloatKd[2];
+
+ com.FloatSep(KP_LEFT_BUFF,IntKp,FloatKp);
+ com.FloatSep(KI_LEFT_BUFF,IntKi,FloatKi);
+ com.FloatSep(KD_LEFT_BUFF,IntKd,FloatKd);
+
- package.robotId = MY_ID;
- package.length = 6;
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = ID;
+ package.length = 14;
package.instructionErrorId = WRITE_DATA;
- package.parameter[0]=intKPL[0];
- package.parameter[1]=intKPL[1];
- package.parameter[2]=floatKPL[0];
- package.parameter[3]=floatKPL[1];
+ package.parameter[0]=IntKp[0];
+ package.parameter[1]=IntKp[1];
+ package.parameter[2]=FloatKp[0];
+ package.parameter[3]=FloatKp[1];
+ package.parameter[4]=IntKi[0];
+ package.parameter[5]=IntKi[1];
+ package.parameter[6]=FloatKi[0];
+ package.parameter[7]=FloatKi[1];
+ package.parameter[8]=IntKd[0];
+ package.parameter[9]=IntKd[1];
+ package.parameter[10]=FloatKd[0];
+ package.parameter[11]=FloatKd[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);
-
+ ////rs485_dirc1=1;
+ 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);
-
+ ////rs485_dirc1=1;
+ 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);
-
+ ////rs485_dirc1=1;
+ 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);
-
+ ////rs485_dirc1=1;
+ 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);
-
+ ////rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
break;
- }
- }
- }break;
-
+ }
+ case GET_MAX_LIDAR_DEGREE: {
+ com.sendMaxMinLidarDegree(lidar_angle_max,lidar_angle_min);
+ /*uint8_t intMax[2],floatMax[2];
+ uint8_t intMin[2],floatMin[2];
+ com.FloatSep(lidar_angle_max,intMax,floatMax);
+ com.FloatSep(lidar_angle_min,intMin,floatMin);
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 10;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intMax[0];
+ package.parameter[1]=intMax[1];
+ package.parameter[2]=floatMax[0];
+ package.parameter[3]=floatMax[1];
+ package.parameter[4]=intMin[0];
+ package.parameter[5]=intMin[1];
+ package.parameter[6]=floatMin[0];
+ package.parameter[7]=floatMin[1];
+
+ ////rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);*/
+ }
+ case GET_LIDAR1: {
+ com.sendSensorDataAll1(lidar.Data);
+ }
+ }
+ }
+ break;
+
+ }
}
- }
}