base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
main.cpp
- Committer:
- rizqicahyo
- Date:
- 2016-04-26
- Revision:
- 3:43d4cb3ece1b
- Parent:
- 2:df6c49846367
- Child:
- 4:7a7a8aa33fd5
File content as of revision 3:43d4cb3ece1b:
/*********************************************************************************************/ /** FILE HEADER **/ /*********************************************************************************************/ #include "mbed.h" #include "Motor.h" #include "PS_PAD.h" #include "esc.h" #include "Servo.h" /*********************************************************************************************/ /** DEKLARASI INPUT OUTPUT **/ /*********************************************************************************************/ //serial Serial pc(USBTX,USBRX); //debug Serial com(PA_0,PA_1); //sensor //joystick PS2 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) //motor (PWM, forward, reverse) Motor motor1(PA_8, PB_0, PC_15); Motor motor2(PA_11, PA_6, PA_5); Motor motor3(PA_9, PC_2, PC_3); Motor motor4(PA_10, PB_5, PB_4); Motor motorS(PB_7, PA_14, PA_15); Motor motorC1(PB_9, PA_12, PC_5); Motor motorC2(PB_8, PB_1, PA_13); //Motor motor4(PB_6, PA_7, PB_12); //pnuematik DigitalOut pnuematik1(PC_11); DigitalOut pnuematik2(PC_10); DigitalOut pnuematik3(PD_2); DigitalOut pnuematik4(PC_12); //Limit Switch DigitalIn limit1(PC_13 ,PullUp); DigitalIn limit2(PC_14 ,PullUp); DigitalIn limit3(PC_1 ,PullUp); DigitalIn limit4(PC_0 ,PullUp); DigitalIn field(PB_10 ,PullUp); //laser pointer DigitalOut laser(PB_3); //servo(PWM) Servo servoEDF(PC_8); //EDF(PWM, timer) ESC edf(PC_6,20); //Timer Ticker timer; /*********************************************************************************************/ /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/ /*********************************************************************************************/ const float gMax_speed = 0.85; //const float gMin_speed = 0.1; const float v0 = 0.4; const float ax = 0.0007; float vcurr = v0; const float tuning1 = 0.0; const float tuning2 = 0.04; const float tuning3 = 0.0; const float tuning4 = 0.04; const float driver0 = 1; const float driver1 = 0.85; const float driver2 = 0.65; const float driver3 = 0.2; // inisialisasi pwm awal servo double pwm = 0.00; // inisialisasi sudut awal double sudut = -85; // variabel kondisi pnuematik int g = 1; //slider auto int c =0; int batas_delay = 270; //data sensor garis dan line following int datasensor[6]; int over; int g_flag; /////// void aktuator(int f); void edf_servo(); void init_sensor(); void linetracer(float speed); void flag_sensor(); /*********************************************************************************************/ /*********************************************************************************************/ /** PROGRAM UTAMA **/ /*********************************************************************************************/ /*********************************************************************************************/ int main() { //inisiasi serial pc.baud(115200); com.baud(115200); if(field == 1) sudut = -85; else sudut = 85; //inisiasi joystick ps2.init(); //set inisiasi servo pada posisi 0 servoEDF.position(sudut); //set edf pada posisi bukan kalibrasi, yaitu set edf 0 edf.setThrottle(0); edf.pulse(); //inisisasi laser laser = 1; //inisiasi pnuematik pnuematik1 = 1; pnuematik2 = 1; pnuematik3 = 0; pnuematik4 = 0; //inisisasi TIMER timer.attach(&flag_sensor,0.0005); timer.detach(); //kondisi robot bool manual=true; bool pool= false; while(manual){ ps2.poll(); aktuator(field); edf_servo(); if(ps2.read(PS_PAD::ANALOG_LEFT)==1) manual = false; } timer.attach(&flag_sensor,0.0005); while(!pool){ init_sensor(); if(vcurr > 0.3) vcurr = (float) 0.3; linetracer(vcurr); //laser = 1; vcurr+=ax; if(limit3==0) pool=true; } motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); timer.detach(); pnuematik1=0; wait_ms(1500); while(limit4!=0){ motorC1.speed(1); motorC2.speed(-1); } motorC1.brake(1); motorC2.brake(1); if(field==1){ pnuematik3 = 0; wait_ms(1500); pnuematik2 = 1; wait_ms(500); pnuematik3 = 1; } else{ pnuematik4 = 0; wait_ms(1500); pnuematik2 = 1; wait_ms(500); pnuematik4 = 1; } return 0; } /*********************************************************************************************/ /** ALGORITMA PROSEDUR DAN FUNGSI **/ /*********************************************************************************************/ void aktuator(int f){ float speed = vcurr; if(vcurr >= gMax_speed) vcurr = gMax_speed; if(f == 1){ if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ //pivot kiri motor2.speed((float)0.5*(speed-tuning2)); motor4.speed((float)-0.5*(speed-tuning4)); motor1.speed((float)0.5*(speed-tuning1)); motor3.speed((float)-0.5*(speed-tuning3)); pc.printf("pivot kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ //pivot kanan motor2.speed((float)-0.5*(speed-tuning2)); motor4.speed((float)0.5*(speed-tuning4)); motor1.speed((float)-0.5*(speed-tuning1)); motor3.speed((float)0.5*(speed-tuning3)); pc.printf("pivot kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){ //serong atas kanan motor2.speed(speed-tuning2); motor4.brake(1); motor1.brake(1); motor3.speed(speed-tuning3); pc.printf("serong atas kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //serong atas kiri motor2.brake(1); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.brake(1); pc.printf("serong atas kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ //serong bawah kiri motor2.speed(-(speed-tuning2)); motor4.brake(1); motor1.brake(1); motor3.speed(-(speed-tuning3)); pc.printf("serong bawah kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){ //serong bawah kanan motor2.brake(1); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.brake(1); pc.printf("serong bawah kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ //maju motor1.speed(-(speed-tuning1)); motor3.speed(speed-tuning3); motor2.speed(speed-tuning2); motor4.speed(-(speed-tuning4)); pc.printf("maju \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){ //mundur motor1.speed(speed-tuning1); motor3.speed(-(speed-tuning3)); motor2.speed(-(speed-tuning2)); motor4.speed(speed-tuning4); pc.printf("mundur \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){ //kanan motor2.speed(speed-tuning2); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.speed(speed-tuning3); pc.printf("kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //kiri motor2.speed(-(speed-tuning2)); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.speed(-(speed-tuning3)); pc.printf("kiri \n"); vcurr += ax; } else{ motor1.brake(1); motor3.brake(1); motor2.brake(1); motor4.brake(1); pc.printf("diam \n"); vcurr = v0; } } else{ if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ //pivot kiri motor2.speed((float)0.5*(speed-tuning2)); motor4.speed((float)-0.5*(speed-tuning4)); motor1.speed((float)0.5*(speed-tuning1)); motor3.speed((float)-0.5*(speed-tuning3)); pc.printf("pivot kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ //pivot kanan motor2.speed((float)-0.5*(speed-tuning2)); motor4.speed((float)0.5*(speed-tuning4)); motor1.speed((float)-0.5*(speed-tuning1)); motor3.speed((float)0.5*(speed-tuning3)); pc.printf("pivot kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){ //serong atas kanan motor2.speed(-(speed-tuning2)); motor4.brake(1); motor1.brake(1); motor3.speed(-(speed-tuning3)); pc.printf("serong atas kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //serong atas kiri motor2.brake(1); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.brake(1); pc.printf("serong atas kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ //serong bawah kiri motor2.speed(speed-tuning2); motor4.brake(1); motor1.brake(1); motor3.speed(speed-tuning3); pc.printf("serong bawah kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){ //serong bawah kanan motor2.brake(1); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.brake(1); pc.printf("serong bawah kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ //maju motor1.speed(speed-tuning1); motor3.speed(-(speed-tuning3)); motor2.speed(-(speed-tuning2)); motor4.speed(speed-tuning4); pc.printf("maju \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){ //mundur motor1.speed(-(speed-tuning1)); motor3.speed(speed-tuning3); motor2.speed(speed-tuning2); motor4.speed(-(speed-tuning4)); pc.printf("mundur \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){ //kanan motor2.speed(-(speed-tuning2)); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.speed(-(speed-tuning3)); pc.printf("kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //kiri motor2.speed(speed-tuning2); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.speed(speed-tuning3); pc.printf("kiri \n"); vcurr += ax; } else{ motor1.brake(1); motor3.brake(1); motor2.brake(1); motor4.brake(1); pc.printf("diam \n"); vcurr = v0; } } if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){ //POWER WINDOW ATAS motorS.speed(1); if (limit1 == 0){ motorS.brake(1); } pc.printf("up \n"); c++; } else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){ //POWER WINDOW BAWAH motorS.speed(-0.5); if (limit2 ==0){ motorS.brake(1); } pc.printf("down \n"); c--; } else{ motorS.brake(1); if ((c <= batas_delay) && (c>=-batas_delay)){ c=0; } pc.printf("diam \n"); } if((c > batas_delay) && (limit1 == 0)){ c = 0; motorS.brake(1); } else if((c < -batas_delay) && (limit2 == 0)){ c = 0; motorS.brake(1); } else if( (c > batas_delay) && (limit1 != 0)){ motorS.speed(1); } else if ((c<-batas_delay) && (limit2 != 0)){ motorS.speed(-0.7); } if ((ps2.read(PS_PAD::PAD_SELECT)==1)) { //mekanisme ambil gripper pc.printf("mekanisme gripper"); if (g==1){ pc.printf("ambil 1"); pnuematik2 = 0; g=2; wait_ms(400); } else { pnuematik2 = 1; wait_ms(400); g=1; } } } void edf_servo(){ if(ps2.read(PS_PAD::PAD_X)==1){ //PWM ++ pwm += 0.0007; if( pwm > 0.7) pwm = 0.8; pc.printf("gaspol \n"); } else if(ps2.read(PS_PAD::PAD_SQUARE)==1){ //PWM-- pwm -= 0.0007; if(pwm < 0) pwm = 0.0; pc.printf("rem ndeng \n"); } if(ps2.read(PS_PAD::PAD_R2)==1){ //SERVO -- sudut += 0.5; if(sudut > 90) sudut = 90; pc.printf("servo max \n"); } else if(ps2.read(PS_PAD::PAD_L2)==1){ //SERVO ++ sudut -= 0.5; if(sudut < -90) sudut = -90; pc.printf("servo min \n"); } if(ps2.read(PS_PAD::PAD_START)==1){ sudut = 0; pwm = 0.22; pnuematik3 = 1; pnuematik4 = 1; } servoEDF.position((float)sudut); edf.setThrottle((float)pwm); edf.pulse(); } /////////////////LINE FOLLOWER///////////////////////// void init_sensor(){ char data; if(com.readable()){ data = com.getc(); for(int i=0; i<6; i++){ datasensor[i] = (data >> i) & 1; } } } void linetracer(float speed){ float speed1,speed2,speed3,speed4; //////////////////logic dari PV (present Value)///////////////////////// if(datasensor[2] && datasensor[3]){ speed1 = speed*driver0; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver0; } else if(datasensor[2]){ speed1 = speed*driver1; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver1; } else if(datasensor[3]){ speed1 = speed*driver0; speed2 = speed*driver1; speed3 = speed*driver1; speed4 = speed*driver0; } else if(datasensor[1]){ speed1 = speed*driver2; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver2; } else if(datasensor[4]){ speed1 = speed*driver0; speed2 = speed*driver2; speed3 = speed*driver2; speed4 = speed*driver0; } else if(datasensor[0]){ speed1 = speed*driver3; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver3; } else if(datasensor[5]){ speed1 = speed*driver0; speed2 = speed*driver3; speed3 = speed*driver3; speed4 = speed*driver0; } else { if(g_flag == 0){ speed1 = speed*driver0; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver0; } else if(g_flag == 3){ speed1 = speed*driver1; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver1; } else if(g_flag == 4){ speed1 = speed*driver0; speed2 = speed*driver1; speed3 = speed*driver1; speed4 = speed*driver0; } else if(g_flag == 2){ speed1 = speed*driver2; speed2 = speed*driver0; speed3 = speed*driver0; speed4 = speed*driver2; } else if(g_flag == 5){ speed1 = speed*driver0; speed2 = speed*driver2; speed3 = speed*driver2; speed4 = speed*driver0; } else if(g_flag == 1){ speed1 = -speed*driver2; speed2 = speed*driver2; speed3 = speed*driver2; speed4 = -speed*driver2; } else if(g_flag == 6){ speed1 = speed*driver2; speed2 = -speed*driver2; speed3 = -speed*driver2; speed4 = speed*driver2; } } motor1.speed((float)-(speed1-0.0)); motor2.speed((float)-(speed2-0.04)); motor3.speed((float)-(speed3-0.0)); motor4.speed((float)-(speed4-0.0)); } void flag_sensor(){ if((datasensor[2] == 1) && (datasensor[3] == 1)) g_flag = 0; else if(datasensor[2] == 1) g_flag = 3; else if(datasensor[3] == 1) g_flag = 4; else if(datasensor[1] == 1) g_flag = 2; else if(datasensor[4] == 1) g_flag = 5; else if(datasensor[0] == 1) g_flag = 1; else if(datasensor[5] == 1) g_flag = 6; }