KRAI 2017
code base baru closeloop
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of
Joystick_ManualBaseBaruV1_2
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 3:1287fccc11be
- Parent:
- 1:56bd3e8f38c5
- Child:
- 4:483c07cc22e1
--- a/main.cpp Sat Jun 27 07:37:28 2015 +0000
+++ b/main.cpp Sun Oct 16 06:27:39 2016 +0000
@@ -1,9 +1,7 @@
/**
-Base 4 Nasional
-
Case Gerak
-1. Pivot Kanan
-2. Pivot Kiri
+1. cw
+2. ccw
3. Maju
4. Mundur
5. Serong Atas Kanan
@@ -12,9 +10,12 @@
8. Serong Bawah Kiri
9. Kanan
10. Kiri
-11. Stop
+11. Analog kiri base
+12. stop
+Urutan motor 1234 searah jarum jam
+Source Code dari
Bima Sahbani EL'12
Fanny Achmad Hindrarta EL'12
**/
@@ -29,80 +30,93 @@
//#define koefperlambatan 0.8
// Deklarasi variabel motor
-Motor motor1(PA_15, PA_13, PA_14); // pwm, fwd, rev
-Motor motor2(PA_0, PC_14, PC_15); // pwm, fwd, rev
-Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
-Motor motor4(PC_6, PC_9, PC_8); // pwm, fwd, rev
-
-// Deklarasi Register Pneumatik
-DigitalOut pneumatik1(PC_11);
-DigitalOut pneumatik2(PD_2);
-
-// Deklarasi Timer Pneumatik
-Timer timer_pneu;
+Motor motor1(PB_6, PA_7 , PB_12); // pwm, fwd, rev
+Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
+Motor motor3(PB_7, PA_15, PA_14); // pwm, fwd, rev
+Motor motor4(PB_8, PB_1 , PA_13); // pwm, fwd, rev
// Inisialisasi Pin TX-RX Joystik dan PC
-joysticknucleo joystick(PA_11,PA_12);
-//Serial pc(USBTX,USBRX);
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
//bool perlambatan=0;
char case_ger;
-bool maju=false,mundur=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,pivki=false,pivka=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false;
+bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
bool stop = true;
-bool t1, t2, pneu1, pneu2;
-int delay_pneumatik;
-double vcurr;
+int jLX,jLY;
+double vcurr=0;
+float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi
+float errorx=0,errory=0;
+
+// Fungsi mapping 0-255 ke -1 sampai 1
+float mapping(int x)
+{
+ float hasil;
+ hasil = ((x+1)/128)-1;
+
+ return (hasil);
+}
+// simpan data analog
+void baca_analog()
+{
+ jLX = joystick.LX;
+ jLY = joystick.LY;
+
+ // Pembacaan nilai Y terbalik
+ x = mapping(jLX);
+ y = -mapping(jLY);
+
+ // Setelah di kalibrasi
+ xk = x + errorx;
+ yk = y + errory;
+}
+// Gerak dari Motor base
int case_gerak()
{
int casegerak;
- if (!joystick.L1 && joystick.R1) {
+ baca_analog();
+ if (!joystick.L1 && joystick.R1) {
// Pivot Kanan
casegerak = 1;
} else if (!joystick.R1 && joystick.L1) {
// Pivot Kiri
casegerak = 2;
- } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>110 && joystick.LX<190) && (joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) ) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Maju
- casegerak = 3;
- } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>90 && joystick.LX<190) && (joystick.LY>=200) )|| ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))) {
+ casegerak = 3;
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Mundur
casegerak = 4;
- } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)))) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Atas Kanan
casegerak = 5;
- } else if(((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))) {
+ } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Bawah Kanan
casegerak = 6;
- } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Atas Kiri
casegerak = 7;
- } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Bawah Kiri
casegerak = 8;
- } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>=210) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) ) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
casegerak = 9;
- } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX<=50) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))) {
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Kiri
- casegerak = 10;
- } else {
- // Stop
- casegerak = 11;
- }
-
- if(joystick.silang_click && t1==0 && t2==0) {
- pneu1 = 1;
- } else {
- pneu1 = 0;
- }
-
- if(joystick.kotak_click && t1==0 && t2==0) {
- pneu2 = 1;
- } else {
- pneu2 = 0;
- }
+ casegerak = 10;
+ } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // analog switch mode
+ if (joystick.SELECT_click) {analog=!analog;}
+ // case gerak analog on off
+ if (analog){
+ casegerak = 11;
+ } else {
+ casegerak = 12;
+ }
+}
return(casegerak);
}
@@ -110,7 +124,7 @@
/**
-** Case 1 : Pivot Kanan
+** Case 1 : Pivot kanan
** Case 2 : Pivot Kiri
** Case 3 : Maju
** Case 4 : Mundur
@@ -120,47 +134,21 @@
** Case 8 : Serong Bawah Kiri
** Case 9 : Kanan
** Case 10 : Kiri
-** Case 11 : Break
+** Case 11 : Analog
+** Case 11 : break
**/
+
+// Pergerakan dari base
void aktuator()
{
double koef;
double s1=0,s2=0,s3=0,s4=0;
- // PNEUMATIK
- if(t1==1) {
- if(timer_pneu.read_ms() - delay_pneumatik > 800) {
- pneumatik1 = 1;
- t1=0;
- }
- }
- if(t2==1) {
- if(timer_pneu.read_ms() - delay_pneumatik > 800) {
- pneumatik2 = 1;
- t2=0;
- }
- }
-
- if (pneu1 == 1 || pneu2==1) {
- timer_pneu.reset();
- delay_pneumatik = timer_pneu.read_ms();
- if(pneu1 == 1) {
- pneumatik1 = 0;
- t1 = 1;
- pneu1 = 0;
- } else if(pneu2 == 1) {
- pneumatik2 = 0;
- t2 = 1;
- pneu2 = 0;
- }
-
- }
-
// MOTOR
switch (case_ger)
{
- case (1):
+case (1):
{
if (pivka) {
if(vcurr<0.1) {
@@ -187,14 +175,14 @@
}
s1 = (float)(-0.5*koef*vcurr);
- s2 = (float)(0.5*koef*vcurr);
+ s2 = (float)(-0.5*koef*vcurr);
s3 = (float)(-0.5*koef*vcurr);
- s4 = (float)(0.5*koef*vcurr);
+ s4 = (float)(-0.5*koef*vcurr);
pivka=true;
- maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("pivKa\n");
+ pc.printf("pivKa\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -229,14 +217,14 @@
}
s1 = (float)(0.5*koef*vcurr);
- s2 = (float)(-0.5*koef*vcurr);
+ s2 = (float)(0.5*koef*vcurr);
s3 = (float)(0.5*koef*vcurr);
- s4 = (float)(-0.5*koef*vcurr);
+ s4 = (float)(0.5*koef*vcurr);
pivki=true;
- maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("pivKi\n");
+ pc.printf("pivKi\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -282,9 +270,9 @@
//s4 =koef*vcurr;
maju=true;
- mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("maju\n");
+ pc.printf("maju\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -324,9 +312,9 @@
s4 = (float)(1*koef*(vcurr+0.005));
mundur=true;
- maju=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("mundur\n");
+ pc.printf("mundur\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -364,9 +352,9 @@
s4 = (float)(0); //-koef*0.1*vcurr;
saka=true;
- maju=mundur=kiri=kanan=sbka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("saka\n");
+ pc.printf("saka\n");
motor1.speed(s1);
motor2.brake(1);
@@ -408,9 +396,9 @@
s4 = (float)(koef*vcurr);
sbka=true;
- maju=mundur=kiri=kanan=saka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("sbka\n");
+ pc.printf("sbka\n");
//motor1.speed(s1);
motor1.brake(1);
@@ -452,9 +440,9 @@
s4 = (float)(-koef*vcurr);
saki=true;
- maju=kiri=kanan=saka=mundur=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("saki\n");
+ pc.printf("saki\n");
//motor1.speed(s1);
motor1.brake(1);
@@ -496,9 +484,9 @@
s4 = (float)(0); //koef*0.1*vcurr;
sbki=true;
- maju=kiri=kanan=saka=saki=sbka=mundur=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("sbki\n");
+ pc.printf("sbki\n");
motor1.speed(s1);
//motor2.speed(s2);
@@ -540,9 +528,9 @@
s4 =(float)(1.0*koef*vcurr);
kanan=true;
- maju=kiri=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("Kanan\n");
+ pc.printf("Kanan\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -581,9 +569,9 @@
s4 =(float)(-1.0*koef*vcurr);
kiri=true;
- maju=kanan=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- //pc.printf("Kiri\n");
+ pc.printf("Kiri\n");
motor1.speed(s1);
motor2.speed(s2);
@@ -592,7 +580,52 @@
break;
}
- default :
+ case (11):
+ {
+ if (analog) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ }
+ else {
+ koef=1;
+ }
+
+ s1 = (float)(0.5*koef*vcurr*((-xk)+(yk)));
+ s2 = (float)(0.5*koef*vcurr*((-xk)+(-yk)));
+ s3 = (float)(0.5*koef*vcurr*((xk)+(-yk)));
+ s4 = (float)(0.5*koef*vcurr*((xk)+(yk)));
+
+
+
+ analog=true;
+ maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ pc.printf("analog x= %d y= %d /n ",x,y);
+
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+ break;
+ }
+ default :
{
//if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
//if (maju && (vcurr>=0.5)) wait_ms(100);
@@ -618,28 +651,32 @@
motor4.brake(1);
//}
- maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
stop = true;
//s1 = 0;s2 =0; s3 =0; s4 =0;
- //pc.printf("Stop\n");
- }
+ pc.printf("Stop\n");
+ }
}
}
+// Kalibrasi tombol analog kiri
+void kalibrasi()
+{
+ errorx = 0-x;
+ errory = 0-y;
+
+}
+
int main (void)
{
// Set baud rate - 115200
joystick.setup();
- //pc.baud(115200);
- //pc.printf("Ready...\n");
- timer_pneu.start();
- pneumatik1=1;
- pneumatik2=1;
- t1=0;
- t2=0;
+ pc.baud(115200);
+ pc.printf("Ready...\n");
+
while(1)
{
// Interrupt Serial
@@ -651,7 +688,11 @@
joystick.olah_data();
//pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
case_ger = case_gerak();
- aktuator();
+ aktuator();
+ //kalibrasi
+ if (joystick.START_click){
+ kalibrasi();
+ }
} else {
joystick.idle();