ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
main.cpp@0:ac8956bbab28, 2019-05-09 (annotated)
- Committer:
- oktavianusirvan
- Date:
- Thu May 09 10:27:51 2019 +0000
- Revision:
- 0:ac8956bbab28
- Child:
- 1:7533e025de71
ini github kan?;
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; |
oktavianusirvan | 0:ac8956bbab28 | 17 | |
oktavianusirvan | 0:ac8956bbab28 | 18 | void BacaEncoder(){// read encoder |
oktavianusirvan | 0:ac8956bbab28 | 19 | pulse = (double)encoder.getPulses(); |
oktavianusirvan | 0:ac8956bbab28 | 20 | encoder.reset(); |
oktavianusirvan | 0:ac8956bbab28 | 21 | total_pulse +=pulse; |
oktavianusirvan | 0:ac8956bbab28 | 22 | total_pulse_in_degree = total_pulse*360/538; |
oktavianusirvan | 0:ac8956bbab28 | 23 | } |
oktavianusirvan | 0:ac8956bbab28 | 24 | |
oktavianusirvan | 0:ac8956bbab28 | 25 | void MoveMotor(int count_ball){ |
oktavianusirvan | 0:ac8956bbab28 | 26 | bool not_stop = 1; |
oktavianusirvan | 0:ac8956bbab28 | 27 | double error, integral, derivative; |
oktavianusirvan | 0:ac8956bbab28 | 28 | double last_error = 0; |
oktavianusirvan | 0:ac8956bbab28 | 29 | total_pulse =0; |
oktavianusirvan | 0:ac8956bbab28 | 30 | |
oktavianusirvan | 0:ac8956bbab28 | 31 | // take max theta |
oktavianusirvan | 0:ac8956bbab28 | 32 | if (count_ball % 2 == 0){ |
oktavianusirvan | 0:ac8956bbab28 | 33 | if (count_ball == 2) theta = 60; |
oktavianusirvan | 0:ac8956bbab28 | 34 | else if (count_ball == 4) theta = 90; |
oktavianusirvan | 0:ac8956bbab28 | 35 | else theta = 0; |
oktavianusirvan | 0:ac8956bbab28 | 36 | } |
oktavianusirvan | 0:ac8956bbab28 | 37 | |
oktavianusirvan | 0:ac8956bbab28 | 38 | // move motor |
oktavianusirvan | 0:ac8956bbab28 | 39 | while ((not_stop) && theta != 0){ |
oktavianusirvan | 0:ac8956bbab28 | 40 | BacaEncoder(); |
oktavianusirvan | 0:ac8956bbab28 | 41 | |
oktavianusirvan | 0:ac8956bbab28 | 42 | //pid |
oktavianusirvan | 0:ac8956bbab28 | 43 | error = theta - total_pulse_in_degree; |
oktavianusirvan | 0:ac8956bbab28 | 44 | integral = integral + error; |
oktavianusirvan | 0:ac8956bbab28 | 45 | derivative = error - last_error; |
oktavianusirvan | 0:ac8956bbab28 | 46 | |
oktavianusirvan | 0:ac8956bbab28 | 47 | pwm = (kp*error) + (ki*integral) + (kd*derivative); |
oktavianusirvan | 0:ac8956bbab28 | 48 | |
oktavianusirvan | 0:ac8956bbab28 | 49 | //limit the power of motor, the max power can be changed |
oktavianusirvan | 0:ac8956bbab28 | 50 | if (pwm > 255) pwm = 255; |
oktavianusirvan | 0:ac8956bbab28 | 51 | else if (pwm < -255) pwm = -255; |
oktavianusirvan | 0:ac8956bbab28 | 52 | |
oktavianusirvan | 0:ac8956bbab28 | 53 | // set speed motor |
oktavianusirvan | 0:ac8956bbab28 | 54 | if (pwm > 0) main_motor._fwd(pwm); |
oktavianusirvan | 0:ac8956bbab28 | 55 | else if (pwm < 0) main_motor._rev(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 | } |
oktavianusirvan | 0:ac8956bbab28 | 61 | last_error = 0; |
oktavianusirvan | 0:ac8956bbab28 | 62 | } |
oktavianusirvan | 0:ac8956bbab28 | 63 | |
oktavianusirvan | 0:ac8956bbab28 | 64 | } |
oktavianusirvan | 0:ac8956bbab28 | 65 | |
oktavianusirvan | 0:ac8956bbab28 | 66 | int main(){ |
oktavianusirvan | 0:ac8956bbab28 | 67 | count_ball = 0 ; |
oktavianusirvan | 0:ac8956bbab28 | 68 | |
oktavianusirvan | 0:ac8956bbab28 | 69 | while (count_ball < 6 ) { // eop if all ball has been throwen |
oktavianusirvan | 0:ac8956bbab28 | 70 | if (infrared ){ |
oktavianusirvan | 0:ac8956bbab28 | 71 | pneu = 1; |
oktavianusirvan | 0:ac8956bbab28 | 72 | milis(500); |
oktavianusirvan | 0:ac8956bbab28 | 73 | pneu = 0; |
oktavianusirvan | 0:ac8956bbab28 | 74 | count_ball +=1; // increment of ball |
oktavianusirvan | 0:ac8956bbab28 | 75 | } |
oktavianusirvan | 0:ac8956bbab28 | 76 | moveMotor(count_ball); |
oktavianusirvan | 0:ac8956bbab28 | 77 | milis(500); // adjust with try |
oktavianusirvan | 0:ac8956bbab28 | 78 | } |
oktavianusirvan | 0:ac8956bbab28 | 79 | } |