KRAI 2016
/
Ultimate_Hybrid_LF_v4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: ESC Motordriver PS_PAD Ping hadah mbed
Fork of
Ultimate_Hybrid_LF
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 7:59a513681663
- Parent:
- 5:256f6eac0358
--- a/main.cpp Thu Apr 28 14:59:21 2016 +0000
+++ b/main.cpp Sat Feb 10 04:24:23 2018 +0000
@@ -2,7 +2,7 @@
/** FILE HEADER **/
/*********************************************************************************************/
#include "mbed.h"
-#include "Motor.h"
+#include "motordriver.h"
#include "PS_PAD.h"
#include "esc.h"
#include "Servo.h"
@@ -12,23 +12,24 @@
/** DEKLARASI INPUT OUTPUT **/
/*********************************************************************************************/
//serial
-Serial pc(USBTX,USBRX); //debug
-Serial com(PA_0,PA_1); //sensor
+Serial pc(USBTX,USBRX); //debug
+Serial com1(PA_0,PA_1); //sensor depan
+Serial com2(PC_6, PC_7); //sensor belakang
//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 motorC2(PB_7, PA_14, PA_15);
-Motor motorC1(PB_9, PA_12, PC_5);
-Motor motorS(PB_8, PB_1, PA_13);
+Motor motor3(PA_8, PB_0, PC_15, 1); //motor1
+Motor motor1(PA_11, PA_6, PA_5, 1); //motor2
+Motor motor4(PA_9, PC_2, PC_3, 1); //motor_slider
+Motor motor2(PA_10, PB_5, PB_4, 1); //motor_griper
+Motor motorC1(PB_7, PA_14, PA_15 , 1); //motor4
+Motor motorC2(PB_9, PA_12, PC_5, 1); //motor6
+Motor motorS(PB_8, PA_13, PB_1, 1); //motor5
//Motor motor4(PB_6, PA_7, PB_12);
-//pnuematik
+//pnuematik
DigitalOut pnuematik1(PC_11);
DigitalOut pnuematik2(PC_10);
DigitalOut pnuematik3(PD_2);
@@ -43,16 +44,17 @@
DigitalIn field(PB_10 ,PullUp);
//laser pointer
-DigitalOut laser(PB_3);
+DigitalIn IR(PB_3 ,PullUp);
//servo(PWM)
Servo servoEDF(PC_8);
//EDF(PWM, timer)
-ESC edf(PC_6,20);
+ESC edf(PC_9,20);
//Timer
Ticker timer;
+Timer timer_linetracer;
//Sensor Ping
Ping ping(PB_2);
@@ -63,19 +65,19 @@
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 ax = 0.0005;
+float vcurr_base = v0;
+float vcurr_slider = v0;
-const float tuning1 = 0.0;
-const float tuning2 = 0.04;
-const float tuning3 = 0.0;
-const float tuning4 = 0.04;
+const float tuning1 = 0.03;
+const float tuning2 = 0.06;
+const float tuning3 = 0.03;
+const float tuning4 = 0.0 ;
-const float driver0 = 1;
-const float driver1 = 0.82;
-const float driver2 = 0.6;
-const float driver3 = 0.15;
-
+const float driver1_0 = 1;
+const float driver1_1 = 0.91;
+const float driver1_2 = 0.78;
+const float driver1_3 = 0.55;
// inisialisasi pwm awal servo
double pwm = 0.00;
@@ -83,17 +85,20 @@
// inisialisasi sudut awal
double sudut = -85;
-// variabel kondisi pnuematik
-int g = 1;
+// variabel counting
+unsigned long int g = 0;
//slider auto
int c =0;
-int batas_delay = 270;
+int batas_delay = 340;
+int flag_river;
//data sensor garis dan line following
-int datasensor[6];
+int datasensor1[6];
+int datasensor2[6];
int over;
-int g_flag;
+int g_flag1;
+int g_flag2;
void aktuator(int f);
@@ -102,6 +107,7 @@
void init_sensor();
void linetracer(float speed);
void flag_sensor();
+void putar(float speed);
float read_jarak();
@@ -113,10 +119,14 @@
int main() {
//inisiasi serial
pc.baud(115200);
- com.baud(115200);
+ com1.baud(115200);
+ com2.baud(115200);
if(field == 1) sudut = -85;
- else sudut = 85;
+ else sudut = 85;
+
+ //inisisasi flag river
+ flag_river = 0;
//inisiasi joystick
ps2.init();
@@ -127,10 +137,7 @@
//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;
@@ -138,8 +145,9 @@
pnuematik4 = 1;
//inisisasi TIMER
- timer.attach(&flag_sensor,0.0005);
- timer.detach();
+ //timer.attach(&flag_sensor,0.0005);
+ //timer.detach();
+ timer_linetracer.reset();
//kondisi robot
bool manual=true;
@@ -159,58 +167,85 @@
//running otomatis
timer.attach(&flag_sensor,0.0005);
+ timer_linetracer.start();
+ vcurr_base = v0;
while(!pool){
init_sensor();
- if(vcurr > 0.4) vcurr = (float) 0.4;
- //else if (vcurr < 0.2) vcurr = (float) 0.2;
-
- linetracer(vcurr);
- //laser = 1;
-
- if((limit3==0) || (read_jarak() <= 9.0)) pool=true;
+ if(vcurr_base > (float)1) vcurr_base = (float) 1;
+ //else if (vcurr_base < 0.2) vcurr_base = (float) 0.2;
+
+ linetracer(vcurr_base);
- //if(read_jarak() <= 45.0) vcurr -= ax;
- //else vcurr += ax;
- vcurr += ax;
+
+ if(timer_linetracer.read_ms() >= 2500){
+ vcurr_base = 0.4;
+ }
+ else{
+ vcurr_base += 0.002;
+ }
for(int i=0; i<6; i++){
- printf("%d ",datasensor[i]);
+ printf("%d ",datasensor1[i]);
}
- printf("%d \n",g_flag);
+ for(int i=5; i>=0; i--){
+ printf("%d ",datasensor2[i]);
+ }
+ printf("%d %d %lf \n",g_flag1, g_flag2, read_jarak());
+ if((limit3==0) || (read_jarak() <= (float)5.0)) pool=true;
}
- motor1.brake(0);
- motor2.brake(0);
- motor3.brake(0);
- motor4.brake(0);
+ putar(0.2);
timer.detach();
-
+ timer_linetracer.stop();
+
+ motor1.stop(1);
+ motor2.stop(1);
+ motor3.stop(1);
+ motor4.stop(1);
+
+ wait_ms(200);
pnuematik1=0;
- wait_ms(1500);
+
+ wait_ms(1000);
while(limit4!=0){
- motorC1.speed(0.95);
+ motorC1.speed(1);
motorC2.speed(-1);
+
+ /* if(g < 600000){
+ motor1.speed(-(1-tuning1));
+ motor2.speed(-(1-tuning2));
+ motor3.speed(-(1-tuning3));
+ motor4.speed(-(1-tuning4));
+
+ g++;
+ }
+ else{
+ motor1.stop(1);
+ motor2.stop(1);
+ motor3.stop(1);
+ motor4.stop(1);
+ }*/
}
- motorC1.brake(1);
- motorC2.brake(1);
+ motorC1.stop(1);
+ motorC2.stop(1);
if(field==1){
pnuematik3 = 0;
+ wait_ms(1300);
+ pnuematik2 = 0;
wait_ms(1500);
- pnuematik2 = 1;
- wait_ms(500);
pnuematik3 = 1;
}
else{
pnuematik4 = 0;
+ wait_ms(1300);
+ pnuematik2 = 0;
wait_ms(1500);
- pnuematik2 = 1;
- wait_ms(500);
pnuematik4 = 1;
}
@@ -221,9 +256,10 @@
/** ALGORITMA PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
void aktuator(int f){
- float speed = vcurr;
+ float speed = vcurr_base;
- if(vcurr >= gMax_speed) vcurr = gMax_speed;
+ if(vcurr_base >= gMax_speed) vcurr_base = gMax_speed;
+ if(vcurr_slider >= 1) vcurr_slider = 1;
if(f == 1){
if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
@@ -234,7 +270,7 @@
motor3.speed((float)-0.5*(speed-tuning3));
pc.printf("pivot kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
//pivot kanan
@@ -244,47 +280,47 @@
motor3.speed((float)0.5*(speed-tuning3));
pc.printf("pivot kanan \n");
- vcurr += ax;
+ vcurr_base += 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);
+ motor4.stop(1);
+ motor1.stop(1);
motor3.speed(speed-tuning3);
pc.printf("serong atas kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//serong atas kiri
- motor2.brake(1);
+ motor2.stop(1);
motor4.speed(-(speed-tuning4));
motor1.speed(-(speed-tuning1));
- motor3.brake(1);
+ motor3.stop(1);
pc.printf("serong atas kiri \n");
- vcurr += ax;
+ vcurr_base += 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);
+ motor4.stop(1);
+ motor1.stop(1);
motor3.speed(-(speed-tuning3));
pc.printf("serong bawah kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
//serong bawah kanan
- motor2.brake(1);
+ motor2.stop(1);
motor4.speed(speed-tuning4);
motor1.speed(speed-tuning1);
- motor3.brake(1);
+ motor3.stop(1);
pc.printf("serong bawah kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
//maju
@@ -294,7 +330,7 @@
motor4.speed(-(speed-tuning4));
pc.printf("maju \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
//mundur
@@ -304,7 +340,7 @@
motor4.speed(speed-tuning4);
pc.printf("mundur \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
//kanan
@@ -314,7 +350,7 @@
motor3.speed(speed-tuning3);
pc.printf("kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//kiri
@@ -324,16 +360,16 @@
motor3.speed(-(speed-tuning3));
pc.printf("kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else{
- motor1.brake(1);
- motor3.brake(1);
- motor2.brake(1);
- motor4.brake(1);
+ motor1.stop(1);
+ motor3.stop(1);
+ motor2.stop(1);
+ motor4.stop(1);
pc.printf("diam \n");
- vcurr = v0;
+ vcurr_base = v0;
}
}
else{
@@ -345,7 +381,7 @@
motor3.speed((float)-0.5*(speed-tuning3));
pc.printf("pivot kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
//pivot kanan
@@ -355,47 +391,47 @@
motor3.speed((float)0.5*(speed-tuning3));
pc.printf("pivot kanan \n");
- vcurr += ax;
+ vcurr_base += 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);
+ motor4.stop(1);
+ motor1.stop(1);
motor3.speed(-(speed-tuning3));
pc.printf("serong atas kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//serong atas kiri
- motor2.brake(1);
+ motor2.stop(1);
motor4.speed(speed-tuning4);
motor1.speed(speed-tuning1);
- motor3.brake(1);
+ motor3.stop(1);
pc.printf("serong atas kiri \n");
- vcurr += ax;
+ vcurr_base += 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);
+ motor4.stop(1);
+ motor1.stop(1);
motor3.speed(speed-tuning3);
pc.printf("serong bawah kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
//serong bawah kanan
- motor2.brake(1);
+ motor2.stop(1);
motor4.speed(-(speed-tuning4));
motor1.speed(-(speed-tuning1));
- motor3.brake(1);
+ motor3.stop(1);
pc.printf("serong bawah kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
//maju
@@ -405,7 +441,7 @@
motor4.speed(speed-tuning4);
pc.printf("maju \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
//mundur
@@ -415,7 +451,7 @@
motor4.speed(-(speed-tuning4));
pc.printf("mundur \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
//kanan
@@ -425,7 +461,7 @@
motor3.speed(-(speed-tuning3));
pc.printf("kanan \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//kiri
@@ -435,44 +471,49 @@
motor3.speed(speed-tuning3);
pc.printf("kiri \n");
- vcurr += ax;
+ vcurr_base += ax;
}
else{
- motor1.brake(1);
- motor3.brake(1);
- motor2.brake(1);
- motor4.brake(1);
+ motor1.stop(1);
+ motor3.stop(1);
+ motor2.stop(1);
+ motor4.stop(1);
pc.printf("diam \n");
- vcurr = v0;
+ vcurr_base = v0;
}
}
- if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
+ if(((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))|| (IR == 0)){
//POWER WINDOW ATAS
- motorS.speed(1);
+ motorS.speed(vcurr_slider);
if (limit1 == 0){
- motorS.brake(1);
+ motorS.stop(1);
}
pc.printf("up \n");
c++;
+
+ vcurr_slider += ax;
}
else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
//POWER WINDOW BAWAH
- motorS.speed(-0.5);
+ motorS.speed(-vcurr_slider);
if (limit2 ==0){
- motorS.brake(1);
+ motorS.stop(1);
}
pc.printf("down \n");
- c--;
+ c--;
+
+ vcurr_slider += ax;
}
else{
- motorS.brake(1);
+ vcurr_slider = v0;
+ motorS.stop(1);
if ((c <= batas_delay) && (c>=-batas_delay)){
c=0;
}
@@ -482,11 +523,11 @@
if((c > batas_delay) && (limit1 == 0)){
c = 0;
- motorS.brake(1);
+ motorS.stop(1);
}
else if((c < -batas_delay) && (limit2 == 0)){
c = 0;
- motorS.brake(1);
+ motorS.stop(1);
}
else if( (c > batas_delay) && (limit1 != 0)){
motorS.speed(1);
@@ -500,7 +541,7 @@
{
//mekanisme ambil gripper
pc.printf("mekanisme gripper");
- if (g==1){
+ /* if (g==1){
pc.printf("ambil 1");
pnuematik2 = 0;
g=2;
@@ -511,23 +552,35 @@
pnuematik2 = 1;
wait_ms(400);
g=1;
- }
+ }*/
+ pc.printf("ambil 1");
+ pnuematik2 = !pnuematik2;
+ //g=2;
+ wait_ms(400);
+
}
}
void edf_servo(){
if(ps2.read(PS_PAD::PAD_X)==1){
//PWM ++
- pwm += 0.0007;
+
+ if( flag_river == 1){
+ pwm += 0.0007;
+ }
+ else{
+ pwm = 0.378;
+ }
+
if( pwm > 0.8) pwm = 0.8;
- pc.printf("gaspol \n");
+ pc.printf(" %f gaspol \n", pwm);
}
else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
//PWM--
pwm -= 0.0007;
if(pwm < 0) pwm = 0.0;
- pc.printf("rem ndeng \n");
+ pc.printf("%f rem ndeng \n", pwm);
}
if(ps2.read(PS_PAD::PAD_R2)==1){
@@ -547,8 +600,9 @@
if(ps2.read(PS_PAD::PAD_START)==1){
+ flag_river = 1;
sudut = 0;
- pwm = 0.22;
+ pwm = 0.295;
}
@@ -561,11 +615,19 @@
void init_sensor(){
char data;
- if(com.readable()){
- data = com.getc();
+ if(com1.readable()){
+ data = com1.getc();
for(int i=0; i<6; i++){
- datasensor[i] = (data >> i) & 1;
+ datasensor1[i] = (data >> i) & 1;
+ }
+ }
+
+ if(com2.readable()){
+ data = com2.getc();
+
+ for(int i=0; i<6; i++){
+ datasensor2[i] = (data >> i) & 1;
}
}
}
@@ -573,111 +635,48 @@
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;
- }
-
+ //////////////////logic dari PV (present Value)/////////////////////////
+ if(datasensor1[2] && datasensor1[3]) { speed1 = speed*driver1_0; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_0; }
+ else if(datasensor1[2]) { speed1 = speed*driver1_1; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_1; }
+ else if(datasensor1[3]) { speed1 = speed*driver1_0; speed2 = speed*driver1_1; speed3 = speed*driver1_1; speed4 = speed*driver1_0; }
+ else if(datasensor1[1]) { speed1 = speed*driver1_2; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_2; }
+ else if(datasensor1[4]) { speed1 = speed*driver1_0; speed2 = speed*driver1_2; speed3 = speed*driver1_2; speed4 = speed*driver1_0; }
+ else if(datasensor1[0]) { speed1 = speed*driver1_3; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_3; }
+ else if(datasensor1[5]) { speed1 = speed*driver1_0; speed2 = speed*driver1_3; speed3 = speed*driver1_3; speed4 = speed*driver1_0; }
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;
- }
+ if(g_flag1 == 0) { speed1 = speed*driver1_0; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_0; }
+ else if(g_flag1 == 3) { speed1 = speed*driver1_1; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_1; }
+ else if(g_flag1 == 4) { speed1 = speed*driver1_0; speed2 = speed*driver1_1; speed3 = speed*driver1_1; speed4 = speed*driver1_0; }
+ else if(g_flag1 == 2) { speed1 = speed*driver1_2; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_2; }
+ else if(g_flag1 == 5) { speed1 = speed*driver1_0; speed2 = speed*driver1_2; speed3 = speed*driver1_2; speed4 = speed*driver1_0; }
+ else if(g_flag1 == 1) { speed1 = 0; speed2 = speed*driver1_2; speed3 = speed*driver1_2; speed4 = 0; }
+ else if(g_flag1 == 6) { speed1 = speed*driver1_2; speed2 = 0; speed3 = 0; speed4 = speed*driver1_2; }
}
-
- motor1.speed(-(float)(speed1-0.04));
- motor2.speed(-(float)(speed2-0.04));
- motor3.speed(-(float)(speed3-0.0));
- motor4.speed(-(float)(speed4-0.0));
+
+ motor1.speed(-(float)(speed1-tuning1-0.04));
+ motor2.speed(-(float)(speed2-tuning2-0.04));
+ motor3.speed(-(float)(speed3-tuning3));
+ motor4.speed(-(float)(speed4-tuning4));
}
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;
+ if((datasensor1[2] == 1) && (datasensor1[3] == 1)) g_flag1 = 0;
+ else if(datasensor1[2] == 1) g_flag1 = 3;
+ else if(datasensor1[3] == 1) g_flag1 = 4;
+ else if(datasensor1[1] == 1) g_flag1 = 2;
+ else if(datasensor1[4] == 1) g_flag1 = 5;
+ else if(datasensor1[0] == 1) g_flag1 = 1;
+ else if(datasensor1[5] == 1) g_flag1 = 6;
+
+ if((datasensor2[2] == 1) && (datasensor2[3] == 1)) g_flag2 = 0;
+ else if(datasensor2[2] == 1) g_flag2 = 3;
+ else if(datasensor2[3] == 1) g_flag2 = 4;
+ else if(datasensor2[1] == 1) g_flag2 = 2;
+ else if(datasensor2[4] == 1) g_flag2 = 5;
+ else if(datasensor2[0] == 1) g_flag2 = 1;
+ else if(datasensor2[5] == 1) g_flag2 = 6;
}
@@ -689,4 +688,48 @@
wait_ms(45);
jarak = ping.Read_cm()/2;
return jarak;
+}
+
+
+void putar(float speed){
+ float speed1, speed2, speed3, speed4;
+
+ while(!((datasensor2[2] == 1) || (datasensor2[3] == 1))){
+ init_sensor();
+
+ if(datasensor2[2] && datasensor2[3]) { speed1 = 0; speed2 = 0; speed3 = 0; speed4 = 0; }
+ else if(datasensor2[3]) { speed1 = -speed*driver1_3; speed2 = -speed*driver1_3; speed3 = -speed*driver1_3; speed4 = -speed*driver1_3; }
+ else if(datasensor2[2]) { speed1 = speed*driver1_3; speed2 = speed*driver1_3; speed3 = speed*driver1_3; speed4 = speed*driver1_3; }
+ else if(datasensor2[4]) { speed1 = -speed*driver1_1; speed2 = -speed*driver1_1; speed3 = -speed*driver1_1; speed4 = -speed*driver1_1; }
+ else if(datasensor2[1]) { speed1 = speed*driver1_1; speed2 = speed*driver1_1; speed3 = speed*driver1_1; speed4 = speed*driver1_1; }
+ else if(datasensor2[5]) { speed1 = -speed*driver1_0; speed2 = -speed*driver1_0; speed3 = -speed*driver1_0; speed4 = -speed*driver1_0; }
+ else if(datasensor2[0]) { speed1 = speed*driver1_0; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_0; }
+ else
+ {
+ if(g_flag2 == 0) { speed1 = speed*driver1_3; speed2 = speed*driver1_3; speed3 = speed*driver1_3; speed4 = speed*driver1_3; }
+ else if(g_flag2 == 3) { speed1 = -speed*driver1_2; speed2 = -speed*driver1_2; speed3 = -speed*driver1_2; speed4 = -speed*driver1_2; }
+ else if(g_flag2 == 4) { speed1 = speed*driver1_2; speed2 = speed*driver1_2; speed3 = speed*driver1_2; speed4 = speed*driver1_2; }
+ else if(g_flag2 == 2) { speed1 = -speed*driver1_1; speed2 = -speed*driver1_1; speed3 = -speed*driver1_1; speed4 = -speed*driver1_1; }
+ else if(g_flag2 == 5) { speed1 = speed*driver1_1; speed2 = speed*driver1_1; speed3 = speed*driver1_1; speed4 = speed*driver1_1; }
+ else if(g_flag2 == 1) { speed1 = -speed*driver1_0; speed2 = -speed*driver1_0; speed3 = -speed*driver1_0; speed4 = -speed*driver1_0; }
+ else if(g_flag2 == 6) { speed1 = speed*driver1_0; speed2 = speed*driver1_0; speed3 = speed*driver1_0; speed4 = speed*driver1_0; }
+ }
+
+ motor1.speed((float)(speed1));
+ motor2.speed((float)(speed2));
+ motor3.speed(-(float)(speed3));
+ motor4.speed(-(float)(speed4));
+ }
+
+ motor1.speed(-(0.35-tuning1));
+ motor2.speed(-(0.35-tuning2));
+ motor3.speed(-(0.35-tuning3));
+ motor4.speed(-(0.35-tuning4));
+
+ wait_ms(300);
+
+ motor1.stop(1);
+ motor2.stop(1);
+ motor3.stop(1);
+ motor4.stop(1);
}
\ No newline at end of file