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