ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
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?
User | Revision | Line number | New 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 | } |