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@1:a2c7dd0a0f6e, 2019-03-29 (annotated)

Committer:
gianarjuna
Date:
Fri Mar 29 11:31:22 2019 +0000
Revision:
1:a2c7dd0a0f6e
Parent:
0:7ab5f1f9dcb8
Child:
2:0351727f6354
Child:
3:8f57f69f6e64
main untuk cek fasa kaki kuda dengan switch

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
gianarjuna 1:a2c7dd0a0f6e 14 CMPS12_KRAI cmps(PB_3, PB_10, 0xc0);
gianarjuna 1:a2c7dd0a0f6e 15 encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING); //encoder motor steering
gianarjuna 1:a2c7dd0a0f6e 16 encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 17 encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING);
gianarjuna 1:a2c7dd0a0f6e 18 Motor motorSteer(PB_14, PC_4, PB_13); //motor arah putaran CCW ++
gianarjuna 1:a2c7dd0a0f6e 19 Motor motorKa(PA_11, PA_7,PB_12); //motor kanan
gianarjuna 1:a2c7dd0a0f6e 20 Motor motorKi(PB_15, PB_1, PB_2); //motor kiri
gianarjuna 1:a2c7dd0a0f6e 21 DigitalIn Ubutton(USER_BUTTON);
gianarjuna 1:a2c7dd0a0f6e 22 Serial pc(USBTX, USBRX, 115200);
gianarjuna 1:a2c7dd0a0f6e 23 DigitalIn switchKi(PA_9,PullUp);
gianarjuna 1:a2c7dd0a0f6e 24 DigitalIn switchKa(PD_2,PullUp);
gianarjuna 1:a2c7dd0a0f6e 25 DigitalOut pneu1[2] = {(PC_6),(PC_8)}; //kanan belakang , kiri depan
gianarjuna 1:a2c7dd0a0f6e 26 DigitalOut pneu2[2] = {(PC_5),(PC_9)}; //kiri belakang, kanan depan
gianarjuna 1:a2c7dd0a0f6e 27 int pulsetemp;
gianarjuna 1:a2c7dd0a0f6e 28 float theta;
gianarjuna 1:a2c7dd0a0f6e 29 float target;
gianarjuna 1:a2c7dd0a0f6e 30 int cp = 0;
gianarjuna 1:a2c7dd0a0f6e 31 int brake_state = 0;
gianarjuna 1:a2c7dd0a0f6e 32 float sum_theta_error;
gianarjuna 1:a2c7dd0a0f6e 33 float theta_error_prev;
gianarjuna 1:a2c7dd0a0f6e 34 float theta_target;
gianarjuna 1:a2c7dd0a0f6e 35 float speed;
gianarjuna 1:a2c7dd0a0f6e 36 float w;
gianarjuna 1:a2c7dd0a0f6e 37 float theta_sp;
gianarjuna 1:a2c7dd0a0f6e 38 float theta_error;
gianarjuna 1:a2c7dd0a0f6e 39 float heading_temp, heading0, heading;
gianarjuna 1:a2c7dd0a0f6e 40 double pulse = 0;
gianarjuna 1:a2c7dd0a0f6e 41 double KI;
gianarjuna 1:a2c7dd0a0f6e 42 int angkat;
gianarjuna 1:a2c7dd0a0f6e 43 int bouncing = 0;
gianarjuna 1:a2c7dd0a0f6e 44 const double KP = 0.05,
gianarjuna 1:a2c7dd0a0f6e 45 KI1 = 0.00000,
gianarjuna 1:a2c7dd0a0f6e 46 KI2 = 0.00000;
gianarjuna 1:a2c7dd0a0f6e 47 unsigned long lastParameter, //millis utk perhintungan parameter
gianarjuna 1:a2c7dd0a0f6e 48 last_mt_printPID,
gianarjuna 1:a2c7dd0a0f6e 49 last_mt_print, //millis utk print
gianarjuna 1:a2c7dd0a0f6e 50 last_mt_button, //millis utk user button
gianarjuna 1:a2c7dd0a0f6e 51 last_start, //millis start uji coba kuda
gianarjuna 1:a2c7dd0a0f6e 52 lastFSM, //finite state machine millis
gianarjuna 1:a2c7dd0a0f6e 53 lastPWM,
gianarjuna 1:a2c7dd0a0f6e 54 lastMotor,
gianarjuna 1:a2c7dd0a0f6e 55 lastCompass,
gianarjuna 1:a2c7dd0a0f6e 56 lastThetaTarget;
gianarjuna 1:a2c7dd0a0f6e 57 int start;
gianarjuna 1:a2c7dd0a0f6e 58 int state=0;
gianarjuna 1:a2c7dd0a0f6e 59 float pwm;
gianarjuna 1:a2c7dd0a0f6e 60 float pwm_maju=0.0;
gianarjuna 1:a2c7dd0a0f6e 61 float temp = 0;
gianarjuna 1:a2c7dd0a0f6e 62 float dThetaMax = 35.0;
gianarjuna 1:a2c7dd0a0f6e 63 float speed_maju=0;
gianarjuna 1:a2c7dd0a0f6e 64 int ohstart = 0;
gianarjuna 1:a2c7dd0a0f6e 65 int belok =0;
gianarjuna 1:a2c7dd0a0f6e 66 int thetaKa;
gianarjuna 1:a2c7dd0a0f6e 67 int thetaKi;
gianarjuna 1:a2c7dd0a0f6e 68 int pulseLangkah;
gianarjuna 1:a2c7dd0a0f6e 69 void hitungParameter();
gianarjuna 1:a2c7dd0a0f6e 70 void getHeading();
gianarjuna 1:a2c7dd0a0f6e 71 void hitungPIDTheta(double theta_sp);
gianarjuna 1:a2c7dd0a0f6e 72 void gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 73 void tambahPWM();
gianarjuna 1:a2c7dd0a0f6e 74 void debug();
gianarjuna 1:a2c7dd0a0f6e 75 void gerakDepan(int end);
gianarjuna 1:a2c7dd0a0f6e 76 void hitungOdometry();
gianarjuna 1:a2c7dd0a0f6e 77 void hitungParameterKa();
gianarjuna 1:a2c7dd0a0f6e 78 void hitungParameterKi();
gianarjuna 1:a2c7dd0a0f6e 79 void initPneu();
gianarjuna 1:a2c7dd0a0f6e 80 void angkatKaki();
gianarjuna 1:a2c7dd0a0f6e 81 //int main(){
gianarjuna 1:a2c7dd0a0f6e 82 // startMillis();
gianarjuna 1:a2c7dd0a0f6e 83 // speed_maju=0.3;
gianarjuna 1:a2c7dd0a0f6e 84 // heading0=(float)cmps.getAngle()/10;
gianarjuna 1:a2c7dd0a0f6e 85 // while(1){
gianarjuna 1:a2c7dd0a0f6e 86 // debug();
gianarjuna 1:a2c7dd0a0f6e 87 // //pc.printf("TEST\n");
gianarjuna 1:a2c7dd0a0f6e 88 // }
gianarjuna 1:a2c7dd0a0f6e 89 //}
gianarjuna 1:a2c7dd0a0f6e 90
gianarjuna 1:a2c7dd0a0f6e 91
gianarjuna 1:a2c7dd0a0f6e 92 int main()
gianarjuna 1:a2c7dd0a0f6e 93 {
gianarjuna 1:a2c7dd0a0f6e 94 encSteer.reset();
gianarjuna 1:a2c7dd0a0f6e 95 startMillis();
gianarjuna 1:a2c7dd0a0f6e 96 theta = 0;
gianarjuna 1:a2c7dd0a0f6e 97 cp = 1;
gianarjuna 1:a2c7dd0a0f6e 98 state=0;
gianarjuna 1:a2c7dd0a0f6e 99 theta_sp =0;
gianarjuna 1:a2c7dd0a0f6e 100 ohstart=0;
gianarjuna 1:a2c7dd0a0f6e 101 KI = KI1;
gianarjuna 1:a2c7dd0a0f6e 102 angkat = 0;
gianarjuna 1:a2c7dd0a0f6e 103 int kanan;
gianarjuna 1:a2c7dd0a0f6e 104 int kiri;
gianarjuna 1:a2c7dd0a0f6e 105 initPneu();
gianarjuna 1:a2c7dd0a0f6e 106 while(1) {
gianarjuna 1:a2c7dd0a0f6e 107 if (millis()- last_mt_print > 15) {
gianarjuna 1:a2c7dd0a0f6e 108 //pc.printf("%.2f\n",speed_maju);
gianarjuna 1:a2c7dd0a0f6e 109 if(!switchKa) {
gianarjuna 1:a2c7dd0a0f6e 110 kanan = 1;
gianarjuna 1:a2c7dd0a0f6e 111 pc.printf("KENA ");
gianarjuna 1:a2c7dd0a0f6e 112 } else {
gianarjuna 1:a2c7dd0a0f6e 113 kanan = 0;
gianarjuna 1:a2c7dd0a0f6e 114 }
gianarjuna 1:a2c7dd0a0f6e 115 if(!switchKi) {
gianarjuna 1:a2c7dd0a0f6e 116 kiri = 1;
gianarjuna 1:a2c7dd0a0f6e 117 } else {
gianarjuna 1:a2c7dd0a0f6e 118 kiri = 0;
gianarjuna 1:a2c7dd0a0f6e 119 }
gianarjuna 1:a2c7dd0a0f6e 120 last_mt_print = millis();
gianarjuna 1:a2c7dd0a0f6e 121 pc.printf("%d %d %.2f %d \n",kiri,kanan,speed_maju, pulsetemp);
gianarjuna 1:a2c7dd0a0f6e 122 }
gianarjuna 1:a2c7dd0a0f6e 123 if(!Ubutton) { //mulai gerak maju
gianarjuna 1:a2c7dd0a0f6e 124 wait_ms(2000); //diam 2 detik sebelum mulai
gianarjuna 1:a2c7dd0a0f6e 125 ohstart = 1;
gianarjuna 1:a2c7dd0a0f6e 126 state=1;
gianarjuna 1:a2c7dd0a0f6e 127 pwm_maju = 0.8;
gianarjuna 1:a2c7dd0a0f6e 128 speed_maju=0.7;
gianarjuna 1:a2c7dd0a0f6e 129 last_start=millis();
gianarjuna 1:a2c7dd0a0f6e 130 }
gianarjuna 1:a2c7dd0a0f6e 131
gianarjuna 1:a2c7dd0a0f6e 132 if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
gianarjuna 1:a2c7dd0a0f6e 133 ohstart=0;
gianarjuna 1:a2c7dd0a0f6e 134 }
315_josh 0:7ab5f1f9dcb8 135
315_josh 0:7ab5f1f9dcb8 136
gianarjuna 1:a2c7dd0a0f6e 137 if (ohstart) {
gianarjuna 1:a2c7dd0a0f6e 138 angkatKaki();
gianarjuna 1:a2c7dd0a0f6e 139 if (millis() - lastFSM > 9) {
gianarjuna 1:a2c7dd0a0f6e 140 if (state==1) { //state maju lurus
gianarjuna 1:a2c7dd0a0f6e 141 theta_sp=0;
gianarjuna 1:a2c7dd0a0f6e 142 belok=0;
gianarjuna 1:a2c7dd0a0f6e 143 pwm_maju=0.80;
gianarjuna 1:a2c7dd0a0f6e 144 }
gianarjuna 1:a2c7dd0a0f6e 145 lastFSM=millis();
gianarjuna 1:a2c7dd0a0f6e 146 }
gianarjuna 1:a2c7dd0a0f6e 147 //urutan prioritas
gianarjuna 1:a2c7dd0a0f6e 148 if (millis() - last_mt_printPID > 7) {
gianarjuna 1:a2c7dd0a0f6e 149 if (!(fabs(theta_sp- theta) < TOLERANCET)) {
gianarjuna 1:a2c7dd0a0f6e 150 hitungPIDTheta(theta_sp);
gianarjuna 1:a2c7dd0a0f6e 151 }
gianarjuna 1:a2c7dd0a0f6e 152 last_mt_printPID = millis();
gianarjuna 1:a2c7dd0a0f6e 153 }
gianarjuna 1:a2c7dd0a0f6e 154 if (millis() - lastParameter > 5) {
gianarjuna 1:a2c7dd0a0f6e 155 hitungParameter();
gianarjuna 1:a2c7dd0a0f6e 156 hitungParameterKa();
gianarjuna 1:a2c7dd0a0f6e 157 hitungParameterKi();
gianarjuna 1:a2c7dd0a0f6e 158 lastParameter = millis();
gianarjuna 1:a2c7dd0a0f6e 159 }
gianarjuna 1:a2c7dd0a0f6e 160 if (millis() - lastMotor>4) {
gianarjuna 1:a2c7dd0a0f6e 161 gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 162 lastMotor=millis();
gianarjuna 1:a2c7dd0a0f6e 163 }
gianarjuna 1:a2c7dd0a0f6e 164 }
gianarjuna 1:a2c7dd0a0f6e 165
gianarjuna 1:a2c7dd0a0f6e 166 }
gianarjuna 1:a2c7dd0a0f6e 167
gianarjuna 1:a2c7dd0a0f6e 168 }
gianarjuna 1:a2c7dd0a0f6e 169
gianarjuna 1:a2c7dd0a0f6e 170 void hitungParameter()
gianarjuna 1:a2c7dd0a0f6e 171 {
gianarjuna 1:a2c7dd0a0f6e 172 pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 173 encSteer.reset();
gianarjuna 1:a2c7dd0a0f6e 174 pulseLangkah = (int)encKiri.getPulses();
gianarjuna 1:a2c7dd0a0f6e 175 encKiri.reset();
gianarjuna 1:a2c7dd0a0f6e 176 pulsetemp += pulseLangkah;
gianarjuna 1:a2c7dd0a0f6e 177 if(bouncing == 1) {
gianarjuna 1:a2c7dd0a0f6e 178 pulsetemp =0;
gianarjuna 1:a2c7dd0a0f6e 179 bouncing = 2;//pc.printf("kena");
gianarjuna 1:a2c7dd0a0f6e 180 }
gianarjuna 1:a2c7dd0a0f6e 181 theta += pulse;
gianarjuna 1:a2c7dd0a0f6e 182 }
gianarjuna 1:a2c7dd0a0f6e 183 void initPneu()
gianarjuna 1:a2c7dd0a0f6e 184 {
gianarjuna 1:a2c7dd0a0f6e 185 pneu1[0]=0;
gianarjuna 1:a2c7dd0a0f6e 186 pneu1[1]=0;
gianarjuna 1:a2c7dd0a0f6e 187 pneu2[0]=0;
gianarjuna 1:a2c7dd0a0f6e 188 pneu2[1]=0;
gianarjuna 1:a2c7dd0a0f6e 189 }
gianarjuna 1:a2c7dd0a0f6e 190
gianarjuna 1:a2c7dd0a0f6e 191 void angkatKaki()
gianarjuna 1:a2c7dd0a0f6e 192 {
gianarjuna 1:a2c7dd0a0f6e 193 if(!switchKi && bouncing == 0) {
gianarjuna 1:a2c7dd0a0f6e 194 bouncing = 1;
gianarjuna 1:a2c7dd0a0f6e 195 }
gianarjuna 1:a2c7dd0a0f6e 196 if (pulsetemp > 0 && pulsetemp < 150 && !angkat) {
gianarjuna 1:a2c7dd0a0f6e 197 pneu2[0] = 1;
gianarjuna 1:a2c7dd0a0f6e 198 pneu2[1] = 1;
gianarjuna 1:a2c7dd0a0f6e 199 angkat = 1;
gianarjuna 1:a2c7dd0a0f6e 200 } else if (pulsetemp > 150 && angkat) {
gianarjuna 1:a2c7dd0a0f6e 201 pneu2[0] = 0;
gianarjuna 1:a2c7dd0a0f6e 202 pneu2[1] = 0;
gianarjuna 1:a2c7dd0a0f6e 203 angkat = 0;
gianarjuna 1:a2c7dd0a0f6e 204 bouncing = 0;
gianarjuna 1:a2c7dd0a0f6e 205 }
gianarjuna 1:a2c7dd0a0f6e 206 }
gianarjuna 1:a2c7dd0a0f6e 207
gianarjuna 1:a2c7dd0a0f6e 208 void hitungParameterKa()
gianarjuna 1:a2c7dd0a0f6e 209 {
gianarjuna 1:a2c7dd0a0f6e 210 int pulseKa = (double)encKanan.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 211 encKanan.reset();
gianarjuna 1:a2c7dd0a0f6e 212
gianarjuna 1:a2c7dd0a0f6e 213 thetaKa += pulseKa;
gianarjuna 1:a2c7dd0a0f6e 214 }
gianarjuna 1:a2c7dd0a0f6e 215
gianarjuna 1:a2c7dd0a0f6e 216 void hitungParameterKi()
gianarjuna 1:a2c7dd0a0f6e 217 {
gianarjuna 1:a2c7dd0a0f6e 218 int pulseKi = (double)encKiri.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 219 encKiri.reset();
gianarjuna 1:a2c7dd0a0f6e 220
gianarjuna 1:a2c7dd0a0f6e 221 thetaKi += pulseKi;
gianarjuna 1:a2c7dd0a0f6e 222 }
315_josh 0:7ab5f1f9dcb8 223
gianarjuna 1:a2c7dd0a0f6e 224 void getHeading()
gianarjuna 1:a2c7dd0a0f6e 225 {
gianarjuna 1:a2c7dd0a0f6e 226 heading_temp = (cmps.getAngle()/10 - heading0); //cari perubahan sudut badan
gianarjuna 1:a2c7dd0a0f6e 227 if(heading_temp > 180.0 && heading_temp <= 360.0) //kondisi agar pembacaan 0 - 180 derajat
gianarjuna 1:a2c7dd0a0f6e 228 heading = (heading_temp - 360.0 );
gianarjuna 1:a2c7dd0a0f6e 229 else if(heading_temp < -180.0 && heading_temp >= -360.0)
gianarjuna 1:a2c7dd0a0f6e 230 heading = (heading_temp + 360.0 );
gianarjuna 1:a2c7dd0a0f6e 231 else
gianarjuna 1:a2c7dd0a0f6e 232 heading = heading_temp;
gianarjuna 1:a2c7dd0a0f6e 233 heading = heading*(-1); //output heading
gianarjuna 1:a2c7dd0a0f6e 234 }
gianarjuna 1:a2c7dd0a0f6e 235 void hitungPIDTheta(double theta_sp)
gianarjuna 1:a2c7dd0a0f6e 236 {
gianarjuna 1:a2c7dd0a0f6e 237 if (theta_sp>dThetaMax)
gianarjuna 1:a2c7dd0a0f6e 238 theta_sp=dThetaMax;
gianarjuna 1:a2c7dd0a0f6e 239 theta_error = theta_sp - theta;
gianarjuna 1:a2c7dd0a0f6e 240 if (theta_error > 180) {
gianarjuna 1:a2c7dd0a0f6e 241 theta_error = theta_error - 360;
gianarjuna 1:a2c7dd0a0f6e 242 } else if (theta_error < -179) {
gianarjuna 1:a2c7dd0a0f6e 243 theta_error = theta_error + 360;
gianarjuna 1:a2c7dd0a0f6e 244 }
gianarjuna 1:a2c7dd0a0f6e 245
gianarjuna 1:a2c7dd0a0f6e 246 sum_theta_error += (theta_error);
gianarjuna 1:a2c7dd0a0f6e 247 w =KP * theta_error + KI *sum_theta_error*TS;
gianarjuna 1:a2c7dd0a0f6e 248 speed = w;
gianarjuna 1:a2c7dd0a0f6e 249 theta_error_prev = theta_error;
gianarjuna 1:a2c7dd0a0f6e 250
gianarjuna 1:a2c7dd0a0f6e 251 if(speed >= PWM_MAX) {
gianarjuna 1:a2c7dd0a0f6e 252 pwm = PWM_MAX;
gianarjuna 1:a2c7dd0a0f6e 253 } else if (speed <= PWM_MIN) {
gianarjuna 1:a2c7dd0a0f6e 254 pwm = PWM_MIN;
gianarjuna 1:a2c7dd0a0f6e 255 } else
gianarjuna 1:a2c7dd0a0f6e 256 pwm = speed;
gianarjuna 1:a2c7dd0a0f6e 257 }
gianarjuna 1:a2c7dd0a0f6e 258 void gerakMotor()
gianarjuna 1:a2c7dd0a0f6e 259 {
gianarjuna 1:a2c7dd0a0f6e 260 //if ((theta_error > 1.0 && (fabs(pwm) > 0.15)))// && (state!=0)))
gianarjuna 1:a2c7dd0a0f6e 261 motorSteer.speed(pwm);
gianarjuna 1:a2c7dd0a0f6e 262 // else {
gianarjuna 1:a2c7dd0a0f6e 263 // motorSteer.speed(0.0);
gianarjuna 1:a2c7dd0a0f6e 264 // motorSteer.brake(BRAKE_HIGH);
gianarjuna 1:a2c7dd0a0f6e 265 // }
gianarjuna 1:a2c7dd0a0f6e 266
gianarjuna 1:a2c7dd0a0f6e 267 tambahPWM();
gianarjuna 1:a2c7dd0a0f6e 268 if ( (state>0) && (fabs(speed_maju) > 0.15) && (ohstart)) {
gianarjuna 1:a2c7dd0a0f6e 269 motorKa.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 270 motorKi.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 271 } else {
gianarjuna 1:a2c7dd0a0f6e 272 motorKa.speed(0);
gianarjuna 1:a2c7dd0a0f6e 273 motorKi.speed(0);
gianarjuna 1:a2c7dd0a0f6e 274 motorKa.brake(1);
gianarjuna 1:a2c7dd0a0f6e 275 motorKi.brake(1);
gianarjuna 1:a2c7dd0a0f6e 276 }
gianarjuna 1:a2c7dd0a0f6e 277 }
gianarjuna 1:a2c7dd0a0f6e 278
gianarjuna 1:a2c7dd0a0f6e 279 void tambahPWM()
gianarjuna 1:a2c7dd0a0f6e 280 {
gianarjuna 1:a2c7dd0a0f6e 281 if (millis() - lastPWM>50) {
gianarjuna 1:a2c7dd0a0f6e 282 if (speed_maju>pwm_maju) {
gianarjuna 1:a2c7dd0a0f6e 283 // speed_maju -=0.05;
gianarjuna 1:a2c7dd0a0f6e 284 } else if (speed_maju<pwm_maju) {
gianarjuna 1:a2c7dd0a0f6e 285 speed_maju+=0.05;
315_josh 0:7ab5f1f9dcb8 286 }
315_josh 0:7ab5f1f9dcb8 287 }
gianarjuna 1:a2c7dd0a0f6e 288 lastPWM = millis();
gianarjuna 1:a2c7dd0a0f6e 289 }
gianarjuna 1:a2c7dd0a0f6e 290 void debug() //program utk cek konstanta derajat
gianarjuna 1:a2c7dd0a0f6e 291 {
gianarjuna 1:a2c7dd0a0f6e 292 theta = encSteer.getPulses()*PULSE_TO_DEG;
gianarjuna 1:a2c7dd0a0f6e 293 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 294 // motorKa.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 295 // motorKi.speed(0.8);
gianarjuna 1:a2c7dd0a0f6e 296 // else
gianarjuna 1:a2c7dd0a0f6e 297 // motorKa.speed(0);
gianarjuna 1:a2c7dd0a0f6e 298 // motorKi.speed(0);
gianarjuna 1:a2c7dd0a0f6e 299 // if (temp>20.0){
gianarjuna 1:a2c7dd0a0f6e 300 // belok = 1;
gianarjuna 1:a2c7dd0a0f6e 301 // }
gianarjuna 1:a2c7dd0a0f6e 302 // if (temp<-20.0){
gianarjuna 1:a2c7dd0a0f6e 303 // belok=0;
gianarjuna 1:a2c7dd0a0f6e 304 // }
gianarjuna 1:a2c7dd0a0f6e 305 // if (millis() - lastCompass>3){
gianarjuna 1:a2c7dd0a0f6e 306 // getHeading();
gianarjuna 1:a2c7dd0a0f6e 307 // lastCompass=millis();
gianarjuna 1:a2c7dd0a0f6e 308 // }
gianarjuna 1:a2c7dd0a0f6e 309 //// if (!belok)
gianarjuna 1:a2c7dd0a0f6e 310 //// motorSteer.speed(speed_maju);
gianarjuna 1:a2c7dd0a0f6e 311 //// else
gianarjuna 1:a2c7dd0a0f6e 312 //// motorSteer.speed(-1*speed_maju);
gianarjuna 1:a2c7dd0a0f6e 313 // if (!Ubutton)
gianarjuna 1:a2c7dd0a0f6e 314 // theta_sp=20.0;
gianarjuna 1:a2c7dd0a0f6e 315 // else
gianarjuna 1:a2c7dd0a0f6e 316 // theta_sp=-20.0;
gianarjuna 1:a2c7dd0a0f6e 317 //
gianarjuna 1:a2c7dd0a0f6e 318 // if (millis() - last_mt_printPID > 7){
gianarjuna 1:a2c7dd0a0f6e 319 // if (!(fabs(theta_sp- theta) < TOLERANCET)){
gianarjuna 1:a2c7dd0a0f6e 320 // hitungPIDTheta(theta_sp);
gianarjuna 1:a2c7dd0a0f6e 321 // }
gianarjuna 1:a2c7dd0a0f6e 322 // last_mt_printPID = millis();
gianarjuna 1:a2c7dd0a0f6e 323 // }
gianarjuna 1:a2c7dd0a0f6e 324 // gerakMotor();
gianarjuna 1:a2c7dd0a0f6e 325 // //motorSteer.speed(pwm);
gianarjuna 1:a2c7dd0a0f6e 326 // printf("pulse = %.2f\t theta = %.2f theta = %.2f pidT=%.2f pwm=%.2f\n",
gianarjuna 1:a2c7dd0a0f6e 327 // theta, heading, theta_error, w,pwm);
gianarjuna 1:a2c7dd0a0f6e 328 }
gianarjuna 1:a2c7dd0a0f6e 329
gianarjuna 1:a2c7dd0a0f6e 330
gianarjuna 1:a2c7dd0a0f6e 331 void hitungOdometry()
gianarjuna 1:a2c7dd0a0f6e 332 {
gianarjuna 1:a2c7dd0a0f6e 333 //Tolong isi program odometry utk pisisi robot terhadap lapangan
gianarjuna 1:a2c7dd0a0f6e 334 //berdasarkan geometry robot dan posisi ping (asumsi ping diletakan di tengah kaki bagian luar
gianarjuna 1:a2c7dd0a0f6e 335 //ukuran robot minta ama mekanik
gianarjuna 1:a2c7dd0a0f6e 336 /* POSISI PING utk di kaki depan
gianarjuna 1:a2c7dd0a0f6e 337 PING PING
gianarjuna 1:a2c7dd0a0f6e 338 #### ####
gianarjuna 1:a2c7dd0a0f6e 339 # # # #
gianarjuna 1:a2c7dd0a0f6e 340 # # # #
gianarjuna 1:a2c7dd0a0f6e 341 PING# # # # PING
gianarjuna 1:a2c7dd0a0f6e 342 # # # #
gianarjuna 1:a2c7dd0a0f6e 343 # # # #
gianarjuna 1:a2c7dd0a0f6e 344 #### ####
gianarjuna 1:a2c7dd0a0f6e 345 */
gianarjuna 1:a2c7dd0a0f6e 346 }