base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
main.cpp
- Committer:
- Najib_irvani
- Date:
- 2016-04-19
- Revision:
- 1:fc1535231c0d
- Parent:
- 0:edddd373a163
- Child:
- 2:df6c49846367
File content as of revision 1:fc1535231c0d:
/*********************************************************************************************/ /** FILE HEADER **/ /*********************************************************************************************/ #include "mbed.h" #include "Motor.h" #include "PS_PAD.h" #include "PID.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(PB_6, PA_7, PB_12); Motor motor3(PA_9, PC_2, PC_3); Motor motor4(PB_7, PA_14, PA_15); Motor motorC1(PB_9, PA_12, PC_5); Motor motorC2(PB_8, PB_1, PA_13); Motor motorS(PA_10, PB_5, PB_4); /* coba coba //motor (PWM, forward, reverse) Motor motor1(PA_8, PB_0, PC_15); Motor motorC1(PA_11, PA_6, PA_5); //Motor motor3(PB_6, PA_7, PB_12); Motor motorC2(PA_9, PC_2, PC_3); Motor motor4(PB_7, PA_14, PA_15); Motor motor2(PB_9, PA_12, PC_5); Motor motor3(PB_8, PB_1, PA_13); Motor motorS(PA_10, PB_5, PB_4); */ //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); //PID line follower(P, I, D, Time Sampling) PID PID(0.992,0.000,0.81,0.001); //servo(PWM) Servo servoEDF(PC_8); //EDF(PWM, timer) ESC edf(PC_6,20); //Timer Pnuematik Timer timer; /*********************************************************************************************/ /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/ /*********************************************************************************************/ float gMax_speed = 1; float v0 = 0.6; float ax = 0.0005; // inisialisasi pwm awal servo double pwm = 0.00; // inisialisasi sudut awal double sudut = -85; // variabel kondisi pnuematik int g = 1; /////// void aktuator(); void edf_servo(); /*********************************************************************************************/ /*********************************************************************************************/ /** PROGRAM UTAMA **/ /*********************************************************************************************/ /*********************************************************************************************/ int main() { //inisiasi serial pc.baud(115200); com.baud(115200); //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(); //inisiasi pnuematik pnuematik1 = 1; pnuematik2 = 1; pnuematik3 = 1; pnuematik4 = 1; //inisiasi PID sensor PID.setInputLimits(-15,15); PID.setOutputLimits(-1.0,1.0); PID.setMode(1.0); PID.setBias(0.0); PID.reset(); //inisisasi TIMER timer.start(); //kondisi robot bool manual=true; while(manual){ ps2.poll(); aktuator(); edf_servo(); if(limit3==0) manual = false; } motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); pnuematik1=0; wait_ms(1500); while(limit4!=0){ motorC1.speed(1); motorC2.speed(-1); } motorC1.brake(1); motorC2.brake(1); pnuematik3 = 0; wait_ms(1500); pnuematik2 = 1; wait_ms(500); pnuematik3 = 1; return 0; } /*********************************************************************************************/ /** ALGORITMA PROSEDUR DAN FUNGSI **/ /*********************************************************************************************/ void aktuator(){ float speed = v0; float tuning1 = 0.0; float tuning2 = 0.23; float tuning3 = 0.1; float tuning4 = 0.26; if(v0 >= gMax_speed) v0 = gMax_speed; if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ //pivot kiri motor2.speed((float)0.6*(speed-tuning2)); motor4.speed((float)-0.6*(speed-tuning4)); motor1.speed((float)0.6*(speed-tuning1)); motor3.speed((float)-0.6*(speed-tuning3)); pc.printf("pivot kiri \n"); v0 += ax; } else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ //pivot kanan motor2.speed((float)-0.6*(speed-tuning2)); motor4.speed((float)0.6*(speed-tuning4)); motor1.speed((float)-0.6*(speed-tuning1)); motor3.speed((float)0.6*(speed-tuning3)); pc.printf("pivot kanan \n"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += 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"); v0 += ax; } else{ motor1.brake(1); motor3.brake(1); motor2.brake(1); motor4.brake(1); pc.printf("diam \n"); v0 = 0.6; } if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){ //POWER WINDOW ATAS motorS.speed(1); pc.printf("up \n"); } else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){ //POWER WINDOW BAWAH motorS.speed(-1); pc.printf("down \n"); } else{ motorS.brake(1); pc.printf("diam \n"); } 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.002; if( pwm > 0.7) pwm = 0.7; pc.printf("gaspol \n"); } else if(ps2.read(PS_PAD::PAD_SQUARE)==1){ //PWM-- pwm -= 0.002; 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.25; } servoEDF.position((float)sudut); edf.setThrottle((float)pwm); edf.pulse(); }