![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
main.cpp@0:a6918b57d3fa, 2017-10-17 (annotated)
- Committer:
- calmantara186
- Date:
- Tue Oct 17 15:43:07 2017 +0000
- Revision:
- 0:a6918b57d3fa
- Child:
- 1:5e0cb74f950e
Selasa, 17 Oktober 2017
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
calmantara186 | 0:a6918b57d3fa | 1 | /** |
calmantara186 | 0:a6918b57d3fa | 2 | * THROWER ROBOT |
calmantara186 | 0:a6918b57d3fa | 3 | * Bismillahirahmanirrahim |
calmantara186 | 0:a6918b57d3fa | 4 | * |
calmantara186 | 0:a6918b57d3fa | 5 | * Updated : |
calmantara186 | 0:a6918b57d3fa | 6 | * -Perhitungan PID masih error |
calmantara186 | 0:a6918b57d3fa | 7 | * -encoder kanan sama encoder kiri belum kebaca. ntah kenapa |
calmantara186 | 0:a6918b57d3fa | 8 | * |
calmantara186 | 0:a6918b57d3fa | 9 | * To do List : |
calmantara186 | 0:a6918b57d3fa | 10 | * -Coba ODOMETRY !! |
calmantara186 | 0:a6918b57d3fa | 11 | * |
calmantara186 | 0:a6918b57d3fa | 12 | * Last Edited by : Calmantara |
calmantara186 | 0:a6918b57d3fa | 13 | **/ |
calmantara186 | 0:a6918b57d3fa | 14 | |
calmantara186 | 0:a6918b57d3fa | 15 | //LIBRARY |
calmantara186 | 0:a6918b57d3fa | 16 | #include "mbed.h" |
calmantara186 | 0:a6918b57d3fa | 17 | #include "Motor.h" |
calmantara186 | 0:a6918b57d3fa | 18 | #include "encoderKRAI.h" |
calmantara186 | 0:a6918b57d3fa | 19 | #include "millis.h" |
calmantara186 | 0:a6918b57d3fa | 20 | |
calmantara186 | 0:a6918b57d3fa | 21 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 22 | * Konstanta dan Variabel * |
calmantara186 | 0:a6918b57d3fa | 23 | ***************************/ |
calmantara186 | 0:a6918b57d3fa | 24 | |
calmantara186 | 0:a6918b57d3fa | 25 | //Deklarasi Motor |
calmantara186 | 0:a6918b57d3fa | 26 | Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 27 | Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 28 | Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 29 | |
calmantara186 | 0:a6918b57d3fa | 30 | //Variabel Motor |
calmantara186 | 0:a6918b57d3fa | 31 | float pwm_kanan = 0.3; |
calmantara186 | 0:a6918b57d3fa | 32 | float pwm_kiri = -0.3; |
calmantara186 | 0:a6918b57d3fa | 33 | float limit_speed = 0.7; |
calmantara186 | 0:a6918b57d3fa | 34 | |
calmantara186 | 0:a6918b57d3fa | 35 | //Variabel Serial |
calmantara186 | 0:a6918b57d3fa | 36 | Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX |
calmantara186 | 0:a6918b57d3fa | 37 | |
calmantara186 | 0:a6918b57d3fa | 38 | //Deklarasi Rotary Encoder |
calmantara186 | 0:a6918b57d3fa | 39 | encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 0:a6918b57d3fa | 40 | encoderKRAI encMotor_kanan(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 0:a6918b57d3fa | 41 | encoderKRAI encMotor_kiri(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 0:a6918b57d3fa | 42 | |
calmantara186 | 0:a6918b57d3fa | 43 | //Variabel Encoder |
calmantara186 | 0:a6918b57d3fa | 44 | float pulses_depan; //variabel untuk membaca nilai encoder motor depan |
calmantara186 | 0:a6918b57d3fa | 45 | float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan |
calmantara186 | 0:a6918b57d3fa | 46 | float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri |
calmantara186 | 0:a6918b57d3fa | 47 | |
calmantara186 | 0:a6918b57d3fa | 48 | //Target nilai encoder |
calmantara186 | 0:a6918b57d3fa | 49 | float target_depan = 0; |
calmantara186 | 0:a6918b57d3fa | 50 | |
calmantara186 | 0:a6918b57d3fa | 51 | //Variabel PID_1 |
calmantara186 | 0:a6918b57d3fa | 52 | unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan |
calmantara186 | 0:a6918b57d3fa | 53 | unsigned long int prev_millis1; //variabel untuk mendapatkan millis sebelumnya |
calmantara186 | 0:a6918b57d3fa | 54 | unsigned short time_sampling = 12; //variabel untuk time sampling |
calmantara186 | 0:a6918b57d3fa | 55 | float max_speed = 0.4; |
calmantara186 | 0:a6918b57d3fa | 56 | float min_speed = -0.4; |
calmantara186 | 0:a6918b57d3fa | 57 | float kp_1 = 0.5; //variabel konstanta PID |
calmantara186 | 0:a6918b57d3fa | 58 | float current_error1, prev_error1; |
calmantara186 | 0:a6918b57d3fa | 59 | float speed, prev_speed; |
calmantara186 | 0:a6918b57d3fa | 60 | |
calmantara186 | 0:a6918b57d3fa | 61 | //Variabel Motor Berhenti |
calmantara186 | 0:a6918b57d3fa | 62 | unsigned long int prev_millis2; //variabel untuk mendapatkan millis sebelumnya |
calmantara186 | 0:a6918b57d3fa | 63 | |
calmantara186 | 0:a6918b57d3fa | 64 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 65 | * Main Function * |
calmantara186 | 0:a6918b57d3fa | 66 | ***************************/ |
calmantara186 | 0:a6918b57d3fa | 67 | |
calmantara186 | 0:a6918b57d3fa | 68 | int main(){ |
calmantara186 | 0:a6918b57d3fa | 69 | |
calmantara186 | 0:a6918b57d3fa | 70 | //Inisiasi Serial |
calmantara186 | 0:a6918b57d3fa | 71 | pc.baud(115200); |
calmantara186 | 0:a6918b57d3fa | 72 | |
calmantara186 | 0:a6918b57d3fa | 73 | //Start Millis |
calmantara186 | 0:a6918b57d3fa | 74 | //wait_ms(5000); |
calmantara186 | 0:a6918b57d3fa | 75 | startMillis(); |
calmantara186 | 0:a6918b57d3fa | 76 | |
calmantara186 | 0:a6918b57d3fa | 77 | //Looping Program |
calmantara186 | 0:a6918b57d3fa | 78 | while(1){ |
calmantara186 | 0:a6918b57d3fa | 79 | current_millis = millis(); |
calmantara186 | 0:a6918b57d3fa | 80 | /* Speed Motor */ |
calmantara186 | 0:a6918b57d3fa | 81 | if(pwm_kanan > limit_speed){ |
calmantara186 | 0:a6918b57d3fa | 82 | pwm_kanan = limit_speed; |
calmantara186 | 0:a6918b57d3fa | 83 | } |
calmantara186 | 0:a6918b57d3fa | 84 | else if(pwm_kanan < -limit_speed){ |
calmantara186 | 0:a6918b57d3fa | 85 | pwm_kanan = -limit_speed; |
calmantara186 | 0:a6918b57d3fa | 86 | } |
calmantara186 | 0:a6918b57d3fa | 87 | if(pwm_kiri > limit_speed){ |
calmantara186 | 0:a6918b57d3fa | 88 | pwm_kiri = limit_speed; |
calmantara186 | 0:a6918b57d3fa | 89 | } |
calmantara186 | 0:a6918b57d3fa | 90 | else if(pwm_kiri < -limit_speed){ |
calmantara186 | 0:a6918b57d3fa | 91 | pwm_kiri = -limit_speed; |
calmantara186 | 0:a6918b57d3fa | 92 | } |
calmantara186 | 0:a6918b57d3fa | 93 | |
calmantara186 | 0:a6918b57d3fa | 94 | motor_kanan.speed(pwm_kanan); |
calmantara186 | 0:a6918b57d3fa | 95 | motor_kiri.speed(pwm_kiri); |
calmantara186 | 0:a6918b57d3fa | 96 | |
calmantara186 | 0:a6918b57d3fa | 97 | //pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_kiri); |
calmantara186 | 0:a6918b57d3fa | 98 | |
calmantara186 | 0:a6918b57d3fa | 99 | if(current_millis - prev_millis1 >= time_sampling){ |
calmantara186 | 0:a6918b57d3fa | 100 | /* masuk ke perhitungan ketika sudah pada time sampling */ |
calmantara186 | 0:a6918b57d3fa | 101 | pulses_depan = (float) encMotor_depan.getRevolutions(); //Mengambil pulses encoder |
calmantara186 | 0:a6918b57d3fa | 102 | current_error1 = target_depan - pulses_depan; |
calmantara186 | 0:a6918b57d3fa | 103 | speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1; |
calmantara186 | 0:a6918b57d3fa | 104 | /* Kondisi untuk menambahkan speed */ |
calmantara186 | 0:a6918b57d3fa | 105 | if(speed > max_speed){ |
calmantara186 | 0:a6918b57d3fa | 106 | pwm_kiri = pwm_kiri+max_speed; |
calmantara186 | 0:a6918b57d3fa | 107 | pwm_kanan = pwm_kanan - max_speed; |
calmantara186 | 0:a6918b57d3fa | 108 | } |
calmantara186 | 0:a6918b57d3fa | 109 | else if ( speed < min_speed){ |
calmantara186 | 0:a6918b57d3fa | 110 | pwm_kiri = pwm_kiri-min_speed; |
calmantara186 | 0:a6918b57d3fa | 111 | pwm_kanan = pwm_kanan + min_speed; |
calmantara186 | 0:a6918b57d3fa | 112 | } |
calmantara186 | 0:a6918b57d3fa | 113 | else { |
calmantara186 | 0:a6918b57d3fa | 114 | pwm_kiri = pwm_kiri + speed; |
calmantara186 | 0:a6918b57d3fa | 115 | pwm_kanan = pwm_kanan - speed; |
calmantara186 | 0:a6918b57d3fa | 116 | } |
calmantara186 | 0:a6918b57d3fa | 117 | prev_speed = speed; |
calmantara186 | 0:a6918b57d3fa | 118 | prev_error1 = current_error1; |
calmantara186 | 0:a6918b57d3fa | 119 | prev_millis1 = current_millis; |
calmantara186 | 0:a6918b57d3fa | 120 | } |
calmantara186 | 0:a6918b57d3fa | 121 | } |
calmantara186 | 0:a6918b57d3fa | 122 | } /* end of main fuction */ |