Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@6:e9ef196da46a, 2019-05-14 (annotated)

Committer:
rahachu
Date:
Tue May 14 15:53:14 2019 +0000
Revision:
6:e9ef196da46a
Parent:
5:dee879b9ae82
Child:
7:68af06a391f0
Try Motor;

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
rahachu 6:e9ef196da46a 10 Motor main_motor(PB_13, PB_14, PC_4); // input pin
rahachu 6:e9ef196da46a 11 DigitalOut pneu(PC_6);// input pin
rahachu 6:e9ef196da46a 12 DigitalIn infrared(PC_1); // input pin
rahachu 6:e9ef196da46a 13 encoderKRAI encoder(PC_10,PC_11,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 ;
rahachu 6:e9ef196da46a 22 DigitalOut pneu1(PC_7);
rahachu 6:e9ef196da46a 23 DigitalOut pneu2(PC_8);
rahachu 6:e9ef196da46a 24 DigitalOut pneu3(PC_9);
rahachu 6:e9ef196da46a 25 DigitalOut pneu4(PC_6);
rahachu 6:e9ef196da46a 26 DigitalOut pneu5(PC_5);
rahachu 6:e9ef196da46a 27 DigitalOut pneu6(PB_6);
oktavianusirvan 0:ac8956bbab28 28
oktavianusirvan 0:ac8956bbab28 29 void BacaEncoder(){// read encoder
oktavianusirvan 0:ac8956bbab28 30 pulse = (double)encoder.getPulses();
oktavianusirvan 0:ac8956bbab28 31 encoder.reset();
dwianiyulia 5:dee879b9ae82 32 total_pulse += pulse;
oktavianusirvan 0:ac8956bbab28 33 total_pulse_in_degree = total_pulse*360/538;
oktavianusirvan 0:ac8956bbab28 34 }
oktavianusirvan 0:ac8956bbab28 35
dwianiyulia 2:710b092cfcc0 36 void PID(double theta){
garin 1:7533e025de71 37 //pid
dwianiyulia 2:710b092cfcc0 38 galat = theta - total_pulse_in_degree;
dwianiyulia 2:710b092cfcc0 39 derivative = galat - last_galat;
garin 1:7533e025de71 40
rahachu 6:e9ef196da46a 41 pwm = (0.006*galat) + (3*derivative);
garin 1:7533e025de71 42
garin 1:7533e025de71 43 //limit the power of motor, the max power can be changed
dwianiyulia 5:dee879b9ae82 44 if (pwm > 0.9) pwm = 0.9;
dwianiyulia 5:dee879b9ae82 45 else if (pwm < -0.9) pwm = -0.9;
dwianiyulia 5:dee879b9ae82 46 }
garin 1:7533e025de71 47
dwianiyulia 2:710b092cfcc0 48 void MoveMotor(double theta){
garin 1:7533e025de71 49 not_stop = 1;
dwianiyulia 4:8a50d972066d 50 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 51 total_pulse =0;
rahachu 6:e9ef196da46a 52 integral = 0;
oktavianusirvan 0:ac8956bbab28 53
oktavianusirvan 0:ac8956bbab28 54 // move motor
dwianiyulia 2:710b092cfcc0 55 last_galat = theta;
oktavianusirvan 0:ac8956bbab28 56 while ((not_stop) && theta != 0){
oktavianusirvan 0:ac8956bbab28 57 BacaEncoder();
rahachu 6:e9ef196da46a 58 //PID(theta);
rahachu 6:e9ef196da46a 59 galat = theta - total_pulse_in_degree;
rahachu 6:e9ef196da46a 60 derivative = galat - last_galat;
rahachu 6:e9ef196da46a 61 integral += galat;
rahachu 6:e9ef196da46a 62 pwm = (0.007*galat) + (3*derivative) + (0.000007*integral) ;
rahachu 6:e9ef196da46a 63
rahachu 6:e9ef196da46a 64 //limit the power of motor, the max power can be changed
rahachu 6:e9ef196da46a 65 if (pwm > 0.9) pwm = 0.9;
rahachu 6:e9ef196da46a 66 else if (pwm < -0.9) pwm = -0.9;
oktavianusirvan 0:ac8956bbab28 67 // set speed motor
rahachu 6:e9ef196da46a 68 if ((pwm > 0.000001) || (pwm < -0.000001)) main_motor.speed(pwm);
oktavianusirvan 0:ac8956bbab28 69 else {
oktavianusirvan 0:ac8956bbab28 70 main_motor.speed(0);
oktavianusirvan 0:ac8956bbab28 71 main_motor.brake(1);
rahachu 6:e9ef196da46a 72 if (fabs(pwm)<0.000001) not_stop = 1;
oktavianusirvan 0:ac8956bbab28 73 }
dwianiyulia 2:710b092cfcc0 74 last_galat = galat;
rahachu 6:e9ef196da46a 75 pc.printf("%f.2 \n",total_pulse_in_degree);
oktavianusirvan 0:ac8956bbab28 76 }
oktavianusirvan 0:ac8956bbab28 77 }
dwianiyulia 2:710b092cfcc0 78 int t1;
oktavianusirvan 0:ac8956bbab28 79
oktavianusirvan 0:ac8956bbab28 80 int main(){
dwianiyulia 5:dee879b9ae82 81 while (1){
rahachu 6:e9ef196da46a 82 BacaEncoder();
rahachu 6:e9ef196da46a 83 //pc.printf("%f.2 \n",total_pulse_in_degree);
rahachu 6:e9ef196da46a 84 MoveMotor(60);
rahachu 6:e9ef196da46a 85 pneu1=0;
rahachu 6:e9ef196da46a 86 pneu2=0;
rahachu 6:e9ef196da46a 87 pneu3=0;
rahachu 6:e9ef196da46a 88 pneu4=0;
rahachu 6:e9ef196da46a 89 pneu5=0;
rahachu 6:e9ef196da46a 90 pneu6=0;
dwianiyulia 4:8a50d972066d 91 }
dwianiyulia 5:dee879b9ae82 92
dwianiyulia 5:dee879b9ae82 93
dwianiyulia 4:8a50d972066d 94 /*while (count_ball<6){
garin 1:7533e025de71 95 if (state && !infrared) {
garin 1:7533e025de71 96 state = 0 ;
garin 1:7533e025de71 97 pneu = 1; // menembakkan bola
garin 1:7533e025de71 98 count_ball++;
dwianiyulia 3:3ee76478b110 99 t1 = us_ticker_read();
garin 1:7533e025de71 100
garin 1:7533e025de71 101 // menggerakkan motor
dwianiyulia 3:3ee76478b110 102 if (count_ball == 2) MoveMotor(60);
dwianiyulia 3:3ee76478b110 103 else if (count_ball == 4) MoveMotor(90);
garin 1:7533e025de71 104
garin 1:7533e025de71 105 } else {
oktavianusirvan 0:ac8956bbab28 106 pneu = 0;
dwianiyulia 3:3ee76478b110 107 if (us_ticker_read()-t1 >1000 ) state = 1; // tunggu pneumatik kembali
oktavianusirvan 0:ac8956bbab28 108 }
dwianiyulia 4:8a50d972066d 109 }*/
garin 1:7533e025de71 110 }