Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@9:383084a01131, 2019-05-19 (annotated)

Committer:
oktavianusirvan
Date:
Sun May 19 18:54:54 2019 +0000
Revision:
9:383084a01131
Parent:
8:069a607ef761
Child:
10:95e41bc4252c
Jadi gaes...;

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;
oktavianusirvan 9:383084a01131 57 pwm = (0.01*galat)+(derivative) ;
rahachu 6:e9ef196da46a 58
rahachu 6:e9ef196da46a 59 //limit the power of motor, the max power can be changed
oktavianusirvan 9:383084a01131 60 if (pwm > 0.4) pwm = 0.4;
oktavianusirvan 9:383084a01131 61 else if (pwm < -0.4) pwm = -0.4;
oktavianusirvan 0:ac8956bbab28 62 // set speed motor
oktavianusirvan 9:383084a01131 63 if (fabs(galat) > 0.6 || (fabs(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;
oktavianusirvan 9:383084a01131 72 pc.printf("%f.2 \n", total_pulse_in_degree);
oktavianusirvan 8:069a607ef761 73 if (fabs(total_pulse_in_degree) > 120){
oktavianusirvan 8:069a607ef761 74 main_motor.speed(0);
oktavianusirvan 8:069a607ef761 75 main_motor.brake(1);
oktavianusirvan 8:069a607ef761 76 not_stop = 0;
garin 7:68af06a391f0 77 break;
garin 7:68af06a391f0 78 }
oktavianusirvan 0:ac8956bbab28 79 }
oktavianusirvan 0:ac8956bbab28 80 }
dwianiyulia 2:710b092cfcc0 81 int t1;
oktavianusirvan 0:ac8956bbab28 82
oktavianusirvan 0:ac8956bbab28 83 int main(){
oktavianusirvan 8:069a607ef761 84 pneu = 1;
garin 7:68af06a391f0 85 encoder.reset();
garin 7:68af06a391f0 86 int state = 1;
garin 7:68af06a391f0 87 wait(1);
garin 7:68af06a391f0 88 count_ball = 0;
oktavianusirvan 8:069a607ef761 89
garin 7:68af06a391f0 90 while (count_ball<6){
garin 7:68af06a391f0 91 pc.printf("%d \n",count_ball);
garin 1:7533e025de71 92 if (state && !infrared) {
oktavianusirvan 9:383084a01131 93 wait(1);
garin 1:7533e025de71 94 state = 0 ;
garin 7:68af06a391f0 95 pneu = 0; // menembakkan bola
garin 7:68af06a391f0 96 wait(1);
garin 1:7533e025de71 97 count_ball++;
garin 7:68af06a391f0 98 // menggerakkan motor
dwianiyulia 3:3ee76478b110 99 if (count_ball == 2) MoveMotor(60);
dwianiyulia 3:3ee76478b110 100 else if (count_ball == 4) MoveMotor(90);
garin 7:68af06a391f0 101 wait(1);
garin 7:68af06a391f0 102
garin 1:7533e025de71 103
garin 1:7533e025de71 104 } else {
garin 7:68af06a391f0 105 pneu = 1;
garin 7:68af06a391f0 106 state = 1;
garin 7:68af06a391f0 107 wait(1);
oktavianusirvan 0:ac8956bbab28 108 }
oktavianusirvan 9:383084a01131 109 if (count_ball==6) pneu = 1;
garin 7:68af06a391f0 110
garin 7:68af06a391f0 111 }
garin 1:7533e025de71 112 }