Magang KRAI 2019 / Mbed 2 deprecated OnMotor

on Motor

Dependencies:   encoderKRAI mbed Motor_new

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?

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;
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 }