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
- Committer:
- gianarjuna
- Date:
- 2019-03-29
- Revision:
- 1:a2c7dd0a0f6e
- Parent:
- 0:7ab5f1f9dcb8
- Child:
- 2:0351727f6354
- Child:
- 3:8f57f69f6e64
File content as of revision 1:a2c7dd0a0f6e:
//DEKLARASI LIBRARY #include "mbed.h" #include "millis.h" #include "Motor.h" #include "encoderKRAI.h" #include "CMPS12_KRAI.h" #define PULSE_TO_DEG 0.135 #define PWM_MAX 0.85 #define PWM_MIN -0.85 #define TOLERANCET 1.0 #define TS 7.0 CMPS12_KRAI cmps(PB_3, PB_10, 0xc0); encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING); //encoder motor steering encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING); encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING); Motor motorSteer(PB_14, PC_4, PB_13); //motor arah putaran CCW ++ Motor motorKa(PA_11, PA_7,PB_12); //motor kanan Motor motorKi(PB_15, PB_1, PB_2); //motor kiri DigitalIn Ubutton(USER_BUTTON); Serial pc(USBTX, USBRX, 115200); DigitalIn switchKi(PA_9,PullUp); DigitalIn switchKa(PD_2,PullUp); DigitalOut pneu1[2] = {(PC_6),(PC_8)}; //kanan belakang , kiri depan DigitalOut pneu2[2] = {(PC_5),(PC_9)}; //kiri belakang, kanan depan int pulsetemp; float theta; float target; int cp = 0; int brake_state = 0; float sum_theta_error; float theta_error_prev; float theta_target; float speed; float w; float theta_sp; float theta_error; float heading_temp, heading0, heading; double pulse = 0; double KI; int angkat; int bouncing = 0; const double KP = 0.05, KI1 = 0.00000, KI2 = 0.00000; unsigned long lastParameter, //millis utk perhintungan parameter last_mt_printPID, last_mt_print, //millis utk print last_mt_button, //millis utk user button last_start, //millis start uji coba kuda lastFSM, //finite state machine millis lastPWM, lastMotor, lastCompass, lastThetaTarget; int start; int state=0; float pwm; float pwm_maju=0.0; float temp = 0; float dThetaMax = 35.0; float speed_maju=0; int ohstart = 0; int belok =0; int thetaKa; int thetaKi; int pulseLangkah; void hitungParameter(); void getHeading(); void hitungPIDTheta(double theta_sp); void gerakMotor(); void tambahPWM(); void debug(); void gerakDepan(int end); void hitungOdometry(); void hitungParameterKa(); void hitungParameterKi(); void initPneu(); void angkatKaki(); //int main(){ // startMillis(); // speed_maju=0.3; // heading0=(float)cmps.getAngle()/10; // while(1){ // debug(); // //pc.printf("TEST\n"); // } //} int main() { encSteer.reset(); startMillis(); theta = 0; cp = 1; state=0; theta_sp =0; ohstart=0; KI = KI1; angkat = 0; int kanan; int kiri; initPneu(); while(1) { if (millis()- last_mt_print > 15) { //pc.printf("%.2f\n",speed_maju); if(!switchKa) { kanan = 1; pc.printf("KENA "); } else { kanan = 0; } if(!switchKi) { kiri = 1; } else { kiri = 0; } last_mt_print = millis(); pc.printf("%d %d %.2f %d \n",kiri,kanan,speed_maju, pulsetemp); } if(!Ubutton) { //mulai gerak maju wait_ms(2000); //diam 2 detik sebelum mulai ohstart = 1; state=1; pwm_maju = 0.8; speed_maju=0.7; last_start=millis(); } if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik ohstart=0; } if (ohstart) { angkatKaki(); if (millis() - lastFSM > 9) { if (state==1) { //state maju lurus theta_sp=0; belok=0; pwm_maju=0.80; } lastFSM=millis(); } //urutan prioritas if (millis() - last_mt_printPID > 7) { if (!(fabs(theta_sp- theta) < TOLERANCET)) { hitungPIDTheta(theta_sp); } last_mt_printPID = millis(); } if (millis() - lastParameter > 5) { hitungParameter(); hitungParameterKa(); hitungParameterKi(); lastParameter = millis(); } if (millis() - lastMotor>4) { gerakMotor(); lastMotor=millis(); } } } } void hitungParameter() { pulse = (double)encSteer.getPulses()*PULSE_TO_DEG; encSteer.reset(); pulseLangkah = (int)encKiri.getPulses(); encKiri.reset(); pulsetemp += pulseLangkah; if(bouncing == 1) { pulsetemp =0; bouncing = 2;//pc.printf("kena"); } theta += pulse; } void initPneu() { pneu1[0]=0; pneu1[1]=0; pneu2[0]=0; pneu2[1]=0; } void angkatKaki() { if(!switchKi && bouncing == 0) { bouncing = 1; } if (pulsetemp > 0 && pulsetemp < 150 && !angkat) { pneu2[0] = 1; pneu2[1] = 1; angkat = 1; } else if (pulsetemp > 150 && angkat) { pneu2[0] = 0; pneu2[1] = 0; angkat = 0; bouncing = 0; } } void hitungParameterKa() { int pulseKa = (double)encKanan.getPulses()*PULSE_TO_DEG; encKanan.reset(); thetaKa += pulseKa; } void hitungParameterKi() { int pulseKi = (double)encKiri.getPulses()*PULSE_TO_DEG; encKiri.reset(); thetaKi += pulseKi; } void getHeading() { heading_temp = (cmps.getAngle()/10 - heading0); //cari perubahan sudut badan if(heading_temp > 180.0 && heading_temp <= 360.0) //kondisi agar pembacaan 0 - 180 derajat heading = (heading_temp - 360.0 ); else if(heading_temp < -180.0 && heading_temp >= -360.0) heading = (heading_temp + 360.0 ); else heading = heading_temp; heading = heading*(-1); //output heading } void hitungPIDTheta(double theta_sp) { if (theta_sp>dThetaMax) theta_sp=dThetaMax; theta_error = theta_sp - theta; if (theta_error > 180) { theta_error = theta_error - 360; } else if (theta_error < -179) { theta_error = theta_error + 360; } sum_theta_error += (theta_error); w =KP * theta_error + KI *sum_theta_error*TS; speed = w; theta_error_prev = theta_error; if(speed >= PWM_MAX) { pwm = PWM_MAX; } else if (speed <= PWM_MIN) { pwm = PWM_MIN; } else pwm = speed; } void gerakMotor() { //if ((theta_error > 1.0 && (fabs(pwm) > 0.15)))// && (state!=0))) motorSteer.speed(pwm); // else { // motorSteer.speed(0.0); // motorSteer.brake(BRAKE_HIGH); // } tambahPWM(); if ( (state>0) && (fabs(speed_maju) > 0.15) && (ohstart)) { motorKa.speed(speed_maju); motorKi.speed(speed_maju); } else { motorKa.speed(0); motorKi.speed(0); motorKa.brake(1); motorKi.brake(1); } } void tambahPWM() { if (millis() - lastPWM>50) { if (speed_maju>pwm_maju) { // speed_maju -=0.05; } else if (speed_maju<pwm_maju) { speed_maju+=0.05; } } lastPWM = millis(); } void debug() //program utk cek konstanta derajat { theta = encSteer.getPulses()*PULSE_TO_DEG; // if (!Ubutton) // motorKa.speed(0.8); // motorKi.speed(0.8); // else // motorKa.speed(0); // motorKi.speed(0); // if (temp>20.0){ // belok = 1; // } // if (temp<-20.0){ // belok=0; // } // if (millis() - lastCompass>3){ // getHeading(); // lastCompass=millis(); // } //// if (!belok) //// motorSteer.speed(speed_maju); //// else //// motorSteer.speed(-1*speed_maju); // if (!Ubutton) // theta_sp=20.0; // else // theta_sp=-20.0; // // if (millis() - last_mt_printPID > 7){ // if (!(fabs(theta_sp- theta) < TOLERANCET)){ // hitungPIDTheta(theta_sp); // } // last_mt_printPID = millis(); // } // gerakMotor(); // //motorSteer.speed(pwm); // printf("pulse = %.2f\t theta = %.2f theta = %.2f pidT=%.2f pwm=%.2f\n", // theta, heading, theta_error, w,pwm); } void hitungOdometry() { //Tolong isi program odometry utk pisisi robot terhadap lapangan //berdasarkan geometry robot dan posisi ping (asumsi ping diletakan di tengah kaki bagian luar //ukuran robot minta ama mekanik /* POSISI PING utk di kaki depan PING PING #### #### # # # # # # # # PING# # # # PING # # # # # # # # #### #### */ }