![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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-03-21
- Revision:
- 1:45f1573d65a1
- Parent:
- 0:84f05cd2f197
- Child:
- 2:f873deba2305
File content as of revision 1:45f1573d65a1:
//*****************************************************/ // 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 "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); DigitalOut rs485_dirc1(RS485_DIRC); Timer timerStart; Timeout time_getsensor; Timeout time_distance; Timeout shutdown; move m1; //*****************************************************/ //--PID parameter-- //-Upper-// float U_Kc=0.2; float U_Ki_gain=0.0; float U_Kd_gain=0.0; float U_Ti=0.0; float U_Td=0.0; float U_Ki=U_Kc*U_Ti; float U_Kd=U_Kc*U_Td; //-lower-// float L_Kc=0.2; float L_Ki_gain=0.0; float L_Kd_gain=0.0; float L_Ti=0.0; float L_Td=0.0; float L_Ki=L_Kc*L_Ti; float L_Kd=L_Kc*L_Td; //*****************************************************/ // 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,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ; 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; 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); //-- encoder -- //-- Motor -- //int dir; //Motor Upper(PWM_LU,A_LU,B_LU); //Motor Lower(PWM_LL,A_LL,B_LL); //-- PID -- //int Upper_SetPoint=20; //int Lower_SetPoint=8; //PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate //PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); //PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate //PID Low_PID(L_Kc, L_Ti, L_Td); //*****************************************************/ //void Start_Up(); void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); //Timer counterUP; //Timer counterLOW; 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; } // pc.printf("%d",Encoderpos); } 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); //ส่งข้อมูล } 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(1,setp2,outPID); } /* void Read_Encoder(PinName Encoder) { ENC.format(8,0); ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k if(Encoder == EncoderA) { EncA = 0; } else { EncB = 0; } ENC.write(0x41); ENC.write(0x09); data = ENC.write(0x00); if(Encoder == EncoderA) { EncA = 1; } else { EncB = 1; } } //**************************************************** int Get_EnValue(int Val) { int i = 0; static unsigned char codes[] = { 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; if ( MY_ID == 0x01 ) { //when it was left side while(Val != codes[i]) { i++; } } if ( MY_ID == 0x02 ) { //when it was right side while(Val != codes[127-i]) { i++; } } return i; } //**************************************************** void SET_UpperPID() { Upper.period(0.001); memory.read(ADDRESS_UP_MARGIN,UpMargin); Up_PID.setMargin(UpMargin); memory.read(ADDRESS_UPPER_KP,U_Kc); Up_PID.setKp(U_Kc); memory.read(ADDRESS_UPPER_KI,U_Ki_gain); Up_PID.setKi(U_Ki_gain); memory.read(ADDRESS_UPPER_KD,U_Kd_gain); Up_PID.setKd(U_Kd_gain); //Up_PID.setMode(0); //Up_PID.setInputLimits(18,62); //Up_PID.setOutputLimits(0,1); } //******************************************************/ /* void SET_LowerPID() { Lower.period(0.001); memory.read(ADDRESS_LOW_MARGIN,LowMargin); Low_PID.setMargin(LowMargin); memory.read(ADDRESS_LOWER_KP,L_Kc); Low_PID.setKp(L_Kc); memory.read(ADDRESS_LOWER_KI,L_Ki_gain); Low_PID.setKi(L_Ki_gain); memory.read(ADDRESS_LOWER_KD,L_Kd_gain); Low_PID.setKd(L_Kd_gain); //Low_PID.setMode(0); //Low_PID.setInputLimits(0,127); // set range //Low_PID.setOutputLimits(0,1); } //******************************************************/ /* void Move_Upper() { Read_Encoder(EncoderA); Upper_Position = (float)Get_EnValue(data); #ifdef DEBUG_UP printf("read_encode = 0x%2x \n\r",data); printf("Setpoint = %d\n\r",Upper_SetPoint); printf("Upper_Position = %f\n\r",Upper_Position); #endif Up_PID.setCurrent(Upper_Position); float speed =Up_PID.compute(); #ifdef DEBUG_UP printf("E_n= %f\n\r",Up_PID.getErrorNow()); printf("Kp = %f\n\r",Up_PID.getKp()); printf("speed = %f\n\n\n\r",speed); #endif Upper.speed(speed); } //******************************************************/ /* void Move_Lower() { Read_Encoder(EncoderB); Lower_Position = (float)Get_EnValue(data); Low_PID.setCurrent(Lower_Position); #ifdef DEBUG_LOW printf("read_encode = 0x%2x \n\r",data); printf("Setpoint = %d\n\r",Lower_SetPoint); printf("Upper_Position = %f\n\r",Lower_Position); #endif float speed =Low_PID.compute(); #ifdef DEBUG_LOW printf("E_n= %f\n\r",Low_PID.getErrorNow()); printf("Kp = %f\n\r",Low_PID.getKp()); printf("speed = %f\n\n\n\r",speed); #endif Lower.speed(speed); } //******************************************************/ 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); printf("******************"); /* while(1) { Read_Encoder(EncoderA); Upper_Position = Get_EnValue(data); Read_Encoder(EncoderB); Lower_Position = Get_EnValue(data); PC.printf("Upper Position : %f\n",Upper_Position); PC.printf("Lower_Position : %f\n",Lower_Position); wait(0.5); } */ //Recieve.attach(&Rc,0.025); //Start_Up(); //SET_UpperPID(); // SET_LowerPID(); // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID); /* while(1) { Upper.speed(-1); wait_ms(400); Upper.speed(1); wait_ms(400); Upper.break(); Lower.speed(-1.0); wait_ms(401); Lower.speed(1.0); wait_ms(401); Lower.break(); } */ // counterUP.start(); // counterLOW.start(); while(1) { /* //printf("timer = %d\n\r",counter.read_ms()); if(counterUP.read_ms() > 1400) { if(Upper_SetPoint < 53) { Upper_SetPoint++; } else Upper_SetPoint =18; counterUP.reset(); } if(counterLOW.read_ms() > 700) { if(Lower_SetPoint < 100) { Lower_SetPoint++; } else Lower_SetPoint =8; counterLOW.reset(); } */ // myled =1; //wait_ms(10); ///////////////////////////////////////////////////// start //Set Set_point // Up_PID.setGoal(Upper_SetPoint); // Low_PID.setGoal(Lower_SetPoint); //Control Motor // Move_Upper(); // Move_Lower(); ///////////////////////////////////////////////////// stop =306us //uint8_t aaa[1]={10}; //com.sendBodyWidth(0x01,aaa); 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; } 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; 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; 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; 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; 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; 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; 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: { uint8_t intKPL[2],floatKPL[2]; com.FloatSep(KP_LEFT,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: { uint8_t intKIL[2],floatKIL[2]; com.FloatSep(KI_LEFT,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: { uint8_t intKDL[2],floatKDL[2]; com.FloatSep(KD_LEFT,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: { uint8_t intKDR[2],floatKDR[2]; com.FloatSep(KP_RIGHT,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: { uint8_t intKIR[2],floatKIR[2]; com.FloatSep(KI_RIGHT,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: { uint8_t intKDR[2],floatKDR[2]; com.FloatSep(KD_RIGHT,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; } } }