สัสชิน
Dependencies: BEAR_Protocol_Edited_V22 BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V2 by
Diff: main.cpp
- Revision:
- 1:45f1573d65a1
- Parent:
- 0:84f05cd2f197
- Child:
- 2:f873deba2305
--- a/main.cpp Mon Feb 15 17:48:23 2016 +0000 +++ b/main.cpp Mon Mar 21 20:21:12 2016 +0000 @@ -1,63 +1,750 @@ -#include "Debug.h" -#include "UI.h" +//*****************************************************/ +// Include // +#include "mbed.h" #include "pinconfig.h" -#include "BEAR_Protocol.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); + -DigitalIn button(USER_BUTTON); -Serial pc(SERIAL_TX,SERIAL_RX); +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; + } -void DebugMode(); +} +//**************************************************** +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() { - if(!button) { - while(!button); - DebugMode(); //-->Debug.h + 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); } - - UI ui(SW_WALK1,SW_WALK2,SW_WALK3,SW_SWEEP,SW_WATER,EMERGENCY); - - pc.printf("System Start\n"); + */ + + + //Recieve.attach(&Rc,0.025); + //Start_Up(); + + //SET_UpperPID(); + // SET_LowerPID(); + + // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID); + /* while(1) { - while(!ui.getEmergencyStatus()) { - ui.RunSystem(); + + 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 DebugMode() -{ - float temp; - int option; - bool first_time = true; - Debug debug(SERIAL_TX,SERIAL_RX); - do { - debug.PrintListMode(); - option = debug.ScanInputData(1); + + - if(option == 1) { - do { - temp = debug.Mode1(); - debug.PrintAll(temp); - } while(temp!=9999); - } + - else if(option == 2) { - do { - if(first_time==false) { - temp = debug.Mode2(); - debug.PrintAll(temp); - } else { - temp = debug.Mode2(); - if(temp!=0) first_time = false; - } - if(temp==9999) first_time = true; - } while(temp!=9999); - } - debug.setChange(); - } while(option!=9999); -} \ No newline at end of file +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; + + } + } +}