KRAI ITB GARUDAGO / Mbed 2 deprecated fungsi_switch_kaki

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:   mbed encoderKRAI Motornew CMPS12_KRAI ping millis

main.cpp@3:8f57f69f6e64, 2019-04-10 (annotated)

Committer:
gatulz
Date:
Wed Apr 10 01:44:10 2019 +0000
Revision:
3:8f57f69f6e64
Parent:
1:a2c7dd0a0f6e 
kuda lewat sanddune

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gianarjuna 1:a2c7dd0a0f6e 1 //DEKLARASI LIBRARY
315_josh 0:7ab5f1f9dcb8 2 #include "mbed.h"
gianarjuna 1:a2c7dd0a0f6e 3 #include "millis.h"
gianarjuna 1:a2c7dd0a0f6e 4 #include "Motor.h"
gianarjuna 1:a2c7dd0a0f6e 5 #include "encoderKRAI.h"
gianarjuna 1:a2c7dd0a0f6e 6 #include "CMPS12_KRAI.h"
gianarjuna 1:a2c7dd0a0f6e 7
gianarjuna 1:a2c7dd0a0f6e 8 #define PULSE_TO_DEG 0.135
gianarjuna 1:a2c7dd0a0f6e 9 #define PWM_MAX 0.85
gianarjuna 1:a2c7dd0a0f6e 10 #define PWM_MIN -0.85
gianarjuna 1:a2c7dd0a0f6e 11 #define TOLERANCET 1.0
gianarjuna 1:a2c7dd0a0f6e 12 #define TS 7.0
315_josh 0:7ab5f1f9dcb8 13
gatulz 3:8f57f69f6e64 14 #define Kp_A 0.06//0.078
gatulz 3:8f57f69f6e64 15 #define Ki_A 8.554e-5
gatulz 3:8f57f69f6e64 16 #define Kd_A 0
gatulz 3:8f57f69f6e64 17 DigitalIn Ubutton(USER_BUTTON);
gatulz 3:8f57f69f6e64 18 Serial pc(USBTX, USBRX, 115200);
gianarjuna 1:a2c7dd0a0f6e 19 CMPS12_KRAI cmps(PB_3, PB_10, 0xc0);
gianarjuna 1:a2c7dd0a0f6e 20 encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING); //encoder motor steering
gianarjuna 1:a2c7dd0a0f6e 21 encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 22 encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 23 Motor motorSteer(PB_14, PC_4, PB_13); //motor arah putaran CCW ++
gatulz 3:8f57f69f6e64 24 Motor motorKi(PA_11, PB_12,PA_7); //motor kanan
gatulz 3:8f57f69f6e64 25 Motor motorKa(PA_5, PA_12, PA_6); //motor kiri
gatulz 3:8f57f69f6e64 26
gianarjuna 1:a2c7dd0a0f6e 27 DigitalIn switchKi(PA_9,PullUp);
gianarjuna 1:a2c7dd0a0f6e 28 DigitalIn switchKa(PD_2,PullUp);
gatulz 3:8f57f69f6e64 29 DigitalOut pneu1[2] = {(PC_1),(PA_1)}; //kanan belakang , kiri depan
gatulz 3:8f57f69f6e64 30 DigitalOut pneu2[2] = {(PB_0),(PA_0)}; //kiri belakang, kanan depan
gianarjuna 1:a2c7dd0a0f6e 31 int pulsetemp;
gianarjuna 1:a2c7dd0a0f6e 32 float theta;
gianarjuna 1:a2c7dd0a0f6e 33 float target;
gianarjuna 1:a2c7dd0a0f6e 34 int cp = 0;
gianarjuna 1:a2c7dd0a0f6e 35 int brake_state = 0;
gianarjuna 1:a2c7dd0a0f6e 36 float sum_theta_error;
gianarjuna 1:a2c7dd0a0f6e 37 float theta_error_prev;
gianarjuna 1:a2c7dd0a0f6e 38 float theta_target;
gianarjuna 1:a2c7dd0a0f6e 39 float speed;
gianarjuna 1:a2c7dd0a0f6e 40 float w;
gianarjuna 1:a2c7dd0a0f6e 41 float theta_sp;
gianarjuna 1:a2c7dd0a0f6e 42 float theta_error;
gianarjuna 1:a2c7dd0a0f6e 43 float heading_temp, heading0, heading;
gianarjuna 1:a2c7dd0a0f6e 44 double pulse = 0;
gianarjuna 1:a2c7dd0a0f6e 45 double KI;
gianarjuna 1:a2c7dd0a0f6e 46 int angkat;
gianarjuna 1:a2c7dd0a0f6e 47 int bouncing = 0;
gatulz 3:8f57f69f6e64 48 double pulseA, pulse_B;
gianarjuna 1:a2c7dd0a0f6e 49 const double KP = 0.05,
gianarjuna 1:a2c7dd0a0f6e 50 KI1 = 0.00000,
gianarjuna 1:a2c7dd0a0f6e 51 KI2 = 0.00000;
gianarjuna 1:a2c7dd0a0f6e 52 unsigned long lastParameter, //millis utk perhintungan parameter
gianarjuna 1:a2c7dd0a0f6e 53 last_mt_printPID,
gatulz 3:8f57f69f6e64 54 last_mt_print=0.0, //millis utk print
gianarjuna 1:a2c7dd0a0f6e 55 last_mt_button, //millis utk user button
gianarjuna 1:a2c7dd0a0f6e 56 last_start, //millis start uji coba kuda
gianarjuna 1:a2c7dd0a0f6e 57 lastFSM, //finite state machine millis
gianarjuna 1:a2c7dd0a0f6e 58 lastPWM,
gianarjuna 1:a2c7dd0a0f6e 59 lastMotor,
gianarjuna 1:a2c7dd0a0f6e 60 lastCompass,
gatulz 3:8f57f69f6e64 61 lastThetaTarget,
gatulz 3:8f57f69f6e64 62 lastSafety,
gatulz 3:8f57f69f6e64 63 lastAngkat;
gianarjuna 1:a2c7dd0a0f6e 64 int start;
gianarjuna 1:a2c7dd0a0f6e 65 int state=0;
gianarjuna 1:a2c7dd0a0f6e 66 float pwm;
gianarjuna 1:a2c7dd0a0f6e 67 float pwm_maju=0.0;
gianarjuna 1:a2c7dd0a0f6e 68 float temp = 0;
gianarjuna 1:a2c7dd0a0f6e 69 float dThetaMax = 35.0;
gianarjuna 1:a2c7dd0a0f6e 70 float speed_maju=0;
gianarjuna 1:a2c7dd0a0f6e 71 int ohstart = 0;
gianarjuna 1:a2c7dd0a0f6e 72 int belok =0;
gianarjuna 1:a2c7dd0a0f6e 73 int thetaKa;
gianarjuna 1:a2c7dd0a0f6e 74 int thetaKi;
gianarjuna 1:a2c7dd0a0f6e 75 int pulseLangkah;
gatulz 3:8f57f69f6e64 76 double pulse_A, c_e_A, p_e_A,
gatulz 3:8f57f69f6e64 77 p2_e_A, prev_speedA, speedA;
gatulz 3:8f57f69f6e64 78 double target_pulse=0;
gatulz 3:8f57f69f6e64 79 int bouncingKi, bouncingKa, angkatKa, angkatKi;
gatulz 3:8f57f69f6e64 80 double pulsetempKa, pulsetempKi;
gatulz 3:8f57f69f6e64 81 int kiri, kanan;
gatulz 3:8f57f69f6e64 82 unsigned long lastStall;
gatulz 3:8f57f69f6e64 83 int stall,langkah;
gatulz 3:8f57f69f6e64 84
gatulz 3:8f57f69f6e64 85 double pulseAngkatA=75.0, //kanan belakang
gatulz 3:8f57f69f6e64 86 pulseAngkatB=95.0, //87 kiri depan
gatulz 3:8f57f69f6e64 87 pulseAngkatC= 465.0, //97 kiri belakang
gatulz 3:8f57f69f6e64 88 pulseAngkatD=580.0, //565 //kanan depan
gatulz 3:8f57f69f6e64 89 pulseTurunA=287.0, //kanan belakang
gatulz 3:8f57f69f6e64 90 pulseTurunB=277.0, //305 kiri depan
gatulz 3:8f57f69f6e64 91 pulseTurunC=700.0, //kiri belakang
gatulz 3:8f57f69f6e64 92 pulseTurunD=760; //785 kanan depan
gatulz 3:8f57f69f6e64 93 int pulseKi, pulseKa;
gatulz 3:8f57f69f6e64 94 int angkatA=0, angkatB=0, angkatC=0,angkatD=0;
gatulz 3:8f57f69f6e64 95 int mulai = 0;
gatulz 3:8f57f69f6e64 96
gatulz 3:8f57f69f6e64 97 void PIDMotor();
gianarjuna 1:a2c7dd0a0f6e 98 void hitungParameter();
gianarjuna 1:a2c7dd0a0f6e 99 void getHeading();
gianarjuna 1:a2c7dd0a0f6e 100 void hitungPIDTheta(double theta_sp);
gianarjuna 1:a2c7dd0a0f6e 101 void gerakMotor();
gatulz 3:8f57f69f6e64 102
gianarjuna 1:a2c7dd0a0f6e 103 void tambahPWM();
gianarjuna 1:a2c7dd0a0f6e 104 void debug();
gianarjuna 1:a2c7dd0a0f6e 105 void gerakDepan(int end);
gianarjuna 1:a2c7dd0a0f6e 106 void hitungOdometry();
gianarjuna 1:a2c7dd0a0f6e 107 void initPneu();
gatulz 3:8f57f69f6e64 108 void angkatKakiKi();
gatulz 3:8f57f69f6e64 109 void angkatKakiKa();
gatulz 3:8f57f69f6e64 110 void FSM();
gianarjuna 1:a2c7dd0a0f6e 111 void angkatKaki();
gianarjuna 1:a2c7dd0a0f6e 112 //int main(){
gianarjuna 1:a2c7dd0a0f6e 113 // while(1){
gatulz 3:8f57f69f6e64 114 // if (!Ubutton){
gatulz 3:8f57f69f6e64 115 // motorKa.speed(0.4);
gatulz 3:8f57f69f6e64 116 // motorKi.speed(0.4);
gatulz 3:8f57f69f6e64 117 // } else {
gatulz 3:8f57f69f6e64 118 // motorKa.speed(0.0);
gatulz 3:8f57f69f6e64 119 // motorKi.speed(0.0);
gatulz 3:8f57f69f6e64 120 // }
gianarjuna 1:a2c7dd0a0f6e 121 // }
gianarjuna 1:a2c7dd0a0f6e 122 //}
gatulz 3:8f57f69f6e64 123 int main() {
gatulz 3:8f57f69f6e64 124 pulseKi=0;
gatulz 3:8f57f69f6e64 125 pulseKa=0;
gatulz 3:8f57f69f6e64 126 speedA =0;
gianarjuna 1:a2c7dd0a0f6e 127 encSteer.reset();
gatulz 3:8f57f69f6e64 128 encKanan.reset();
gatulz 3:8f57f69f6e64 129 encKiri.reset();
gianarjuna 1:a2c7dd0a0f6e 130 startMillis();
gianarjuna 1:a2c7dd0a0f6e 131 theta = 0;
gianarjuna 1:a2c7dd0a0f6e 132 cp = 1;
gianarjuna 1:a2c7dd0a0f6e 133 state=0;
gianarjuna 1:a2c7dd0a0f6e 134 theta_sp =0;
gianarjuna 1:a2c7dd0a0f6e 135 ohstart=0;
gianarjuna 1:a2c7dd0a0f6e 136 KI = KI1;
gatulz 3:8f57f69f6e64 137
gatulz 3:8f57f69f6e64 138 angkatKi = 0;
gatulz 3:8f57f69f6e64 139 angkatKa = 0;
gianarjuna 1:a2c7dd0a0f6e 140 initPneu();
gatulz 3:8f57f69f6e64 141 kanan=0;
gatulz 3:8f57f69f6e64 142 mulai=0;
gianarjuna 1:a2c7dd0a0f6e 143 while(1) {
gatulz 3:8f57f69f6e64 144 // pc.printf("TES");
gatulz 3:8f57f69f6e64 145 // ohstart=1;
gatulz 3:8f57f69f6e64 146 // if (millis() - last_mt_print>17) {
gatulz 3:8f57f69f6e64 147 // pc.printf("TES %d - %.2f %.2f %.2f\n",ohstart,pulse_A,pulse_B, speedA);//encKanan.getPulses(), encKiri.getPulses());
gatulz 3:8f57f69f6e64 148 // last_mt_print=millis();
gatulz 3:8f57f69f6e64 149 // }
gatulz 3:8f57f69f6e64 150 // if (millis()>=10300)
gatulz 3:8f57f69f6e64 151 // ohstart = 0;
gatulz 3:8f57f69f6e64 152 // if(!Ubutton && millis() - last_mt_button>1000) { //mulai gerak maju
gatulz 3:8f57f69f6e64 153 // pulse_A +=0.3;
gatulz 3:8f57f69f6e64 154 // last_mt_button=millis();
gatulz 3:8f57f69f6e64 155 // }
gatulz 3:8f57f69f6e64 156 // if(!Ubutton) { //mulai gerak maju
gatulz 3:8f57f69f6e64 157 // wait_ms(2000); //diam 2 detik sebelum mulai
gatulz 3:8f57f69f6e64 158 // ohstart = 1;
gatulz 3:8f57f69f6e64 159 // state=1;
gatulz 3:8f57f69f6e64 160 // last_start=millis();
gatulz 3:8f57f69f6e64 161 // }
gatulz 3:8f57f69f6e64 162 //
gatulz 3:8f57f69f6e64 163 // if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
gatulz 3:8f57f69f6e64 164 // ohstart=0;
gatulz 3:8f57f69f6e64 165 // }
gatulz 3:8f57f69f6e64 166 //
gatulz 3:8f57f69f6e64 167 // if (ohstart) {
gatulz 3:8f57f69f6e64 168 //
gatulz 3:8f57f69f6e64 169 //
gatulz 3:8f57f69f6e64 170 // }
gatulz 3:8f57f69f6e64 171 if (millis()- last_mt_print > 17){
gianarjuna 1:a2c7dd0a0f6e 172 //pc.printf("%.2f\n",speed_maju);
gatulz 3:8f57f69f6e64 173 if(!switchKa){
gianarjuna 1:a2c7dd0a0f6e 174 kanan = 1;
gatulz 3:8f57f69f6e64 175
gatulz 3:8f57f69f6e64 176 // pc.printf("KENA Kanan\t");
gatulz 3:8f57f69f6e64 177 }
gatulz 3:8f57f69f6e64 178 else{
gianarjuna 1:a2c7dd0a0f6e 179 kanan = 0;
gatulz 3:8f57f69f6e64 180 bouncingKa = 0;
gianarjuna 1:a2c7dd0a0f6e 181 }
gatulz 3:8f57f69f6e64 182 if(!switchKi){
gianarjuna 1:a2c7dd0a0f6e 183 kiri = 1;
gatulz 3:8f57f69f6e64 184 // pc.printf("KENA Kiri\t");
gatulz 3:8f57f69f6e64 185 }
gatulz 3:8f57f69f6e64 186 else{
gianarjuna 1:a2c7dd0a0f6e 187 kiri = 0;
gatulz 3:8f57f69f6e64 188 bouncingKi = 0;
gianarjuna 1:a2c7dd0a0f6e 189 }
gianarjuna 1:a2c7dd0a0f6e 190 last_mt_print = millis();
gatulz 3:8f57f69f6e64 191 pc.printf("%d %d s = %d\t%d\t %d%d%d%d \n",
gatulz 3:8f57f69f6e64 192 kanan, ohstart,
gatulz 3:8f57f69f6e64 193 pulseKa, pulseKi, angkatA,
gatulz 3:8f57f69f6e64 194 angkatB, angkatC, mulai);//kanan,speedA, pulseKi, pulseKa);//,pingKidep.distance(),pingKadep.distance());
gianarjuna 1:a2c7dd0a0f6e 195 }
gatulz 3:8f57f69f6e64 196 if (millis() - lastAngkat>11){
gatulz 3:8f57f69f6e64 197 angkatKaki();
gatulz 3:8f57f69f6e64 198 lastAngkat=millis();
gatulz 3:8f57f69f6e64 199 }
gatulz 3:8f57f69f6e64 200 if (millis()>=28300)
gatulz 3:8f57f69f6e64 201 ohstart = 0;
gatulz 3:8f57f69f6e64 202 if(!Ubutton){ //mulai gerak maju
gianarjuna 1:a2c7dd0a0f6e 203 wait_ms(2000); //diam 2 detik sebelum mulai
gianarjuna 1:a2c7dd0a0f6e 204 ohstart = 1;
gianarjuna 1:a2c7dd0a0f6e 205 state=1;
gatulz 3:8f57f69f6e64 206 stall=0;
gatulz 3:8f57f69f6e64 207 lastStall=millis();
gatulz 3:8f57f69f6e64 208 //pwm_maju = 0.8;
gatulz 3:8f57f69f6e64 209 //speed_maju=0.7;
gatulz 3:8f57f69f6e64 210 target_pulse = 5.0;
gianarjuna 1:a2c7dd0a0f6e 211 last_start=millis();
gianarjuna 1:a2c7dd0a0f6e 212 }
gatulz 3:8f57f69f6e64 213 // urutan prioritas
gatulz 3:8f57f69f6e64 214 // if (millis() - last_mt_printPID > 7){
gatulz 3:8f57f69f6e64 215 // if (!(fabs(theta_sp- theta) < TOLERANCET)){
gatulz 3:8f57f69f6e64 216 // hitungPIDTheta(theta_sp);
gatulz 3:8f57f69f6e64 217 // }
gatulz 3:8f57f69f6e64 218 // last_mt_printPID = millis();
gatulz 3:8f57f69f6e64 219 // }
gatulz 3:8f57f69f6e64 220 //
gatulz 3:8f57f69f6e64 221 // if (millis() - lastCompass>3) {
gatulz 3:8f57f69f6e64 222 // getHeading();
gatulz 3:8f57f69f6e64 223 // lastCompass = millis();
gatulz 3:8f57f69f6e64 224 // }
gianarjuna 1:a2c7dd0a0f6e 225
gatulz 3:8f57f69f6e64 226 //state machine
gatulz 3:8f57f69f6e64 227 // if (millis() - last_start>7000) {
gatulz 3:8f57f69f6e64 228 // ohstart = 0; //kembali ke posisi awal (mati) setelah 15 detik
gatulz 3:8f57f69f6e64 229 // target_pulse=0;
gatulz 3:8f57f69f6e64 230 // }
gatulz 3:8f57f69f6e64 231 // if (((kidep_dist < 150)||(kadep_dist < 150)) && (state == 1)) {
gatulz 3:8f57f69f6e64 232 // //state = 2; //kuda mendeteksi tembok di kanan atau kiri, siap untuk belok
gatulz 3:8f57f69f6e64 233 // }
gatulz 3:8f57f69f6e64 234 // if (((kidep_dist < 30) || (kadep_dist < 30)) && (state == 2)) {
gatulz 3:8f57f69f6e64 235 // state = 3; //kuda deteksi obstacle, siap untuk angkat kaki
gatulz 3:8f57f69f6e64 236 // }
gatulz 3:8f57f69f6e64 237 // /*if (kondisi cek sudah lewat obstacle dan udh mau belok lagi) {
gatulz 3:8f57f69f6e64 238 // state = 4; //kuda belok lagi, siap untuk deteksi tali dan angkat kaki
gatulz 3:8f57f69f6e64 239 // }*/
gatulz 3:8f57f69f6e64 240 // /*if (kondisi deteksi tali) {
gatulz 3:8f57f69f6e64 241 // state 5; //angkat kaki
gatulz 3:8f57f69f6e64 242 // }*/
gatulz 3:8f57f69f6e64 243 // /*if (switch nanjak dipencet) {
gatulz 3:8f57f69f6e64 244 // state 6; //jalan lurus nanjak
gatulz 3:8f57f69f6e64 245 // }*/
gatulz 3:8f57f69f6e64 246 if (millis() - lastParameter > 5){
gatulz 3:8f57f69f6e64 247 hitungParameter();
gatulz 3:8f57f69f6e64 248 PIDMotor();
gatulz 3:8f57f69f6e64 249 lastParameter = millis();
gianarjuna 1:a2c7dd0a0f6e 250 }
315_josh 0:7ab5f1f9dcb8 251
gatulz 3:8f57f69f6e64 252 if (ohstart){
gatulz 3:8f57f69f6e64 253 if (millis() - lastFSM > 9){
gatulz 3:8f57f69f6e64 254 FSM();
gianarjuna 1:a2c7dd0a0f6e 255 lastFSM=millis();
gianarjuna 1:a2c7dd0a0f6e 256 }
gatulz 3:8f57f69f6e64 257 if (millis() - lastMotor>2){
gianarjuna 1:a2c7dd0a0f6e 258 gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 259 lastMotor=millis();
gianarjuna 1:a2c7dd0a0f6e 260 }
gatulz 3:8f57f69f6e64 261
gatulz 3:8f57f69f6e64 262 // angkatKakiKi();
gatulz 3:8f57f69f6e64 263 // angkatKakiKa();
gatulz 3:8f57f69f6e64 264 //
gatulz 3:8f57f69f6e64 265 // if (millis() - last_ping > 9) {
gatulz 3:8f57f69f6e64 266 // kidep_dist = pingKidep.distance();
gatulz 3:8f57f69f6e64 267 // kadep_dist = pingKadep.distance();
gatulz 3:8f57f69f6e64 268 // //belki_dist = pingKibel.distance();
gatulz 3:8f57f69f6e64 269 // //belka_dist = pingKabel.distance();
gatulz 3:8f57f69f6e64 270 // last_ping = millis();
gatulz 3:8f57f69f6e64 271 // }
gatulz 3:8f57f69f6e64 272 }
gatulz 3:8f57f69f6e64 273 else {
gatulz 3:8f57f69f6e64 274 target_pulse=0;
gatulz 3:8f57f69f6e64 275 motorKa.speed(0);
gatulz 3:8f57f69f6e64 276 motorKi.speed(0);
gatulz 3:8f57f69f6e64 277 // motorKa.brake(1);
gatulz 3:8f57f69f6e64 278 // motorKi.brake(1);
gatulz 3:8f57f69f6e64 279 }
gatulz 3:8f57f69f6e64 280 //fungsi untuk safety dari stall yang terlalu lama
gatulz 3:8f57f69f6e64 281 if (millis() - lastSafety>19){
gatulz 3:8f57f69f6e64 282 if (pulse_A<=0.01 && !stall){
gatulz 3:8f57f69f6e64 283 lastStall=millis();
gatulz 3:8f57f69f6e64 284 stall=1;
gatulz 3:8f57f69f6e64 285 }
gatulz 3:8f57f69f6e64 286 if (pulse_A>0.3 && stall){
gatulz 3:8f57f69f6e64 287 lastStall=millis();
gatulz 3:8f57f69f6e64 288 stall=0;
gatulz 3:8f57f69f6e64 289 }
gatulz 3:8f57f69f6e64 290 if (millis() - lastStall>3000 && stall){
gatulz 3:8f57f69f6e64 291 ohstart=0;
gatulz 3:8f57f69f6e64 292 target_pulse=0;
gatulz 3:8f57f69f6e64 293 }
gianarjuna 1:a2c7dd0a0f6e 294 }
gianarjuna 1:a2c7dd0a0f6e 295
gatulz 3:8f57f69f6e64 296 //fungsi angkatKaki
gatulz 3:8f57f69f6e64 297 //if (millis() - last_start>5000)
gatulz 3:8f57f69f6e64 298 if (mulai>3){
gatulz 3:8f57f69f6e64 299 // pneu1[1]=1;
gatulz 3:8f57f69f6e64 300 // pneu2[1]=1;
gatulz 3:8f57f69f6e64 301 angkatKaki();
gatulz 3:8f57f69f6e64 302 }else{
gatulz 3:8f57f69f6e64 303 pneu1[0]=0;
gatulz 3:8f57f69f6e64 304 pneu1[1]=0;
gatulz 3:8f57f69f6e64 305 pneu2[0]=0;
gatulz 3:8f57f69f6e64 306 pneu2[1]=0;
gatulz 3:8f57f69f6e64 307 }
gianarjuna 1:a2c7dd0a0f6e 308 }
gianarjuna 1:a2c7dd0a0f6e 309
gianarjuna 1:a2c7dd0a0f6e 310 }
gianarjuna 1:a2c7dd0a0f6e 311
gatulz 3:8f57f69f6e64 312 void FSM(){
gatulz 3:8f57f69f6e64 313 if (state==1){ //state maju lurus
gatulz 3:8f57f69f6e64 314 theta_sp=0;
gatulz 3:8f57f69f6e64 315 belok=0;
gatulz 3:8f57f69f6e64 316 //pwm_maju=0.80;
gatulz 3:8f57f69f6e64 317 target_pulse = 3;
gatulz 3:8f57f69f6e64 318 }
gatulz 3:8f57f69f6e64 319 else if (state == 2) { //state belok, sudah deteksi tembok
gatulz 3:8f57f69f6e64 320 //theta_target = 15.0;
gatulz 3:8f57f69f6e64 321 //ubahTheta();
gatulz 3:8f57f69f6e64 322 //belok = 1;
gatulz 3:8f57f69f6e64 323 //KI = KI2;
gatulz 3:8f57f69f6e64 324 //pwm_maju = 0.85;
gatulz 3:8f57f69f6e64 325 }
gatulz 3:8f57f69f6e64 326 else if (state == 3) { //state angkat kaki, udh deteksi obstacle
gatulz 3:8f57f69f6e64 327 //kuda deteksi obstacle, angkat kaki
gatulz 3:8f57f69f6e64 328 }
gatulz 3:8f57f69f6e64 329 /*else if (state == 4) { //state belok lagi, udh lewat sand dune
gatulz 3:8f57f69f6e64 330 //kuda belok lagi,
gatulz 3:8f57f69f6e64 331 }*/
gatulz 3:8f57f69f6e64 332 /*else if (state == 5) { //state angkat kaki, deteksi tali
gatulz 3:8f57f69f6e64 333 //kuda deteksi tali, angkat kaki
gatulz 3:8f57f69f6e64 334 }*/
gatulz 3:8f57f69f6e64 335 /*else if (state == 6) { //state naik gunung
gatulz 3:8f57f69f6e64 336 //jalan lurus nanjak
gatulz 3:8f57f69f6e64 337 }*/
gatulz 3:8f57f69f6e64 338 }
gatulz 3:8f57f69f6e64 339 void hitungParameter(){
gianarjuna 1:a2c7dd0a0f6e 340 pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 341 encSteer.reset();
gianarjuna 1:a2c7dd0a0f6e 342 theta += pulse;
gianarjuna 1:a2c7dd0a0f6e 343 }
gatulz 3:8f57f69f6e64 344 void gerakMotor()
gianarjuna 1:a2c7dd0a0f6e 345 {
gatulz 3:8f57f69f6e64 346 // state=1;
gatulz 3:8f57f69f6e64 347 // if (!Ubutton){
gatulz 3:8f57f69f6e64 348 // speedA = 0.78;
gatulz 3:8f57f69f6e64 349 // }
gatulz 3:8f57f69f6e64 350 // else
gatulz 3:8f57f69f6e64 351 // speedA = 0.0;
gatulz 3:8f57f69f6e64 352 // speedA=1.0;
gatulz 3:8f57f69f6e64 353 if (speedA > 0.15 && (ohstart)) {
gatulz 3:8f57f69f6e64 354 // if (ohstart){
gatulz 3:8f57f69f6e64 355 motorKa.speed(speedA);
gatulz 3:8f57f69f6e64 356 motorKi.speed(speedA);
gatulz 3:8f57f69f6e64 357 } else {
gatulz 3:8f57f69f6e64 358 motorKa.speed(0);
gatulz 3:8f57f69f6e64 359 motorKi.speed(0);
gatulz 3:8f57f69f6e64 360 // motorKa.brake(1);
gatulz 3:8f57f69f6e64 361 // motorKi.brake(1);
gatulz 3:8f57f69f6e64 362 }
gatulz 3:8f57f69f6e64 363 }
gatulz 3:8f57f69f6e64 364
gatulz 3:8f57f69f6e64 365 void PIDMotor(){
gatulz 3:8f57f69f6e64 366 //target_pulse = 10.0;
gatulz 3:8f57f69f6e64 367 pulse_A = (double)encKiri.getPulses();
gatulz 3:8f57f69f6e64 368 //pulse_B = (double)encKanan.getPulses();
gatulz 3:8f57f69f6e64 369 //encKanan.reset();
gatulz 3:8f57f69f6e64 370 encKiri.reset();
gatulz 3:8f57f69f6e64 371 pulseKi += (int)pulse_A;
gatulz 3:8f57f69f6e64 372 pulseKa += (int)pulse_A;
gatulz 3:8f57f69f6e64 373 // pulseKa = pulseKa%360;
gatulz 3:8f57f69f6e64 374 // pulseKi = pulseKa%360;
gatulz 3:8f57f69f6e64 375
gatulz 3:8f57f69f6e64 376 // if (millis()-lastStall>2000){
gatulz 3:8f57f69f6e64 377 // ohstart=0;
gatulz 3:8f57f69f6e64 378 // speedA=0;
gatulz 3:8f57f69f6e64 379 // target_pulse=0;
gatulz 3:8f57f69f6e64 380 // state=0;
gatulz 3:8f57f69f6e64 381 // }
gatulz 3:8f57f69f6e64 382
gatulz 3:8f57f69f6e64 383 c_e_A = (double) target_pulse - pulse_A;
gatulz 3:8f57f69f6e64 384 // thetaKa += pulse_A;
gatulz 3:8f57f69f6e64 385 speedA = (double) ( prev_speedA +
gatulz 3:8f57f69f6e64 386 Kp_A*(c_e_A - p_e_A) +
gatulz 3:8f57f69f6e64 387 Ki_A*TS*(c_e_A + p_e_A)/2 +
gatulz 3:8f57f69f6e64 388 Kd_A*(c_e_A - 2*p_e_A + p2_e_A) );
gatulz 3:8f57f69f6e64 389 if (speedA>0.90)
gatulz 3:8f57f69f6e64 390 speedA = 0.90;
gatulz 3:8f57f69f6e64 391 else if (speed<-0.90)
gatulz 3:8f57f69f6e64 392 speedA = -0.90;
gatulz 3:8f57f69f6e64 393 if (!ohstart)
gatulz 3:8f57f69f6e64 394 speedA=0.0;
gatulz 3:8f57f69f6e64 395 prev_speedA = speedA;
gatulz 3:8f57f69f6e64 396 p2_e_A = p_e_A;
gatulz 3:8f57f69f6e64 397 p_e_A = c_e_A;
gatulz 3:8f57f69f6e64 398 }
gatulz 3:8f57f69f6e64 399 void initPneu(){
gianarjuna 1:a2c7dd0a0f6e 400 pneu1[0]=0;
gianarjuna 1:a2c7dd0a0f6e 401 pneu1[1]=0;
gianarjuna 1:a2c7dd0a0f6e 402 pneu2[0]=0;
gianarjuna 1:a2c7dd0a0f6e 403 pneu2[1]=0;
gianarjuna 1:a2c7dd0a0f6e 404 }
gatulz 3:8f57f69f6e64 405 //
gatulz 3:8f57f69f6e64 406 //void angkatKakiKi(){
gatulz 3:8f57f69f6e64 407 // int batasKi = 15;
gatulz 3:8f57f69f6e64 408 // if(!switchKi && bouncingKi == 0){
gatulz 3:8f57f69f6e64 409 // bouncingKi = 1;
gatulz 3:8f57f69f6e64 410 // pulsetempKi = 0;
gatulz 3:8f57f69f6e64 411 // }
gatulz 3:8f57f69f6e64 412 // if (pulsetempKi > batasKi && pulsetempKi < (batasKi + 95) && !angkatKi){
gatulz 3:8f57f69f6e64 413 // pneu2[0] = 1; //blkg
gatulz 3:8f57f69f6e64 414 // pneu2[1] = 1; //dpn
gatulz 3:8f57f69f6e64 415 // angkatKi = 1;
gatulz 3:8f57f69f6e64 416 // }
gatulz 3:8f57f69f6e64 417 // else if (pulsetempKi > (batasKi+120) && angkatKi){
gatulz 3:8f57f69f6e64 418 // pneu2[0] = 0;
gatulz 3:8f57f69f6e64 419 // pneu2[1] = 0;
gatulz 3:8f57f69f6e64 420 // angkatKi = 0;
gatulz 3:8f57f69f6e64 421 // bouncingKi = 0;
gatulz 3:8f57f69f6e64 422 // }
gatulz 3:8f57f69f6e64 423 //}
gatulz 3:8f57f69f6e64 424 //void angkatKakiKa(){
gatulz 3:8f57f69f6e64 425 // int batasKa = 0;
gatulz 3:8f57f69f6e64 426 // if(!switchKa && bouncingKa == 0){
gatulz 3:8f57f69f6e64 427 // bouncingKa = 1;
gatulz 3:8f57f69f6e64 428 // pulsetempKa = 0;
gatulz 3:8f57f69f6e64 429 // }
gatulz 3:8f57f69f6e64 430 // if (pulsetempKa > batasKa && pulsetempKa < (batasKa+130) && !angkatKa){
gatulz 3:8f57f69f6e64 431 // pneu1[0] = 1;
gatulz 3:8f57f69f6e64 432 // pneu1[1] = 1;
gatulz 3:8f57f69f6e64 433 // angkatKa = 1;
gatulz 3:8f57f69f6e64 434 // }
gatulz 3:8f57f69f6e64 435 // else if (pulsetempKa > (batasKa+130) && angkatKa){
gatulz 3:8f57f69f6e64 436 // pneu1[0] = 0;
gatulz 3:8f57f69f6e64 437 // pneu1[1] = 0;
gatulz 3:8f57f69f6e64 438 // angkatKa = 0;
gatulz 3:8f57f69f6e64 439 // bouncingKa = 0;
gatulz 3:8f57f69f6e64 440 // }
gatulz 3:8f57f69f6e64 441 //}
gatulz 3:8f57f69f6e64 442 void getHeading() {
gianarjuna 1:a2c7dd0a0f6e 443 heading_temp = (cmps.getAngle()/10 - heading0); //cari perubahan sudut badan
gianarjuna 1:a2c7dd0a0f6e 444 if(heading_temp > 180.0 && heading_temp <= 360.0) //kondisi agar pembacaan 0 - 180 derajat
gianarjuna 1:a2c7dd0a0f6e 445 heading = (heading_temp - 360.0 );
gianarjuna 1:a2c7dd0a0f6e 446 else if(heading_temp < -180.0 && heading_temp >= -360.0)
gianarjuna 1:a2c7dd0a0f6e 447 heading = (heading_temp + 360.0 );
gianarjuna 1:a2c7dd0a0f6e 448 else
gianarjuna 1:a2c7dd0a0f6e 449 heading = heading_temp;
gianarjuna 1:a2c7dd0a0f6e 450 heading = heading*(-1); //output heading
gianarjuna 1:a2c7dd0a0f6e 451 }
gatulz 3:8f57f69f6e64 452 void hitungPIDTheta(double theta_sp){
gianarjuna 1:a2c7dd0a0f6e 453 if (theta_sp>dThetaMax)
gianarjuna 1:a2c7dd0a0f6e 454 theta_sp=dThetaMax;
gianarjuna 1:a2c7dd0a0f6e 455 theta_error = theta_sp - theta;
gatulz 3:8f57f69f6e64 456 if (theta_error > 180){
gianarjuna 1:a2c7dd0a0f6e 457 theta_error = theta_error - 360;
gatulz 3:8f57f69f6e64 458 }
gatulz 3:8f57f69f6e64 459 else if (theta_error < -179){
gianarjuna 1:a2c7dd0a0f6e 460 theta_error = theta_error + 360;
gianarjuna 1:a2c7dd0a0f6e 461 }
gianarjuna 1:a2c7dd0a0f6e 462
gianarjuna 1:a2c7dd0a0f6e 463 sum_theta_error += (theta_error);
gianarjuna 1:a2c7dd0a0f6e 464 w =KP * theta_error + KI *sum_theta_error*TS;
gianarjuna 1:a2c7dd0a0f6e 465 speed = w;
gianarjuna 1:a2c7dd0a0f6e 466 theta_error_prev = theta_error;
gianarjuna 1:a2c7dd0a0f6e 467
gatulz 3:8f57f69f6e64 468 if(speed >= PWM_MAX){
gianarjuna 1:a2c7dd0a0f6e 469 pwm = PWM_MAX;
gatulz 3:8f57f69f6e64 470 } else if (speed <= PWM_MIN){
gianarjuna 1:a2c7dd0a0f6e 471 pwm = PWM_MIN;
gianarjuna 1:a2c7dd0a0f6e 472 } else
gianarjuna 1:a2c7dd0a0f6e 473 pwm = speed;
gianarjuna 1:a2c7dd0a0f6e 474 }
gatulz 3:8f57f69f6e64 475 void tambahPWM(){
gatulz 3:8f57f69f6e64 476 if (millis() - lastPWM>50){
gatulz 3:8f57f69f6e64 477 if (speed_maju>pwm_maju){
gianarjuna 1:a2c7dd0a0f6e 478 // speed_maju -=0.05;
gatulz 3:8f57f69f6e64 479 }
gatulz 3:8f57f69f6e64 480 else if (speed_maju<pwm_maju){
gatulz 3:8f57f69f6e64 481 speed_maju+=0.05;
315_josh 0:7ab5f1f9dcb8 482 }
315_josh 0:7ab5f1f9dcb8 483 }
gianarjuna 1:a2c7dd0a0f6e 484 lastPWM = millis();
gianarjuna 1:a2c7dd0a0f6e 485 }
gatulz 3:8f57f69f6e64 486 void debug(){ //program utk cek konstanta derajat
gianarjuna 1:a2c7dd0a0f6e 487 theta = encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 488 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 489 // motorKa.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 490 // motorKi.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 491 // else
gianarjuna 1:a2c7dd0a0f6e 492 // motorKa.speed(0);
gianarjuna 1:a2c7dd0a0f6e 493 // motorKi.speed(0);
gianarjuna 1:a2c7dd0a0f6e 494 // if (temp>20.0){
gianarjuna 1:a2c7dd0a0f6e 495 // belok = 1;
gianarjuna 1:a2c7dd0a0f6e 496 // }
gianarjuna 1:a2c7dd0a0f6e 497 // if (temp<-20.0){
gianarjuna 1:a2c7dd0a0f6e 498 // belok=0;
gianarjuna 1:a2c7dd0a0f6e 499 // }
gianarjuna 1:a2c7dd0a0f6e 500 // if (millis() - lastCompass>3){
gianarjuna 1:a2c7dd0a0f6e 501 // getHeading();
gianarjuna 1:a2c7dd0a0f6e 502 // lastCompass=millis();
gianarjuna 1:a2c7dd0a0f6e 503 // }
gianarjuna 1:a2c7dd0a0f6e 504 //// if (!belok)
gianarjuna 1:a2c7dd0a0f6e 505 //// motorSteer.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 506 //// else
gianarjuna 1:a2c7dd0a0f6e 507 //// motorSteer.speed(-1*speed_maju);
gianarjuna 1:a2c7dd0a0f6e 508 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 509 // theta_sp=20.0;
gianarjuna 1:a2c7dd0a0f6e 510 // else
gianarjuna 1:a2c7dd0a0f6e 511 // theta_sp=-20.0;
gianarjuna 1:a2c7dd0a0f6e 512 //
gianarjuna 1:a2c7dd0a0f6e 513 // if (millis() - last_mt_printPID > 7){
gianarjuna 1:a2c7dd0a0f6e 514 // if (!(fabs(theta_sp- theta) < TOLERANCET)){
gianarjuna 1:a2c7dd0a0f6e 515 // hitungPIDTheta(theta_sp);
gianarjuna 1:a2c7dd0a0f6e 516 // }
gianarjuna 1:a2c7dd0a0f6e 517 // last_mt_printPID = millis();
gianarjuna 1:a2c7dd0a0f6e 518 // }
gianarjuna 1:a2c7dd0a0f6e 519 // gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 520 // //motorSteer.speed(pwm);
gianarjuna 1:a2c7dd0a0f6e 521 // printf("pulse = %.2f\t theta = %.2f theta = %.2f pidT=%.2f pwm=%.2f\n",
gianarjuna 1:a2c7dd0a0f6e 522 // theta, heading, theta_error, w,pwm);
gianarjuna 1:a2c7dd0a0f6e 523 }
gianarjuna 1:a2c7dd0a0f6e 524
gatulz 3:8f57f69f6e64 525 void ubahTheta() {
gatulz 3:8f57f69f6e64 526 if (millis() - lastThetaTarget >23) {
gatulz 3:8f57f69f6e64 527 if (theta_sp < theta_target) {
gatulz 3:8f57f69f6e64 528 theta_sp += 1.0;
gatulz 3:8f57f69f6e64 529 }
gatulz 3:8f57f69f6e64 530 else if (theta_sp > theta_target) {
gatulz 3:8f57f69f6e64 531 theta_sp -= 1.0;
gatulz 3:8f57f69f6e64 532 }
gatulz 3:8f57f69f6e64 533 lastThetaTarget = millis();
gatulz 3:8f57f69f6e64 534 }
gatulz 3:8f57f69f6e64 535 }
gianarjuna 1:a2c7dd0a0f6e 536
gatulz 3:8f57f69f6e64 537 void angkatKaki(){
gatulz 3:8f57f69f6e64 538 //1 putaran 650 pulse
gatulz 3:8f57f69f6e64 539 // pulseKa=pulseKa%890;
gatulz 3:8f57f69f6e64 540 // pulseKi=pulseKi%650;
gatulz 3:8f57f69f6e64 541
gatulz 3:8f57f69f6e64 542 if(kiri && bouncingKi == 0){
gatulz 3:8f57f69f6e64 543 bouncingKi = 1;
gatulz 3:8f57f69f6e64 544 // pulseKi = 0;
gatulz 3:8f57f69f6e64 545 // pulseKa = 325;
gatulz 3:8f57f69f6e64 546 }
gatulz 3:8f57f69f6e64 547 if (pulseKa>900){
gatulz 3:8f57f69f6e64 548 mulai++;
gatulz 3:8f57f69f6e64 549 pulseKa=0;
gatulz 3:8f57f69f6e64 550 }
gatulz 3:8f57f69f6e64 551 else if(kanan && bouncingKa == 0){
gatulz 3:8f57f69f6e64 552 bouncingKa = 1;
gatulz 3:8f57f69f6e64 553 pulseKi = 325;
gatulz 3:8f57f69f6e64 554 pulseKa = 0;
gatulz 3:8f57f69f6e64 555 mulai++;
gatulz 3:8f57f69f6e64 556 }
gatulz 3:8f57f69f6e64 557 // //if (millis()>9000){
gatulz 3:8f57f69f6e64 558 //// if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
gatulz 3:8f57f69f6e64 559 //// pneu1[0] = 1; //blkg
gatulz 3:8f57f69f6e64 560 //// angkatA = 1;
gatulz 3:8f57f69f6e64 561 //// bouncingKi = 0;
gatulz 3:8f57f69f6e64 562 //// }
gatulz 3:8f57f69f6e64 563 //// else if (pulseKi>pulseTurunA && angkatA){
gatulz 3:8f57f69f6e64 564 //// pneu1[0] = 0;
gatulz 3:8f57f69f6e64 565 //// angkatA = 0;
gatulz 3:8f57f69f6e64 566 //// }
gatulz 3:8f57f69f6e64 567 // if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
gatulz 3:8f57f69f6e64 568 // pneu1[0] = 0; //kiri dpn
gatulz 3:8f57f69f6e64 569 // angkatB = 0;
gatulz 3:8f57f69f6e64 570 // }
gatulz 3:8f57f69f6e64 571 // else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
gatulz 3:8f57f69f6e64 572 // pneu1[0] = 1; //kiri dpn
gatulz 3:8f57f69f6e64 573 // angkatB = 1;
gatulz 3:8f57f69f6e64 574 // }
gatulz 3:8f57f69f6e64 575 //// if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC && !angkatC){
gatulz 3:8f57f69f6e64 576 //// pneu2[0] = 1; //blkg
gatulz 3:8f57f69f6e64 577 //// angkatC = 1;
gatulz 3:8f57f69f6e64 578 //// }
gatulz 3:8f57f69f6e64 579 //// else if (pulseKa>pulseTurunC && angkatC){
gatulz 3:8f57f69f6e64 580 //// pneu2[0] = 0;
gatulz 3:8f57f69f6e64 581 //// angkatC = 0;
gatulz 3:8f57f69f6e64 582 //// bouncingKa = 0;
gatulz 3:8f57f69f6e64 583 //// }
gatulz 3:8f57f69f6e64 584 // if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
gatulz 3:8f57f69f6e64 585 // pneu2[0] = 0; //kanan dpn
gatulz 3:8f57f69f6e64 586 // angkatD = 0;
gatulz 3:8f57f69f6e64 587 // }
gatulz 3:8f57f69f6e64 588 // else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
gatulz 3:8f57f69f6e64 589 // pneu2[0] = 1; //kanan dpn
gatulz 3:8f57f69f6e64 590 // angkatD = 1;
gatulz 3:8f57f69f6e64 591 //
gatulz 3:8f57f69f6e64 592 // }
gatulz 3:8f57f69f6e64 593 //// if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
gatulz 3:8f57f69f6e64 594 //// pneu1[0] = 1; //blkg
gatulz 3:8f57f69f6e64 595 //// angkatA = 1;
gatulz 3:8f57f69f6e64 596 //// bouncingKi = 0;
gatulz 3:8f57f69f6e64 597 //// }
gatulz 3:8f57f69f6e64 598 //// else if (pulseKi>pulseTurunA && angkatA){
gatulz 3:8f57f69f6e64 599 //// pneu1[0] = 0;
gatulz 3:8f57f69f6e64 600 //// angkatA = 0;
gatulz 3:8f57f69f6e64 601 //// }
gatulz 3:8f57f69f6e64 602 if (mulai>9){
gatulz 3:8f57f69f6e64 603
gatulz 3:8f57f69f6e64 604 } else if (mulai>3 && mulai<7){
gatulz 3:8f57f69f6e64 605 if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
gatulz 3:8f57f69f6e64 606 pneu1[1] = 1; //kiri dpn
gatulz 3:8f57f69f6e64 607 angkatB = 1;
gatulz 3:8f57f69f6e64 608 }
gatulz 3:8f57f69f6e64 609 else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
gatulz 3:8f57f69f6e64 610 pneu1[1] = 0; //kiri dpn
gatulz 3:8f57f69f6e64 611 angkatB = 0;
gatulz 3:8f57f69f6e64 612 }
gatulz 3:8f57f69f6e64 613 if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
gatulz 3:8f57f69f6e64 614 pneu2[1] = 1; //kanan dpn
gatulz 3:8f57f69f6e64 615 angkatD = 1;
gatulz 3:8f57f69f6e64 616 }
gatulz 3:8f57f69f6e64 617 else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
gatulz 3:8f57f69f6e64 618 pneu2[1] = 0; //kanan dpn
gatulz 3:8f57f69f6e64 619 angkatD = 0;
gatulz 3:8f57f69f6e64 620 }
gatulz 3:8f57f69f6e64 621 } else if (mulai>=7 && mulai<10){
gatulz 3:8f57f69f6e64 622 if (mulai<9){
gatulz 3:8f57f69f6e64 623 pneu1[1]=1;
gatulz 3:8f57f69f6e64 624 pneu2[1]=1;
gatulz 3:8f57f69f6e64 625 } else{
gatulz 3:8f57f69f6e64 626 pneu1[1]=0;
gatulz 3:8f57f69f6e64 627 pneu2[1]=0;
gatulz 3:8f57f69f6e64 628 }
gatulz 3:8f57f69f6e64 629 if (pulseKa > pulseAngkatA && pulseKa < pulseTurunA){
gatulz 3:8f57f69f6e64 630 pneu1[0] = 1; //blkg kanan
gatulz 3:8f57f69f6e64 631 angkatA = 1;
gatulz 3:8f57f69f6e64 632 //bouncingKi = 0;
gatulz 3:8f57f69f6e64 633 }
gatulz 3:8f57f69f6e64 634 else if (pulseKa < pulseAngkatA ||pulseKa>pulseTurunA){
gatulz 3:8f57f69f6e64 635 pneu1[0] = 0;
gatulz 3:8f57f69f6e64 636 angkatA = 0;
gatulz 3:8f57f69f6e64 637 }
gatulz 3:8f57f69f6e64 638
gatulz 3:8f57f69f6e64 639 if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC){
gatulz 3:8f57f69f6e64 640 pneu2[0] = 1; //blkg kiri
gatulz 3:8f57f69f6e64 641 angkatC = 1;
gatulz 3:8f57f69f6e64 642 }
gatulz 3:8f57f69f6e64 643 else if (pulseKa < pulseAngkatC || pulseKa>pulseTurunC){
gatulz 3:8f57f69f6e64 644 pneu2[0] = 0;
gatulz 3:8f57f69f6e64 645 angkatC = 0;
gatulz 3:8f57f69f6e64 646 //bouncingKa = 0;
gatulz 3:8f57f69f6e64 647 }
gatulz 3:8f57f69f6e64 648 } else if(mulai>10 && mulai<12){
gatulz 3:8f57f69f6e64 649 pneu1[1]=0;
gatulz 3:8f57f69f6e64 650 pneu2[1]=0;
gatulz 3:8f57f69f6e64 651 pneu1[0]=1;
gatulz 3:8f57f69f6e64 652 pneu2[0]=1;
gatulz 3:8f57f69f6e64 653 }
gianarjuna 1:a2c7dd0a0f6e 654 }