Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@11:38e621509cdc, 2019-08-08 (annotated)

Committer:
oktavianusirvan
Date:
Thu Aug 08 12:18:35 2019 +0000
Revision:
11:38e621509cdc
Parent:
10:95e41bc4252c 
ini codenya;

Who changed what in which revision?

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