Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@5:dee879b9ae82, 2019-05-14 (annotated)

Committer:
dwianiyulia
Date:
Tue May 14 09:16:38 2019 +0000
Revision:
5:dee879b9ae82
Parent:
4:8a50d972066d
Child:
6:e9ef196da46a
rev;

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();
dwianiyulia 5:dee879b9ae82 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 4:8a50d972066d 34 pwm = (0.06*galat) + (3*derivative);
garin 1:7533e025de71 35
garin 1:7533e025de71 36 //limit the power of motor, the max power can be changed
dwianiyulia 5:dee879b9ae82 37 if (pwm > 0.9) pwm = 0.9;
dwianiyulia 5:dee879b9ae82 38 else if (pwm < -0.9) pwm = -0.9;
dwianiyulia 5:dee879b9ae82 39 }
garin 1:7533e025de71 40
dwianiyulia 2:710b092cfcc0 41 void MoveMotor(double theta){
garin 1:7533e025de71 42 not_stop = 1;
dwianiyulia 4:8a50d972066d 43 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 44 total_pulse =0;
oktavianusirvan 0:ac8956bbab28 45
oktavianusirvan 0:ac8956bbab28 46 // move motor
dwianiyulia 2:710b092cfcc0 47 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 48 while ((not_stop) && theta != 0){
oktavianusirvan 0:ac8956bbab28 49 BacaEncoder();
garin 1:7533e025de71 50 PID(theta);
oktavianusirvan 0:ac8956bbab28 51 // set speed motor
dwianiyulia 5:dee879b9ae82 52 if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm);
oktavianusirvan 0:ac8956bbab28 53 else {
oktavianusirvan 0:ac8956bbab28 54 main_motor.speed(0);
oktavianusirvan 0:ac8956bbab28 55 main_motor.brake(1);
dwianiyulia 5:dee879b9ae82 56 if (fabs(galat)<0.001) not_stop = 1;
oktavianusirvan 0:ac8956bbab28 57 }
dwianiyulia 2:710b092cfcc0 58 last_galat = galat;
oktavianusirvan 0:ac8956bbab28 59 }
oktavianusirvan 0:ac8956bbab28 60 }
dwianiyulia 2:710b092cfcc0 61 int t1;
oktavianusirvan 0:ac8956bbab28 62
oktavianusirvan 0:ac8956bbab28 63 int main(){
garin 1:7533e025de71 64 count_ball = 0;
garin 1:7533e025de71 65 int state = 1; // posisi awal
dwianiyulia 5:dee879b9ae82 66 while (1){
dwianiyulia 5:dee879b9ae82 67 main_motor.speed(0.5);
dwianiyulia 4:8a50d972066d 68 }
dwianiyulia 5:dee879b9ae82 69
dwianiyulia 5:dee879b9ae82 70
dwianiyulia 4:8a50d972066d 71 /*while (count_ball<6){
garin 1:7533e025de71 72 if (state && !infrared) {
garin 1:7533e025de71 73 state = 0 ;
garin 1:7533e025de71 74 pneu = 1; // menembakkan bola
garin 1:7533e025de71 75 count_ball++;
dwianiyulia 3:3ee76478b110 76 t1 = us_ticker_read();
garin 1:7533e025de71 77
garin 1:7533e025de71 78 // menggerakkan motor
dwianiyulia 3:3ee76478b110 79 if (count_ball == 2) MoveMotor(60);
dwianiyulia 3:3ee76478b110 80 else if (count_ball == 4) MoveMotor(90);
garin 1:7533e025de71 81
garin 1:7533e025de71 82 } else {
oktavianusirvan 0:ac8956bbab28 83 pneu = 0;
dwianiyulia 3:3ee76478b110 84 if (us_ticker_read()-t1 >1000 ) state = 1; // tunggu pneumatik kembali
oktavianusirvan 0:ac8956bbab28 85 }
dwianiyulia 4:8a50d972066d 86 }*/
garin 1:7533e025de71 87 }