Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@3:3ee76478b110, 2019-05-14 (annotated)

Committer:
dwianiyulia
Date:
Tue May 14 06:37:33 2019 +0000
Revision:
3:3ee76478b110
Parent:
2:710b092cfcc0
Child:
4:8a50d972066d
udah bisa di compile;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oktavianusirvan 0:ac8956bbab28 1 //
oktavianusirvan 0:ac8956bbab28 2
oktavianusirvan 0:ac8956bbab28 3 //
oktavianusirvan 0:ac8956bbab28 4 #include <stdio.h>
oktavianusirvan 0:ac8956bbab28 5 #include <Motor.h>
oktavianusirvan 0:ac8956bbab28 6 #include <encoderKRAI.h>
oktavianusirvan 0:ac8956bbab28 7 #include <mbed.h>
dwianiyulia 2:710b092cfcc0 8 #include <stdint.h>
oktavianusirvan 0:ac8956bbab28 9 // declare
oktavianusirvan 0:ac8956bbab28 10 Motor main_motor(PB_13, PB_14, PB_15 ); // input pin
oktavianusirvan 0:ac8956bbab28 11 DigitalOut pneu(PC_5);// input pin
oktavianusirvan 0:ac8956bbab28 12 DigitalIn infrared(PC_5); // input pin
oktavianusirvan 0:ac8956bbab28 13 encoderKRAI encoder(PB_1,PB_2,538,encoderKRAI::X4_ENCODING);// input pin
oktavianusirvan 0:ac8956bbab28 14
oktavianusirvan 0:ac8956bbab28 15 int count, count_ball ;
oktavianusirvan 0:ac8956bbab28 16 double pulse, total_pulse,total_pulse_in_degree,theta,pwm;
oktavianusirvan 0:ac8956bbab28 17 double kp,ki,kd;
garin 1:7533e025de71 18 bool not_stop ;
dwianiyulia 2:710b092cfcc0 19 double galat, integral, derivative;
dwianiyulia 2:710b092cfcc0 20 double last_galat ;
oktavianusirvan 0:ac8956bbab28 21
oktavianusirvan 0:ac8956bbab28 22 void BacaEncoder(){// read encoder
oktavianusirvan 0:ac8956bbab28 23 pulse = (double)encoder.getPulses();
oktavianusirvan 0:ac8956bbab28 24 encoder.reset();
oktavianusirvan 0:ac8956bbab28 25 total_pulse +=pulse;
oktavianusirvan 0:ac8956bbab28 26 total_pulse_in_degree = total_pulse*360/538;
oktavianusirvan 0:ac8956bbab28 27 }
oktavianusirvan 0:ac8956bbab28 28
dwianiyulia 2:710b092cfcc0 29 void PID(double theta){
garin 1:7533e025de71 30 //pid
dwianiyulia 2:710b092cfcc0 31 galat = theta - total_pulse_in_degree;
dwianiyulia 2:710b092cfcc0 32 derivative = galat - last_galat;
garin 1:7533e025de71 33
dwianiyulia 2:710b092cfcc0 34 pwm = (kp*galat) + (kd*derivative);
garin 1:7533e025de71 35
garin 1:7533e025de71 36 //limit the power of motor, the max power can be changed
garin 1:7533e025de71 37 if (pwm > 1) pwm = 1;
garin 1:7533e025de71 38 else if (pwm < -1) pwm = -1;
garin 1:7533e025de71 39
garin 1:7533e025de71 40
garin 1:7533e025de71 41 }
dwianiyulia 2:710b092cfcc0 42 void MoveMotor(double theta){
garin 1:7533e025de71 43 not_stop = 1;
dwianiyulia 2:710b092cfcc0 44 last_galat = 0;
oktavianusirvan 0:ac8956bbab28 45 total_pulse =0;
oktavianusirvan 0:ac8956bbab28 46
oktavianusirvan 0:ac8956bbab28 47 // move motor
dwianiyulia 2:710b092cfcc0 48 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 49 while ((not_stop) && theta != 0){
oktavianusirvan 0:ac8956bbab28 50 BacaEncoder();
garin 1:7533e025de71 51 PID(theta);
oktavianusirvan 0:ac8956bbab28 52 // set speed motor
garin 1:7533e025de71 53 if (pwm != 0) main_motor.speed(pwm);
oktavianusirvan 0:ac8956bbab28 54 else {
oktavianusirvan 0:ac8956bbab28 55 main_motor.speed(0);
oktavianusirvan 0:ac8956bbab28 56 main_motor.brake(1);
oktavianusirvan 0:ac8956bbab28 57 not_stop = 0;
oktavianusirvan 0:ac8956bbab28 58 }
dwianiyulia 2:710b092cfcc0 59 last_galat = galat;
oktavianusirvan 0:ac8956bbab28 60 }
oktavianusirvan 0:ac8956bbab28 61 }
dwianiyulia 2:710b092cfcc0 62 int t1;
oktavianusirvan 0:ac8956bbab28 63
oktavianusirvan 0:ac8956bbab28 64 int main(){
garin 1:7533e025de71 65 count_ball = 0;
garin 1:7533e025de71 66 int state = 1; // posisi awal
oktavianusirvan 0:ac8956bbab28 67
garin 1:7533e025de71 68 while (count_ball<6){
garin 1:7533e025de71 69 if (state && !infrared) {
garin 1:7533e025de71 70 state = 0 ;
garin 1:7533e025de71 71 pneu = 1; // menembakkan bola
garin 1:7533e025de71 72 count_ball++;
dwianiyulia 3:3ee76478b110 73 t1 = us_ticker_read();
garin 1:7533e025de71 74
garin 1:7533e025de71 75 // menggerakkan motor
dwianiyulia 3:3ee76478b110 76 if (count_ball == 2) MoveMotor(60);
dwianiyulia 3:3ee76478b110 77 else if (count_ball == 4) MoveMotor(90);
garin 1:7533e025de71 78
garin 1:7533e025de71 79 } else {
oktavianusirvan 0:ac8956bbab28 80 pneu = 0;
dwianiyulia 3:3ee76478b110 81 if (us_ticker_read()-t1 >1000 ) state = 1; // tunggu pneumatik kembali
oktavianusirvan 0:ac8956bbab28 82 }
oktavianusirvan 0:ac8956bbab28 83 }
garin 1:7533e025de71 84 }