55+
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of CleaningMachine_Betago by
main.cpp
- Committer:
- palmdotax
- Date:
- 2016-05-24
- Revision:
- 3:edaab92dbd2f
- Parent:
- 2:f873deba2305
File content as of revision 3:edaab92dbd2f:
//*****************************************************/ // Include // #include "mbed.h" #include "pinconfig.h" #include "PID.h" //#include "Motor.h" #include "eeprom.h" #include "Receiver.h" #include "Motion_EEPROM_Address.h" #include "move.h" #include "UNTRASONIC.h" #include "BufferedSerial.h" #include "rplidar.h" RPLidar lidar; //#include "pidcontrol.h" #define EEPROM_DELAY 2 DigitalOut rs485_dirc1(RS485_DIRC); //#define DEBUG_UP //#define DEBUG_LOW InterruptIn encoderA_d(PB_12); DigitalIn encoderB_d(PB_13); InterruptIn encoderA_1(PB_1); DigitalIn encoderB_1(PB_2); InterruptIn encoderA_2(PB_14); DigitalIn encoderB_2(PB_15); Timer timerStart; Timeout time_getsensor; Timeout time_distance; Timeout shutdown; move m1; //*****************************************************/ // 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; //pid double setp1=0,setp2=0; float outPID =0; float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ; PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1); PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1); //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); int16_t MY_ID = 0x00; //-- Memorry -- 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(); //rplidar float distances = 0; float angle = 0; bool startBit = 0; char quality =0 ; void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); DigitalOut myled(LED1); void Rx_interrupt() { //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);} } 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;} pulse_d+=1; if(pulse_d==128) { Z_d+=1; pulse_d=0; } } void getvelo1()//จาก encoder { valocity1=pulse_1*((2*3.14*r)/128); PC.printf("valocity=%f \n",valocity1); count=0; timerStart.reset(); } void getvelo2() { valocity2=pulse_2*((2*3.14*r)/128); PC.printf("valocity=%f \n",valocity2); count=0; timerStart.reset(); } void get_d()//ระยะทาง { real_d=Z_d*(2*3.14*r); //ส่งข้อมูล } 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))) { lidar.startScan(); // motor=1; // start motor rotating at max allowed speed // analogWrite(RPLIDAR_MOTOR, 255); //delay(1000); // } } } 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; } P1.setProcessValue(valocity1); outPID=P1.compute(); //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; } P2.setProcessValue(valocity2); outPID=P2.compute(); //pc.printf("outPID=%f \n",outPID); m1.movespeed_2(setp2,outPID); } void RC() { myled =1; uint8_t data_array[30]; uint8_t id=0; uint8_t ins=0; uint8_t status=0xFF; status = com.ReceiveCommand(&id,data_array,&ins); PC.printf("status = 0x%02x\n\r",status); if(status == ANDANTE_ERRBIT_NONE) { CmdCheck((int16_t)id,data_array,ins); PC.printf("s******************************"); } } /*******************************************************/ int main() { PC.baud(115200); lidar.begin(); printf("******************"); encoderA_1.rise(&EncoderA_1); timerStart.start(); P1.setMode(1); P1.setBias(0); // pc.printf("READY \n"); //pc.attach(&Rx_interrupt, Serial::RxIrq); while(1) { get_rplidar(); RC(); //wait_ms(1); } } void CmdCheck(int16_t id,uint8_t *command,uint8_t ins) { 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]; break; } case SET_VELOCITY_LEFT: { // uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); VL=Int/1000; PC.printf("VL=%f /n",VL); break; } case SET_VELOCITY_RIGHT: { // uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); VR=Int/1000; break; } case SET_VELOCITY_MAX_LEFT: { // uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); VLmax=Int/1000; break; } case SET_VELOCITY_MAX_RIGHT: { // uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); VRmax=Int/1000; PC.printf("VRmax = %f",VRmax); PC.printf("*****************************"); break; } //save to rom case SET_KP_LEFT: { uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KP_LEFT=Int/1000; 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 Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KI_LEFT=Int/1000; 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: { uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KD_LEFT=Int/1000; 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: { uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KP_RIGHT=Int/1000; 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 Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KI_RIGHT=Int/1000; 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: { uint8_t int_buffer[2]; float Int; int_buffer[0]=command[1]; int_buffer[1]=command[2]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); KD_RIGHT=Int/1000; 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: { switch (command[0]) { case GET_LIDAR: { 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; package.robotId = MY_ID; 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[3]=floatVelo_L[1]; 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; package.robotId = MY_ID; 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[3]=floatVelo_R[1]; 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); ANDANTE_PROTOCOL_PACKET package; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKPL[0]; package.parameter[1]=intKPL[1]; package.parameter[2]=floatKPL[0]; package.parameter[3]=floatKPL[1]; rs485_dirc1=1; 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; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKIL[0]; package.parameter[1]=intKIL[1]; package.parameter[2]=floatKIL[0]; package.parameter[3]=floatKIL[1]; 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; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKDL[0]; package.parameter[1]=intKDL[1]; package.parameter[2]=floatKDL[0]; package.parameter[3]=floatKDL[1]; 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; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKDR[0]; package.parameter[1]=intKDR[1]; package.parameter[2]=floatKDR[0]; package.parameter[3]=floatKDR[1]; 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; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKIR[0]; package.parameter[1]=intKIR[1]; package.parameter[2]=floatKIR[0]; package.parameter[3]=floatKIR[1]; 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; package.robotId = MY_ID; package.length = 6; package.instructionErrorId = WRITE_DATA; package.parameter[0]=intKDR[0]; package.parameter[1]=intKDR[1]; package.parameter[2]=floatKDR[0]; package.parameter[3]=floatKDR[1]; rs485_dirc1=1; wait_us(RS485_DELAY); com1->sendCommunicatePacket(&package); break; } } }break; } } }