Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

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?

UserRevisionLine numberNew 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 */