Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@7:68af06a391f0, 2019-05-17 (annotated)

Committer:
garin
Date:
Fri May 17 10:03:49 2019 +0000
Revision:
7:68af06a391f0
Parent:
6:e9ef196da46a
Child:
8:069a607ef761
vdda

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 <Motor.h>
oktavianusirvan 0:ac8956bbab28 5 #include <encoderKRAI.h>
oktavianusirvan 0:ac8956bbab28 6 #include <mbed.h>
oktavianusirvan 0:ac8956bbab28 7 // declare
garin 7:68af06a391f0 8 Motor main_motor(PB_15, PB_14, PB_13 ); // input pin
rahachu 6:e9ef196da46a 9 DigitalOut pneu(PC_6);// input pin
garin 7:68af06a391f0 10 DigitalOut pneu1(PC_5);// input pin
garin 7:68af06a391f0 11 DigitalOut pneu2(PC_8);// input pin
garin 7:68af06a391f0 12 DigitalIn infrared(PB_12, PullUp); // input pin
garin 7:68af06a391f0 13 encoderKRAI encoder(PB_2,PB_1,538,encoderKRAI::X4_ENCODING);// input pin
rahachu 6:e9ef196da46a 14 Serial pc(USBTX, USBRX,115200);
oktavianusirvan 0:ac8956bbab28 15
oktavianusirvan 0:ac8956bbab28 16 int count, count_ball ;
oktavianusirvan 0:ac8956bbab28 17 double pulse, total_pulse,total_pulse_in_degree,theta,pwm;
oktavianusirvan 0:ac8956bbab28 18 double kp,ki,kd;
garin 1:7533e025de71 19 bool not_stop ;
dwianiyulia 2:710b092cfcc0 20 double galat, integral, derivative;
dwianiyulia 2:710b092cfcc0 21 double last_galat ;
garin 7:68af06a391f0 22
oktavianusirvan 0:ac8956bbab28 23
oktavianusirvan 0:ac8956bbab28 24 void BacaEncoder(){// read encoder
oktavianusirvan 0:ac8956bbab28 25 pulse = (double)encoder.getPulses();
oktavianusirvan 0:ac8956bbab28 26 encoder.reset();
dwianiyulia 5:dee879b9ae82 27 total_pulse += pulse;
oktavianusirvan 0:ac8956bbab28 28 total_pulse_in_degree = total_pulse*360/538;
oktavianusirvan 0:ac8956bbab28 29 }
oktavianusirvan 0:ac8956bbab28 30
dwianiyulia 2:710b092cfcc0 31 void PID(double theta){
garin 1:7533e025de71 32 //pid
dwianiyulia 2:710b092cfcc0 33 galat = theta - total_pulse_in_degree;
dwianiyulia 2:710b092cfcc0 34 derivative = galat - last_galat;
garin 1:7533e025de71 35
garin 7:68af06a391f0 36 pwm = (0.006*galat);
garin 1:7533e025de71 37
garin 1:7533e025de71 38 //limit the power of motor, the max power can be changed
garin 7:68af06a391f0 39 if (pwm > 0.6) pwm = 0.6;
garin 7:68af06a391f0 40 else if (pwm < -0.6) pwm = -0.6;
dwianiyulia 5:dee879b9ae82 41 }
garin 1:7533e025de71 42
dwianiyulia 2:710b092cfcc0 43 void MoveMotor(double theta){
garin 1:7533e025de71 44 not_stop = 1;
dwianiyulia 4:8a50d972066d 45 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 46 total_pulse =0;
rahachu 6:e9ef196da46a 47 integral = 0;
oktavianusirvan 0:ac8956bbab28 48
oktavianusirvan 0:ac8956bbab28 49 // move motor
dwianiyulia 2:710b092cfcc0 50 last_galat = theta;
garin 7:68af06a391f0 51 while ((not_stop) && theta!=0){
oktavianusirvan 0:ac8956bbab28 52 BacaEncoder();
rahachu 6:e9ef196da46a 53 //PID(theta);
rahachu 6:e9ef196da46a 54 galat = theta - total_pulse_in_degree;
rahachu 6:e9ef196da46a 55 derivative = galat - last_galat;
rahachu 6:e9ef196da46a 56 integral += galat;
garin 7:68af06a391f0 57 pwm = (0.007*galat)+(derivative*3) ;
rahachu 6:e9ef196da46a 58
rahachu 6:e9ef196da46a 59 //limit the power of motor, the max power can be changed
garin 7:68af06a391f0 60 if (pwm > 0.6) pwm = 0.6;
garin 7:68af06a391f0 61 else if (pwm < -0.6) pwm = -0.6;
oktavianusirvan 0:ac8956bbab28 62 // set speed motor
garin 7:68af06a391f0 63 if ((pwm > 0.1) || (pwm < -0.1)) main_motor.speed(pwm);
oktavianusirvan 0:ac8956bbab28 64 else {
oktavianusirvan 0:ac8956bbab28 65 main_motor.speed(0);
oktavianusirvan 0:ac8956bbab28 66 main_motor.brake(1);
garin 7:68af06a391f0 67 not_stop = 0;
garin 7:68af06a391f0 68 break;
oktavianusirvan 0:ac8956bbab28 69 }
garin 7:68af06a391f0 70
dwianiyulia 2:710b092cfcc0 71 last_galat = galat;
garin 7:68af06a391f0 72 pc.printf("%f.2 \n",pwm);
garin 7:68af06a391f0 73 if (fabs(galat) > 150){
garin 7:68af06a391f0 74 break;
garin 7:68af06a391f0 75 }
oktavianusirvan 0:ac8956bbab28 76 }
oktavianusirvan 0:ac8956bbab28 77 }
dwianiyulia 2:710b092cfcc0 78 int t1;
oktavianusirvan 0:ac8956bbab28 79
oktavianusirvan 0:ac8956bbab28 80 int main(){
garin 7:68af06a391f0 81 encoder.reset();
garin 7:68af06a391f0 82 int state = 1;
garin 7:68af06a391f0 83 wait(1);
garin 7:68af06a391f0 84 count_ball = 0;
garin 7:68af06a391f0 85 while (count_ball<6){
garin 7:68af06a391f0 86 pc.printf("%d \n",count_ball);
garin 1:7533e025de71 87 if (state && !infrared) {
garin 1:7533e025de71 88 state = 0 ;
garin 7:68af06a391f0 89 pneu = 0; // menembakkan bola
garin 7:68af06a391f0 90 wait(1);
garin 1:7533e025de71 91 count_ball++;
garin 7:68af06a391f0 92 // menggerakkan motor
dwianiyulia 3:3ee76478b110 93 if (count_ball == 2) MoveMotor(60);
dwianiyulia 3:3ee76478b110 94 else if (count_ball == 4) MoveMotor(90);
garin 7:68af06a391f0 95 wait(1);
garin 7:68af06a391f0 96
garin 1:7533e025de71 97
garin 1:7533e025de71 98 } else {
garin 7:68af06a391f0 99 pneu = 1;
garin 7:68af06a391f0 100 state = 1;
garin 7:68af06a391f0 101 wait(1);
oktavianusirvan 0:ac8956bbab28 102 }
garin 7:68af06a391f0 103
garin 7:68af06a391f0 104 }
garin 1:7533e025de71 105 }