KATAPANG
/
PID
bla
main.cpp
- Committer:
- arayhan878
- Date:
- 2018-08-29
- Revision:
- 0:1d81f3296468
- Child:
- 1:679511199e94
File content as of revision 0:1d81f3296468:
#include "mbed.h" ///////////////////////PID/////////////////////// int out, e, integral, derivative, preError, output; float Dt = 1; float Kp = 1; float Ki = 0; float Kd = 2; int setpoint = 0; ///////////////////////PID//////////////////////// Serial pc(USBTX, USBRX); Serial bt(PA_11, PA_12); int co, ch, ch1; DigitalOut dir1(PA_10); DigitalOut dir2(PB_3); DigitalOut dir3(PA_7); DigitalOut dir4(PB_6); DigitalOut dir5(PC_7); DigitalOut dir6(PA_9); PwmOut pwm1(PA_8); PwmOut pwm2(PB_10); PwmOut pwm3(PB_4); int count = 0; int pos1 = 0; int pos2 = 0; float roda1, roda2, roda3; float pw1,pw2,pw3; int pwma1, pwma2, pwma3; int abs_pwm1, abs_pwm2, abs_pwm3; double dx, dy, dz; void atur(double x, double y, double 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/255; dir1 = 1; dir2 = 0; } else { abs_pwm1 = pwma1; pw1=(float) abs_pwm1/255; dir1 = 0; dir2 = 1; } if (pwma2 < 0) { abs_pwm2 = abs(pwma2); pw2=(float) abs_pwm2/255; dir3 = 1; dir4 = 0; } else { abs_pwm2 = pwma2; pw2=(float) abs_pwm2/255; dir3 = 0; dir4 = 1; } if (pwma3 < 0) { abs_pwm3 = abs(pwma3); pw3=(float) abs_pwm3/255; dir5 = 0; dir6 = 1; } else { abs_pwm3 = pwma3; pw3=(float) abs_pwm3/255; dir5 = 1; dir6 = 0; } } 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("saaaarttt"); /////////////////////////////////PID/////////////////////////////// preError = 0; integral = 0; /////////////////////////////////PID///////////////////////////////// pc.printf("starttttttt\n"); while(1) { if(bt.readable()) { ch1 = bt.getc(); ch = ch1 - 90; if(ch != -34) { pc.printf("yapppp "); pc.printf(" sudut : %d",ch); //////////////////////////////////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; atur(0, 0, out); //pc.printf(" pw1 : %f",pw1); //pc.printf(" pw2 : %f",pw2); //pc.printf(" pw3 : %f",pw3); //pc.printf(" I : %d",integral); //pc.printf(" D : %d",derivative); //pc.printf(" error : %d",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 ); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); } 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; //} // else // { // berhenti(); // pc.printf("Stop"); // } } }