Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

main.cpp@1:7533e025de71, 2019-05-12 (annotated)

Committer:
garin
Date:
Sun May 12 23:45:42 2019 +0000
Revision:
1:7533e025de71
Parent:
0:ac8956bbab28
Child:
2:710b092cfcc0
update main dan fungsi

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>
oktavianusirvan 0:ac8956bbab28 8 // declare
oktavianusirvan 0:ac8956bbab28 9 Motor main_motor(PB_13, PB_14, PB_15 ); // input pin
oktavianusirvan 0:ac8956bbab28 10 DigitalOut pneu(PC_5);// input pin
oktavianusirvan 0:ac8956bbab28 11 DigitalIn infrared(PC_5); // input pin
oktavianusirvan 0:ac8956bbab28 12 encoderKRAI encoder(PB_1,PB_2,538,encoderKRAI::X4_ENCODING);// input pin
oktavianusirvan 0:ac8956bbab28 13
oktavianusirvan 0:ac8956bbab28 14 int count, count_ball ;
oktavianusirvan 0:ac8956bbab28 15 double pulse, total_pulse,total_pulse_in_degree,theta,pwm;
oktavianusirvan 0:ac8956bbab28 16 double kp,ki,kd;
garin 1:7533e025de71 17 bool not_stop ;
garin 1:7533e025de71 18 double error, integral, derivative;
garin 1:7533e025de71 19 double last_error ;
oktavianusirvan 0:ac8956bbab28 20
oktavianusirvan 0:ac8956bbab28 21 void BacaEncoder(){// read encoder
oktavianusirvan 0:ac8956bbab28 22 pulse = (double)encoder.getPulses();
oktavianusirvan 0:ac8956bbab28 23 encoder.reset();
oktavianusirvan 0:ac8956bbab28 24 total_pulse +=pulse;
oktavianusirvan 0:ac8956bbab28 25 total_pulse_in_degree = total_pulse*360/538;
oktavianusirvan 0:ac8956bbab28 26 }
oktavianusirvan 0:ac8956bbab28 27
garin 1:7533e025de71 28 void PID(theta){
garin 1:7533e025de71 29 //pid
garin 1:7533e025de71 30 error = theta - total_pulse_in_degree;
garin 1:7533e025de71 31 derivative = error - last_error;
garin 1:7533e025de71 32
garin 1:7533e025de71 33 pwm = (kp*error) + (kd*derivative);
garin 1:7533e025de71 34
garin 1:7533e025de71 35 //limit the power of motor, the max power can be changed
garin 1:7533e025de71 36 if (pwm > 1) pwm = 1;
garin 1:7533e025de71 37 else if (pwm < -1) pwm = -1;
garin 1:7533e025de71 38
garin 1:7533e025de71 39
garin 1:7533e025de71 40 }
garin 1:7533e025de71 41 void MoveMotor(theta){
garin 1:7533e025de71 42 not_stop = 1;
garin 1:7533e025de71 43 last_error = 0;
oktavianusirvan 0:ac8956bbab28 44 total_pulse =0;
oktavianusirvan 0:ac8956bbab28 45
oktavianusirvan 0:ac8956bbab28 46 // move motor
garin 1:7533e025de71 47 last_error = theta;
oktavianusirvan 0:ac8956bbab28 48 while ((not_stop) && theta != 0){
oktavianusirvan 0:ac8956bbab28 49 BacaEncoder();
garin 1:7533e025de71 50 PID(theta);
oktavianusirvan 0:ac8956bbab28 51 // set speed motor
garin 1:7533e025de71 52 if (pwm != 0) main_motor.speed(pwm);
oktavianusirvan 0:ac8956bbab28 53 else {
oktavianusirvan 0:ac8956bbab28 54 main_motor.speed(0);
oktavianusirvan 0:ac8956bbab28 55 main_motor.brake(1);
oktavianusirvan 0:ac8956bbab28 56 not_stop = 0;
oktavianusirvan 0:ac8956bbab28 57 }
garin 1:7533e025de71 58 last_error = error;
oktavianusirvan 0:ac8956bbab28 59 }
oktavianusirvan 0:ac8956bbab28 60 }
oktavianusirvan 0:ac8956bbab28 61
oktavianusirvan 0:ac8956bbab28 62 int main(){
garin 1:7533e025de71 63 count_ball = 0;
garin 1:7533e025de71 64 int state = 1; // posisi awal
oktavianusirvan 0:ac8956bbab28 65
garin 1:7533e025de71 66 while (count_ball<6){
garin 1:7533e025de71 67 if (state && !infrared) {
garin 1:7533e025de71 68 state = 0 ;
garin 1:7533e025de71 69 pneu = 1; // menembakkan bola
garin 1:7533e025de71 70 count_ball++;
garin 1:7533e025de71 71 t1 = milis();
garin 1:7533e025de71 72
garin 1:7533e025de71 73 // menggerakkan motor
garin 1:7533e025de71 74 if (count_ball = 2) moveMotor(60);
garin 1:7533e025de71 75 else if (count_ball = 4) moveMotor(90);
garin 1:7533e025de71 76
garin 1:7533e025de71 77 } else {
oktavianusirvan 0:ac8956bbab28 78 pneu = 0;
garin 1:7533e025de71 79 if (milis()-t1 >1000 ) state = 1; // tunggu pneumatik kembali
oktavianusirvan 0:ac8956bbab28 80 }
oktavianusirvan 0:ac8956bbab28 81 }
garin 1:7533e025de71 82 }