Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

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?

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