base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
Diff: main.cpp
- Revision:
- 1:fc1535231c0d
- Parent:
- 0:edddd373a163
- Child:
- 2:df6c49846367
diff -r edddd373a163 -r fc1535231c0d main.cpp --- a/main.cpp Tue Apr 19 02:47:31 2016 +0000 +++ b/main.cpp Tue Apr 19 13:32:26 2016 +0000 @@ -28,6 +28,18 @@ 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); @@ -57,7 +69,7 @@ /*********************************************************************************************/ float gMax_speed = 1; float v0 = 0.6; -float ax = 0.0007; +float ax = 0.0005; // inisialisasi pwm awal servo double pwm = 0.00; @@ -154,33 +166,39 @@ /*********************************************************************************************/ void aktuator(){ float speed = v0; - float tuning = 0.01; + 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(speed*0.7); - motor4.speed(-speed*0.7); - motor1.speed(speed*0.7-tuning); - motor3.speed(-(speed*0.7-tuning)); + 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(-speed*0.7); - motor4.speed(speed*0.7); - motor1.speed(-(speed*0.7-tuning)); - motor3.speed(speed*0.7-tuning); + 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); + motor2.speed(speed-tuning2); motor4.brake(1); motor1.brake(1); - motor3.speed(speed-tuning); + motor3.speed(speed-tuning3); pc.printf("serong atas kanan \n"); v0 += ax; @@ -188,8 +206,8 @@ 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); - motor1.speed(-(speed-tuning)); + motor4.speed(-(speed-tuning4)); + motor1.speed(-(speed-tuning1)); motor3.brake(1); pc.printf("serong atas kiri \n"); @@ -197,10 +215,10 @@ } else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ //serong bawah kiri - motor2.speed(-speed); + motor2.speed(-(speed-tuning2)); motor4.brake(1); motor1.brake(1); - motor3.speed(-(speed-tuning)); + motor3.speed(-(speed-tuning3)); pc.printf("serong bawah kiri \n"); v0 += ax; @@ -208,8 +226,8 @@ 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); - motor1.speed(speed-tuning); + motor4.speed(speed-tuning4); + motor1.speed(speed-tuning1); motor3.brake(1); pc.printf("serong bawah kanan \n"); @@ -217,40 +235,40 @@ } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ //maju - motor1.speed(-(speed-tuning)); - motor3.speed(speed-tuning); - motor2.speed(speed); - motor4.speed(-speed); + 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-tuning); - motor3.speed(-(speed-tuning)); - motor2.speed(-speed); - motor4.speed(speed); + 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); - motor4.speed(speed); - motor1.speed(speed); - motor3.speed(speed); + 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); - motor4.speed(-speed); - motor1.speed(-speed); - motor3.speed(-speed); + motor2.speed(-(speed-tuning2)); + motor4.speed(-(speed-tuning4)); + motor1.speed(-(speed-tuning1)); + motor3.speed(-(speed-tuning3)); pc.printf("kiri \n"); v0 += ax;