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@2:0351727f6354, 2019-04-04 (annotated)

Committer:
gianarjuna
Date:
Thu Apr 04 04:17:26 2019 +0000
Revision:
2:0351727f6354
Parent:
1:a2c7dd0a0f6e 
+ ping

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 2:0351727f6354 7 #include "hcsr04.h"
gianarjuna 1:a2c7dd0a0f6e 8
gianarjuna 1:a2c7dd0a0f6e 9 #define PULSE_TO_DEG 0.135
gianarjuna 1:a2c7dd0a0f6e 10 #define PWM_MAX 0.85
gianarjuna 1:a2c7dd0a0f6e 11 #define PWM_MIN -0.85
gianarjuna 1:a2c7dd0a0f6e 12 #define TOLERANCET 1.0
gianarjuna 1:a2c7dd0a0f6e 13 #define TS 7.0
315_josh 0:7ab5f1f9dcb8 14
gianarjuna 1:a2c7dd0a0f6e 15 CMPS12_KRAI cmps(PB_3, PB_10, 0xc0);
gianarjuna 1:a2c7dd0a0f6e 16 encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING); //encoder motor steering
gianarjuna 1:a2c7dd0a0f6e 17 encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 18 encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 19 Motor motorSteer(PB_14, PC_4, PB_13); //motor arah putaran CCW ++
gianarjuna 1:a2c7dd0a0f6e 20 Motor motorKa(PA_11, PA_7,PB_12); //motor kanan
gianarjuna 1:a2c7dd0a0f6e 21 Motor motorKi(PB_15, PB_1, PB_2); //motor kiri
gianarjuna 1:a2c7dd0a0f6e 22 DigitalIn Ubutton(USER_BUTTON);
gianarjuna 1:a2c7dd0a0f6e 23 Serial pc(USBTX, USBRX, 115200);
gianarjuna 1:a2c7dd0a0f6e 24 DigitalIn switchKi(PA_9,PullUp);
gianarjuna 1:a2c7dd0a0f6e 25 DigitalIn switchKa(PD_2,PullUp);
gianarjuna 2:0351727f6354 26 DigitalOut pneu1[2] = {(PB_0),(PA_1)}; //kanan belakang , kiri depan
gianarjuna 2:0351727f6354 27 DigitalOut pneu2[2] = {(PC_1),(PA_0)}; //kiri belakang, kanan depan
gianarjuna 2:0351727f6354 28 HCSR04 pingKidep(PC_12, PA_13); //TRIGGER , ECHO
gianarjuna 2:0351727f6354 29 HCSR04 pingKadep(PC_15, PC_14);
gianarjuna 2:0351727f6354 30 //HCSR04 pingKibel(PB_4, PA_8);
gianarjuna 2:0351727f6354 31 //HCSR04 pingKabel(PB_, PA_);
gianarjuna 2:0351727f6354 32
gianarjuna 2:0351727f6354 33
gianarjuna 2:0351727f6354 34
gianarjuna 2:0351727f6354 35 int pulsetempKi;
gianarjuna 2:0351727f6354 36 int pulsetempKa;
gianarjuna 1:a2c7dd0a0f6e 37 float theta;
gianarjuna 1:a2c7dd0a0f6e 38 float target;
gianarjuna 1:a2c7dd0a0f6e 39 int cp = 0;
gianarjuna 1:a2c7dd0a0f6e 40 int brake_state = 0;
gianarjuna 1:a2c7dd0a0f6e 41 float sum_theta_error;
gianarjuna 1:a2c7dd0a0f6e 42 float theta_error_prev;
gianarjuna 1:a2c7dd0a0f6e 43 float theta_target;
gianarjuna 1:a2c7dd0a0f6e 44 float speed;
gianarjuna 1:a2c7dd0a0f6e 45 float w;
gianarjuna 1:a2c7dd0a0f6e 46 float theta_sp;
gianarjuna 1:a2c7dd0a0f6e 47 float theta_error;
gianarjuna 1:a2c7dd0a0f6e 48 float heading_temp, heading0, heading;
gianarjuna 1:a2c7dd0a0f6e 49 double pulse = 0;
gianarjuna 1:a2c7dd0a0f6e 50 double KI;
gianarjuna 2:0351727f6354 51 int angkatKi;
gianarjuna 2:0351727f6354 52 int angkatKa;
gianarjuna 2:0351727f6354 53 int bouncingKi = 0;
gianarjuna 2:0351727f6354 54 int bouncingKa = 0;
gianarjuna 2:0351727f6354 55
gianarjuna 2:0351727f6354 56 const double KP = 0.05 ,
gianarjuna 2:0351727f6354 57 KI1 = 0.00000,
gianarjuna 2:0351727f6354 58 KI2 = 0.00000;
gianarjuna 1:a2c7dd0a0f6e 59 unsigned long lastParameter, //millis utk perhintungan parameter
gianarjuna 2:0351727f6354 60 last_mt_printPID,
gianarjuna 2:0351727f6354 61 last_mt_print, //millis utk print
gianarjuna 2:0351727f6354 62 last_mt_button, //millis utk user button
gianarjuna 2:0351727f6354 63 last_start, //millis start uji coba kuda
gianarjuna 2:0351727f6354 64 lastFSM, //finite state machine millis
gianarjuna 2:0351727f6354 65 lastPWM,
gianarjuna 2:0351727f6354 66 lastMotor,
gianarjuna 2:0351727f6354 67 lastCompass,
gianarjuna 2:0351727f6354 68 last_ping,
gianarjuna 2:0351727f6354 69 lastThetaTarget;
gianarjuna 1:a2c7dd0a0f6e 70 int start;
gianarjuna 1:a2c7dd0a0f6e 71 int state=0;
gianarjuna 1:a2c7dd0a0f6e 72 float pwm;
gianarjuna 1:a2c7dd0a0f6e 73 float pwm_maju=0.0;
gianarjuna 1:a2c7dd0a0f6e 74 float temp = 0;
gianarjuna 1:a2c7dd0a0f6e 75 float dThetaMax = 35.0;
gianarjuna 1:a2c7dd0a0f6e 76 float speed_maju=0;
gianarjuna 1:a2c7dd0a0f6e 77 int ohstart = 0;
gianarjuna 1:a2c7dd0a0f6e 78 int belok =0;
gianarjuna 2:0351727f6354 79 int kidep_dist,kadep_dist, kibel_dist, kabel_dist;
gianarjuna 1:a2c7dd0a0f6e 80 int thetaKa;
gianarjuna 1:a2c7dd0a0f6e 81 int thetaKi;
gianarjuna 2:0351727f6354 82 int pulseLangkahKi;
gianarjuna 2:0351727f6354 83 int pulseLangkahKa;
gianarjuna 1:a2c7dd0a0f6e 84 void hitungParameter();
gianarjuna 1:a2c7dd0a0f6e 85 void getHeading();
gianarjuna 1:a2c7dd0a0f6e 86 void hitungPIDTheta(double theta_sp);
gianarjuna 1:a2c7dd0a0f6e 87 void gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 88 void tambahPWM();
gianarjuna 1:a2c7dd0a0f6e 89 void debug();
gianarjuna 1:a2c7dd0a0f6e 90 void gerakDepan(int end);
gianarjuna 1:a2c7dd0a0f6e 91 void hitungOdometry();
gianarjuna 1:a2c7dd0a0f6e 92 void initPneu();
gianarjuna 2:0351727f6354 93 void angkatKakiKi();
gianarjuna 2:0351727f6354 94 void angkatKakiKa();
gianarjuna 1:a2c7dd0a0f6e 95 //int main(){
gianarjuna 1:a2c7dd0a0f6e 96 // startMillis();
gianarjuna 1:a2c7dd0a0f6e 97 // speed_maju=0.3;
gianarjuna 1:a2c7dd0a0f6e 98 // heading0=(float)cmps.getAngle()/10;
gianarjuna 1:a2c7dd0a0f6e 99 // while(1){
gianarjuna 1:a2c7dd0a0f6e 100 // debug();
gianarjuna 1:a2c7dd0a0f6e 101 // //pc.printf("TEST\n");
gianarjuna 1:a2c7dd0a0f6e 102 // }
gianarjuna 1:a2c7dd0a0f6e 103 //}
gianarjuna 1:a2c7dd0a0f6e 104
gianarjuna 1:a2c7dd0a0f6e 105
gianarjuna 2:0351727f6354 106 int main(){
gianarjuna 1:a2c7dd0a0f6e 107 encSteer.reset();
gianarjuna 2:0351727f6354 108 encKanan.reset();
gianarjuna 2:0351727f6354 109 encKiri.reset();
gianarjuna 1:a2c7dd0a0f6e 110 startMillis();
gianarjuna 1:a2c7dd0a0f6e 111 theta = 0;
gianarjuna 1:a2c7dd0a0f6e 112 cp = 1;
gianarjuna 1:a2c7dd0a0f6e 113 state=0;
gianarjuna 1:a2c7dd0a0f6e 114 theta_sp =0;
gianarjuna 1:a2c7dd0a0f6e 115 ohstart=0;
gianarjuna 1:a2c7dd0a0f6e 116 KI = KI1;
gianarjuna 2:0351727f6354 117 angkatKi = 0;
gianarjuna 2:0351727f6354 118 angkatKa = 0;
gianarjuna 1:a2c7dd0a0f6e 119 int kanan;
gianarjuna 1:a2c7dd0a0f6e 120 int kiri;
gianarjuna 1:a2c7dd0a0f6e 121 initPneu();
gianarjuna 2:0351727f6354 122
gianarjuna 2:0351727f6354 123 while(1){
gianarjuna 2:0351727f6354 124 if (millis()- last_mt_print > 15){
gianarjuna 1:a2c7dd0a0f6e 125 //pc.printf("%.2f\n",speed_maju);
gianarjuna 2:0351727f6354 126 if(!switchKa){
gianarjuna 2:0351727f6354 127 kanan = 1;
gianarjuna 2:0351727f6354 128 pc.printf("KENA Kanan\t");
gianarjuna 1:a2c7dd0a0f6e 129 }
gianarjuna 2:0351727f6354 130 else{kanan = 0;}
gianarjuna 2:0351727f6354 131 if(!switchKi){
gianarjuna 1:a2c7dd0a0f6e 132 kiri = 1;
gianarjuna 2:0351727f6354 133 pc.printf("KENA Kiri\t");
gianarjuna 2:0351727f6354 134 }
gianarjuna 2:0351727f6354 135 else{kiri = 0;}
gianarjuna 1:a2c7dd0a0f6e 136 last_mt_print = millis();
gianarjuna 2:0351727f6354 137 pc.printf("%d %d s = %.2f puls = %d %d \t%d %d \n",kiri,kanan,speed_maju, pulsetempKi, pulsetempKa,pingKidep.distance(),pingKadep.distance());
gianarjuna 1:a2c7dd0a0f6e 138 }
gianarjuna 2:0351727f6354 139 if(!Ubutton){ //mulai gerak maju
gianarjuna 1:a2c7dd0a0f6e 140 wait_ms(2000); //diam 2 detik sebelum mulai
gianarjuna 1:a2c7dd0a0f6e 141 ohstart = 1;
gianarjuna 1:a2c7dd0a0f6e 142 state=1;
gianarjuna 1:a2c7dd0a0f6e 143 pwm_maju = 0.8;
gianarjuna 1:a2c7dd0a0f6e 144 speed_maju=0.7;
gianarjuna 1:a2c7dd0a0f6e 145 last_start=millis();
gianarjuna 1:a2c7dd0a0f6e 146 }
gianarjuna 1:a2c7dd0a0f6e 147
gianarjuna 2:0351727f6354 148 //state machine
gianarjuna 2:0351727f6354 149 if (millis() - last_start>15000) {
gianarjuna 2:0351727f6354 150 ohstart = 0; //kembali ke posisi awal (mati) setelah 15 detik
gianarjuna 2:0351727f6354 151 }
gianarjuna 2:0351727f6354 152 if (((kidep_dist < 150)||(kadep_dist < 150)) && (state == 1)) {
gianarjuna 2:0351727f6354 153 //state = 2; //kuda mendeteksi tembok di kanan atau kiri, siap untuk belok
gianarjuna 2:0351727f6354 154 }
gianarjuna 2:0351727f6354 155 if (((kidep_dist < 30) || (kadep_dist < 30)) && (state == 2)) {
gianarjuna 2:0351727f6354 156 state = 3; //kuda deteksi obstacle, siap untuk angkat kaki
gianarjuna 1:a2c7dd0a0f6e 157 }
gianarjuna 2:0351727f6354 158 /*if (kondisi cek sudah lewat obstacle dan udh mau belok lagi) {
gianarjuna 2:0351727f6354 159 state = 4; //kuda belok lagi, siap untuk deteksi tali dan angkat kaki
gianarjuna 2:0351727f6354 160 }*/
gianarjuna 2:0351727f6354 161 /*if (kondisi deteksi tali) {
gianarjuna 2:0351727f6354 162 state 5; //angkat kaki
gianarjuna 2:0351727f6354 163 }*/
gianarjuna 2:0351727f6354 164 /*if (switch nanjak dipencet) {
gianarjuna 2:0351727f6354 165 state 6; //jalan lurus nanjak
gianarjuna 2:0351727f6354 166 }*/
315_josh 0:7ab5f1f9dcb8 167
gianarjuna 2:0351727f6354 168 if (ohstart){
gianarjuna 2:0351727f6354 169 angkatKakiKi();
gianarjuna 2:0351727f6354 170 //angkatKakiKa();
gianarjuna 2:0351727f6354 171 if (millis() - lastFSM > 9){
gianarjuna 2:0351727f6354 172 if (state==1){ //state maju lurus
gianarjuna 1:a2c7dd0a0f6e 173 theta_sp=0;
gianarjuna 1:a2c7dd0a0f6e 174 belok=0;
gianarjuna 1:a2c7dd0a0f6e 175 pwm_maju=0.80;
gianarjuna 1:a2c7dd0a0f6e 176 }
gianarjuna 2:0351727f6354 177 else if (state == 2) { //state belok, sudah deteksi tembok
gianarjuna 2:0351727f6354 178 //theta_target = 15.0;
gianarjuna 2:0351727f6354 179 //ubahTheta();
gianarjuna 2:0351727f6354 180 //belok = 1;
gianarjuna 2:0351727f6354 181 //KI = KI2;
gianarjuna 2:0351727f6354 182 //pwm_maju = 0.85;
gianarjuna 2:0351727f6354 183 }
gianarjuna 2:0351727f6354 184 else if (state == 3) { //state angkat kaki, udh deteksi obstacle
gianarjuna 2:0351727f6354 185 //kuda deteksi obstacle, angkat kaki
gianarjuna 2:0351727f6354 186 }
gianarjuna 2:0351727f6354 187 /*else if (state == 4) { //state belok lagi, udh lewat sand dune
gianarjuna 2:0351727f6354 188 //kuda belok lagi,
gianarjuna 2:0351727f6354 189 }*/
gianarjuna 2:0351727f6354 190 /*else if (state == 5) { //state angkat kaki, deteksi tali
gianarjuna 2:0351727f6354 191 //kuda deteksi tali, angkat kaki
gianarjuna 2:0351727f6354 192 }*/
gianarjuna 2:0351727f6354 193 /*else if (state == 6) { //state naik gunung
gianarjuna 2:0351727f6354 194 //jalan lurus nanjak
gianarjuna 2:0351727f6354 195 }*/
gianarjuna 1:a2c7dd0a0f6e 196 lastFSM=millis();
gianarjuna 1:a2c7dd0a0f6e 197 }
gianarjuna 1:a2c7dd0a0f6e 198 //urutan prioritas
gianarjuna 2:0351727f6354 199 if (millis() - last_mt_printPID > 7){
gianarjuna 2:0351727f6354 200 if (!(fabs(theta_sp- theta) < TOLERANCET)){
gianarjuna 1:a2c7dd0a0f6e 201 hitungPIDTheta(theta_sp);
gianarjuna 1:a2c7dd0a0f6e 202 }
gianarjuna 1:a2c7dd0a0f6e 203 last_mt_printPID = millis();
gianarjuna 1:a2c7dd0a0f6e 204 }
gianarjuna 2:0351727f6354 205 if (millis() - lastParameter > 5){
gianarjuna 1:a2c7dd0a0f6e 206 hitungParameter();
gianarjuna 1:a2c7dd0a0f6e 207 lastParameter = millis();
gianarjuna 2:0351727f6354 208 }
gianarjuna 2:0351727f6354 209 if (millis() - lastMotor>4){
gianarjuna 1:a2c7dd0a0f6e 210 gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 211 lastMotor=millis();
gianarjuna 1:a2c7dd0a0f6e 212 }
gianarjuna 2:0351727f6354 213 if (millis() - lastCompass>3) {
gianarjuna 2:0351727f6354 214 getHeading();
gianarjuna 2:0351727f6354 215 lastCompass = millis();
gianarjuna 2:0351727f6354 216 }
gianarjuna 2:0351727f6354 217 if (millis() - last_ping > 9) {
gianarjuna 2:0351727f6354 218 kidep_dist = pingKidep.distance();
gianarjuna 2:0351727f6354 219 kadep_dist = pingKadep.distance();
gianarjuna 2:0351727f6354 220 //belki_dist = pingKibel.distance();
gianarjuna 2:0351727f6354 221 //belka_dist = pingKabel.distance();
gianarjuna 2:0351727f6354 222 last_ping = millis();
gianarjuna 2:0351727f6354 223 }
gianarjuna 1:a2c7dd0a0f6e 224 }
gianarjuna 1:a2c7dd0a0f6e 225
gianarjuna 1:a2c7dd0a0f6e 226 }
gianarjuna 1:a2c7dd0a0f6e 227
gianarjuna 1:a2c7dd0a0f6e 228 }
gianarjuna 1:a2c7dd0a0f6e 229
gianarjuna 2:0351727f6354 230 void hitungParameter(){
gianarjuna 1:a2c7dd0a0f6e 231 pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 232 encSteer.reset();
gianarjuna 2:0351727f6354 233 pulseLangkahKi = (int)encKiri.getPulses();
gianarjuna 2:0351727f6354 234 pulseLangkahKa = (int)encKanan.getPulses();
gianarjuna 1:a2c7dd0a0f6e 235 encKiri.reset();
gianarjuna 2:0351727f6354 236 encKanan.reset();
gianarjuna 2:0351727f6354 237 pulsetempKi += pulseLangkahKi;
gianarjuna 2:0351727f6354 238 pulsetempKa += pulseLangkahKa;
gianarjuna 1:a2c7dd0a0f6e 239 theta += pulse;
gianarjuna 1:a2c7dd0a0f6e 240 }
gianarjuna 2:0351727f6354 241 void initPneu(){
gianarjuna 1:a2c7dd0a0f6e 242 pneu1[0]=0;
gianarjuna 1:a2c7dd0a0f6e 243 pneu1[1]=0;
gianarjuna 1:a2c7dd0a0f6e 244 pneu2[0]=0;
gianarjuna 1:a2c7dd0a0f6e 245 pneu2[1]=0;
gianarjuna 1:a2c7dd0a0f6e 246 }
gianarjuna 1:a2c7dd0a0f6e 247
gianarjuna 2:0351727f6354 248 void angkatKakiKi(){
gianarjuna 2:0351727f6354 249 int batasKi = 15;
gianarjuna 2:0351727f6354 250 if(!switchKi && bouncingKi == 0){
gianarjuna 2:0351727f6354 251 bouncingKi = 1;
gianarjuna 2:0351727f6354 252 pulsetempKi = 0;
gianarjuna 1:a2c7dd0a0f6e 253 }
gianarjuna 2:0351727f6354 254 if (pulsetempKi > batasKi && pulsetempKi < (batasKi + 95) && !angkatKi){
gianarjuna 2:0351727f6354 255 pneu2[0] = 1; //blkg
gianarjuna 2:0351727f6354 256 pneu2[1] = 1; //dpn
gianarjuna 2:0351727f6354 257 angkatKi = 1;
gianarjuna 2:0351727f6354 258 }
gianarjuna 2:0351727f6354 259 else if (pulsetempKi > (batasKi+120) && angkatKi){
gianarjuna 1:a2c7dd0a0f6e 260 pneu2[0] = 0;
gianarjuna 1:a2c7dd0a0f6e 261 pneu2[1] = 0;
gianarjuna 2:0351727f6354 262 angkatKi = 0;
gianarjuna 2:0351727f6354 263 bouncingKi = 0;
gianarjuna 1:a2c7dd0a0f6e 264 }
gianarjuna 1:a2c7dd0a0f6e 265 }
gianarjuna 1:a2c7dd0a0f6e 266
gianarjuna 2:0351727f6354 267 void angkatKakiKa(){
gianarjuna 2:0351727f6354 268 int batasKa = 0;
gianarjuna 2:0351727f6354 269 if(!switchKa && bouncingKa == 0){
gianarjuna 2:0351727f6354 270 bouncingKa = 1;
gianarjuna 2:0351727f6354 271 pulsetempKa = 0;
gianarjuna 2:0351727f6354 272 }
gianarjuna 2:0351727f6354 273 if (pulsetempKa > batasKa && pulsetempKa < (batasKa+130) && !angkatKa){
gianarjuna 2:0351727f6354 274 pneu1[0] = 1;
gianarjuna 2:0351727f6354 275 pneu1[1] = 1;
gianarjuna 2:0351727f6354 276 angkatKa = 1;
gianarjuna 2:0351727f6354 277 }
gianarjuna 2:0351727f6354 278 else if (pulsetempKa > (batasKa+130) && angkatKa){
gianarjuna 2:0351727f6354 279 pneu1[0] = 0;
gianarjuna 2:0351727f6354 280 pneu1[1] = 0;
gianarjuna 2:0351727f6354 281 angkatKa = 0;
gianarjuna 2:0351727f6354 282 bouncingKa = 0;
gianarjuna 2:0351727f6354 283 }
gianarjuna 1:a2c7dd0a0f6e 284 }
gianarjuna 1:a2c7dd0a0f6e 285
gianarjuna 2:0351727f6354 286 void getHeading() {
gianarjuna 1:a2c7dd0a0f6e 287 heading_temp = (cmps.getAngle()/10 - heading0); //cari perubahan sudut badan
gianarjuna 1:a2c7dd0a0f6e 288 if(heading_temp > 180.0 && heading_temp <= 360.0) //kondisi agar pembacaan 0 - 180 derajat
gianarjuna 1:a2c7dd0a0f6e 289 heading = (heading_temp - 360.0 );
gianarjuna 1:a2c7dd0a0f6e 290 else if(heading_temp < -180.0 && heading_temp >= -360.0)
gianarjuna 1:a2c7dd0a0f6e 291 heading = (heading_temp + 360.0 );
gianarjuna 1:a2c7dd0a0f6e 292 else
gianarjuna 1:a2c7dd0a0f6e 293 heading = heading_temp;
gianarjuna 1:a2c7dd0a0f6e 294 heading = heading*(-1); //output heading
gianarjuna 1:a2c7dd0a0f6e 295 }
gianarjuna 2:0351727f6354 296 void hitungPIDTheta(double theta_sp){
gianarjuna 1:a2c7dd0a0f6e 297 if (theta_sp>dThetaMax)
gianarjuna 1:a2c7dd0a0f6e 298 theta_sp=dThetaMax;
gianarjuna 1:a2c7dd0a0f6e 299 theta_error = theta_sp - theta;
gianarjuna 2:0351727f6354 300 if (theta_error > 180){
gianarjuna 1:a2c7dd0a0f6e 301 theta_error = theta_error - 360;
gianarjuna 2:0351727f6354 302 }
gianarjuna 2:0351727f6354 303 else if (theta_error < -179){
gianarjuna 1:a2c7dd0a0f6e 304 theta_error = theta_error + 360;
gianarjuna 1:a2c7dd0a0f6e 305 }
gianarjuna 1:a2c7dd0a0f6e 306
gianarjuna 1:a2c7dd0a0f6e 307 sum_theta_error += (theta_error);
gianarjuna 1:a2c7dd0a0f6e 308 w =KP * theta_error + KI *sum_theta_error*TS;
gianarjuna 1:a2c7dd0a0f6e 309 speed = w;
gianarjuna 1:a2c7dd0a0f6e 310 theta_error_prev = theta_error;
gianarjuna 1:a2c7dd0a0f6e 311
gianarjuna 2:0351727f6354 312 if(speed >= PWM_MAX){
gianarjuna 1:a2c7dd0a0f6e 313 pwm = PWM_MAX;
gianarjuna 2:0351727f6354 314 } else if (speed <= PWM_MIN){
gianarjuna 1:a2c7dd0a0f6e 315 pwm = PWM_MIN;
gianarjuna 1:a2c7dd0a0f6e 316 } else
gianarjuna 1:a2c7dd0a0f6e 317 pwm = speed;
gianarjuna 1:a2c7dd0a0f6e 318 }
gianarjuna 2:0351727f6354 319 void gerakMotor(){
gianarjuna 1:a2c7dd0a0f6e 320 //if ((theta_error > 1.0 && (fabs(pwm) > 0.15)))// && (state!=0)))
gianarjuna 1:a2c7dd0a0f6e 321 motorSteer.speed(pwm);
gianarjuna 1:a2c7dd0a0f6e 322 // else {
gianarjuna 1:a2c7dd0a0f6e 323 // motorSteer.speed(0.0);
gianarjuna 1:a2c7dd0a0f6e 324 // motorSteer.brake(BRAKE_HIGH);
gianarjuna 1:a2c7dd0a0f6e 325 // }
gianarjuna 1:a2c7dd0a0f6e 326
gianarjuna 1:a2c7dd0a0f6e 327 tambahPWM();
gianarjuna 2:0351727f6354 328 if ( (state>0) && (fabs(speed_maju) > 0.15) && (ohstart)){
gianarjuna 1:a2c7dd0a0f6e 329 motorKa.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 330 motorKi.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 331 } else {
gianarjuna 1:a2c7dd0a0f6e 332 motorKa.speed(0);
gianarjuna 1:a2c7dd0a0f6e 333 motorKi.speed(0);
gianarjuna 1:a2c7dd0a0f6e 334 motorKa.brake(1);
gianarjuna 1:a2c7dd0a0f6e 335 motorKi.brake(1);
gianarjuna 1:a2c7dd0a0f6e 336 }
gianarjuna 1:a2c7dd0a0f6e 337 }
gianarjuna 1:a2c7dd0a0f6e 338
gianarjuna 2:0351727f6354 339 void tambahPWM(){
gianarjuna 2:0351727f6354 340 if (millis() - lastPWM>50){
gianarjuna 2:0351727f6354 341 if (speed_maju>pwm_maju){
gianarjuna 1:a2c7dd0a0f6e 342 // speed_maju -=0.05;
315_josh 0:7ab5f1f9dcb8 343 }
gianarjuna 2:0351727f6354 344 else if (speed_maju<pwm_maju){
gianarjuna 2:0351727f6354 345 speed_maju+=0.05;
gianarjuna 2:0351727f6354 346 }
315_josh 0:7ab5f1f9dcb8 347 }
gianarjuna 1:a2c7dd0a0f6e 348 lastPWM = millis();
gianarjuna 1:a2c7dd0a0f6e 349 }
gianarjuna 2:0351727f6354 350 void debug(){ //program utk cek konstanta derajat
gianarjuna 1:a2c7dd0a0f6e 351 theta = encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 352 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 353 // motorKa.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 354 // motorKi.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 355 // else
gianarjuna 1:a2c7dd0a0f6e 356 // motorKa.speed(0);
gianarjuna 1:a2c7dd0a0f6e 357 // motorKi.speed(0);
gianarjuna 1:a2c7dd0a0f6e 358 // if (temp>20.0){
gianarjuna 1:a2c7dd0a0f6e 359 // belok = 1;
gianarjuna 1:a2c7dd0a0f6e 360 // }
gianarjuna 1:a2c7dd0a0f6e 361 // if (temp<-20.0){
gianarjuna 1:a2c7dd0a0f6e 362 // belok=0;
gianarjuna 1:a2c7dd0a0f6e 363 // }
gianarjuna 1:a2c7dd0a0f6e 364 // if (millis() - lastCompass>3){
gianarjuna 1:a2c7dd0a0f6e 365 // getHeading();
gianarjuna 1:a2c7dd0a0f6e 366 // lastCompass=millis();
gianarjuna 1:a2c7dd0a0f6e 367 // }
gianarjuna 1:a2c7dd0a0f6e 368 //// if (!belok)
gianarjuna 1:a2c7dd0a0f6e 369 //// motorSteer.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 370 //// else
gianarjuna 1:a2c7dd0a0f6e 371 //// motorSteer.speed(-1*speed_maju);
gianarjuna 1:a2c7dd0a0f6e 372 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 373 // theta_sp=20.0;
gianarjuna 1:a2c7dd0a0f6e 374 // else
gianarjuna 1:a2c7dd0a0f6e 375 // theta_sp=-20.0;
gianarjuna 1:a2c7dd0a0f6e 376 //
gianarjuna 1:a2c7dd0a0f6e 377 // if (millis() - last_mt_printPID > 7){
gianarjuna 1:a2c7dd0a0f6e 378 // if (!(fabs(theta_sp- theta) < TOLERANCET)){
gianarjuna 1:a2c7dd0a0f6e 379 // hitungPIDTheta(theta_sp);
gianarjuna 1:a2c7dd0a0f6e 380 // }
gianarjuna 1:a2c7dd0a0f6e 381 // last_mt_printPID = millis();
gianarjuna 1:a2c7dd0a0f6e 382 // }
gianarjuna 1:a2c7dd0a0f6e 383 // gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 384 // //motorSteer.speed(pwm);
gianarjuna 1:a2c7dd0a0f6e 385 // printf("pulse = %.2f\t theta = %.2f theta = %.2f pidT=%.2f pwm=%.2f\n",
gianarjuna 1:a2c7dd0a0f6e 386 // theta, heading, theta_error, w,pwm);
gianarjuna 1:a2c7dd0a0f6e 387 }
gianarjuna 1:a2c7dd0a0f6e 388
gianarjuna 2:0351727f6354 389 void ubahTheta() {
gianarjuna 2:0351727f6354 390 if (millis() - lastThetaTarget >23) {
gianarjuna 2:0351727f6354 391 if (theta_sp < theta_target) {
gianarjuna 2:0351727f6354 392 theta_sp += 1.0;
gianarjuna 2:0351727f6354 393 }
gianarjuna 2:0351727f6354 394 else if (theta_sp > theta_target) {
gianarjuna 2:0351727f6354 395 theta_sp -= 1.0;
gianarjuna 2:0351727f6354 396 }
gianarjuna 2:0351727f6354 397 lastThetaTarget = millis();
gianarjuna 2:0351727f6354 398 }
gianarjuna 2:0351727f6354 399 }
gianarjuna 1:a2c7dd0a0f6e 400
gianarjuna 2:0351727f6354 401 void hitungOdometry(){
gianarjuna 1:a2c7dd0a0f6e 402 //Tolong isi program odometry utk pisisi robot terhadap lapangan
gianarjuna 1:a2c7dd0a0f6e 403 //berdasarkan geometry robot dan posisi ping (asumsi ping diletakan di tengah kaki bagian luar
gianarjuna 1:a2c7dd0a0f6e 404 //ukuran robot minta ama mekanik
gianarjuna 1:a2c7dd0a0f6e 405 /* POSISI PING utk di kaki depan
gianarjuna 1:a2c7dd0a0f6e 406 PING PING
gianarjuna 1:a2c7dd0a0f6e 407 #### ####
gianarjuna 1:a2c7dd0a0f6e 408 # # # #
gianarjuna 1:a2c7dd0a0f6e 409 # # # #
gianarjuna 2:0351727f6354 410 PING# # # # PING
gianarjuna 1:a2c7dd0a0f6e 411 # # # #
gianarjuna 1:a2c7dd0a0f6e 412 # # # #
gianarjuna 1:a2c7dd0a0f6e 413 #### ####
gianarjuna 1:a2c7dd0a0f6e 414 */
gianarjuna 2:0351727f6354 415 }