bla
Dependencies: mbed
Fork of PID by
main.cpp
- Committer:
- arayhan878
- Date:
- 2018-11-01
- Revision:
- 2:a214cf60ce58
- Parent:
- 1:edd7f45256f9
File content as of revision 2:a214cf60ce58:
#include "mbed.h" ///////////////////////PID/////////////////////// float e, integral, derivative, preError; float out, output; float Dt = 0.01; float Kp = 0.13; float Ki = 0.024; float Kd = 0.0014; int setpoint = 120; ////////////////////////////////////////////////// ////////////////////bluetooth//////////////////// Serial pc(USBTX, USBRX); Serial bt(PB_6, PB_7); ///////////////////////////////////////////////// /////////////////////motor/////////////////////// DigitalOut dir5(PB_8); DigitalOut dir6(PB_9); DigitalOut dir3(PC_5); DigitalOut dir4(PC_8); DigitalOut dir1(PB_12); DigitalOut dir2(PA_12); PwmOut pwm3(PC_9); PwmOut pwm2(PC_6); PwmOut pwm1(PA_11); //////////////////////////////////////////////// int count = 0; int pos1 = 0; int pos2 = 0; int co, ch, ch1; float roda1, roda2, roda3; float pw1,pw2,pw3; float pwma1, pwma2, pwma3; float abs_pwm1, abs_pwm2, abs_pwm3; double dx, dy, dz; //////////////////////inverse/////////////////////// void atur(float x, float y, float the) { double a = sqrt(3.); roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the; roda2 = 1 * ((-0.5 * x) + (a / 2 * y)) + the; roda3 = 1 * (x) + the; if (roda1 > 255) {pwma1 = 255;} else if (roda1 < -255) {pwma1 = -255;} else pwma1 = roda1; if (roda2 > 255) {pwma2 = 255;} else if (roda2 < -255) {pwma2 = -255;} else pwma2 = roda2; if (roda3 > 255) {pwma3 = 255;} else if (roda3 < -255) {pwma3 = -255;} else pwma3 = roda3; if (pwma1 < 0) { abs_pwm1 = abs(pwma1); pw1=(float) abs_pwm1/200; dir1 = 1; dir2 = 0; } else { abs_pwm1 = pwma1; pw1=(float) abs_pwm1/200; dir1 = 0; dir2 = 1; } if (pwma2 < 0) { abs_pwm2 = abs(pwma2); pw2=(float) abs_pwm2/200; dir3 = 1; dir4 = 0; } else { abs_pwm2 = pwma2; pw2=(float) abs_pwm2/200; dir3 = 0; dir4 = 1; } if (pwma3 < 0) { abs_pwm3 = abs(pwma3); pw3=(float) abs_pwm3/200; dir5 = 1; dir6 = 0; } else { abs_pwm3 = pwma3; pw3=(float) abs_pwm3/200; dir5 = 0; dir6 = 1; } } void berhenti() { pwm1 = 0.5; pwm2 = 0.5; pwm3 = 0.5; dir1 = 1; dir2 = 1; dir3 = 1; dir4 = 1; dir5 = 1; dir6 = 1; } //////////////////////////////////////////////// int main() { pc.baud(9600); bt.baud(9600); pc.printf("starttttttt\n"); /////////////////////////////////PID/////////////////////////////// preError = 0; integral = 0; //////////////////////////////////////////////////////////////////// while(1) { if(bt.readable()) { ch1 = bt.getc(); ch = ch1; if(ch != 0) { //////////////////////////////////PID/////////////////////////////// e = setpoint - ch; integral = integral + (e * Dt); derivative = (e - preError) / Dt; output = (Kp * e) + (Ki * integral) + (Kd * derivative); preError = e; //////////////////////////////////PID/////////////////////////////// out = (output*-1); if (e>20 || e<-20) { pc.printf(" PID"); pc.printf(" yapppp "); pc.printf(" sudut : %d",ch); pc.printf(" out : %f",out); pc.printf(" e : %f\n",e); atur(0, 0, out); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); if (out > -10 && out < -1) { atur(0, 0, -10); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); wait(0.1); } if (out < 10 && out > 1) { atur(0, 0, 10); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); wait(0.1); } } if (e <20 && e>-20) { atur(0, -50, 0); pc.printf(" Maju"); pc.printf(" yapppp "); pc.printf(" sudut : %d",ch); pc.printf(" e : %f\n",e); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); } // pc.printf(" out : %f",out); // pc.printf(" pw1 : %f",pw1); // pc.printf(" pw2 : %f",pw2); // pc.printf(" pw3 : %f",pw3); // pc.printf(" I : %f",integral); // pc.printf(" D : %f",derivative); // pc.printf(" error : %f",e); //pc.printf(" pwm : %d",out); // pc.printf(" rod1 = %0.2f ", roda1 ); // pc.printf(" rod2 = %0.2f ", roda2 ); // pc.printf(" rod3 = %0.2f\n ", roda3 ); // wait(1); } else { pc.printf("nope \n"); atur(0, 0, 0); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); } } //atur(0, 150, 0); //mundur //pc.printf("Mundur"); // pc.printf("Kiri"); // atur(-150, 0, 0); //kiri // pwm1.write(pw1); // pwm2.write(pw2); // pwm3.write(pw3); //} //else if (data == 'g') { // pc.printf("Kiri"); // atur(-60, 0, 0); //kiri //} //else if (data == 'i') { // pc.printf("Kanan"); // atur(60, 0, 0); //kanan //} //else if (data == 'l') //{ // atur(0, 0, -45); //Putar // pc.printf("Putar Kanan"); // data = 0; //} //else if (data == 'j') //{ // atur(0, 0, 45); //Putar // pc.printf("Putar Kiri"); // data = 0; //} } }