Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@4:8a50d972066d, 2019-05-14 (annotated)

Committer:
dwianiyulia
Date:
Tue May 14 08:37:02 2019 +0000
Revision:
4:8a50d972066d
Parent:
3:3ee76478b110
Child:
5:dee879b9ae82
idontknow;

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