KATAPANG / Mbed 2 deprecated PID1

bla

Dependencies:   mbed

Fork of PID by KATAPANG

main.cpp@1:edd7f45256f9, 2018-10-31 (annotated)

Committer:
arayhan878
Date:
Wed Oct 31 15:44:06 2018 +0000
Revision:
1:edd7f45256f9
Parent:
0:1d81f3296468
Child:
2:a214cf60ce58
t

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arayhan878 0:1d81f3296468 1 #include "mbed.h"
arayhan878 0:1d81f3296468 2
arayhan878 0:1d81f3296468 3 ///////////////////////PID///////////////////////
arayhan878 1:edd7f45256f9 4 float e, integral, derivative, preError;
arayhan878 1:edd7f45256f9 5 float out, output;
arayhan878 1:edd7f45256f9 6 float Dt = 0.01;
arayhan878 1:edd7f45256f9 7 float Kp = 0.13;
arayhan878 1:edd7f45256f9 8 float Ki = 0.01;
arayhan878 1:edd7f45256f9 9 float Kd = 0.001;
arayhan878 0:1d81f3296468 10 int setpoint = 0;
arayhan878 1:edd7f45256f9 11 //////////////////////////////////////////////////
arayhan878 1:edd7f45256f9 12 ////////////////////bluetooth////////////////////
arayhan878 0:1d81f3296468 13 Serial pc(USBTX, USBRX);
arayhan878 1:edd7f45256f9 14 Serial bt(PB_6, PB_7);
arayhan878 1:edd7f45256f9 15 /////////////////////////////////////////////////
arayhan878 1:edd7f45256f9 16 /////////////////////motor///////////////////////
arayhan878 1:edd7f45256f9 17 DigitalOut dir5(PB_8);
arayhan878 1:edd7f45256f9 18 DigitalOut dir6(PB_9);
arayhan878 1:edd7f45256f9 19 DigitalOut dir3(PC_5);
arayhan878 1:edd7f45256f9 20 DigitalOut dir4(PC_8);
arayhan878 1:edd7f45256f9 21 DigitalOut dir1(PB_12);
arayhan878 1:edd7f45256f9 22 DigitalOut dir2(PA_12);
arayhan878 1:edd7f45256f9 23 PwmOut pwm3(PC_9);
arayhan878 1:edd7f45256f9 24 PwmOut pwm2(PC_6);
arayhan878 1:edd7f45256f9 25 PwmOut pwm1(PA_11);
arayhan878 1:edd7f45256f9 26 ////////////////////////////////////////////////
arayhan878 0:1d81f3296468 27
arayhan878 0:1d81f3296468 28 int count = 0;
arayhan878 0:1d81f3296468 29 int pos1 = 0;
arayhan878 0:1d81f3296468 30 int pos2 = 0;
arayhan878 1:edd7f45256f9 31 int co, ch, ch1;
arayhan878 0:1d81f3296468 32
arayhan878 0:1d81f3296468 33 float roda1, roda2, roda3;
arayhan878 0:1d81f3296468 34 float pw1,pw2,pw3;
arayhan878 1:edd7f45256f9 35 float pwma1, pwma2, pwma3;
arayhan878 1:edd7f45256f9 36 float abs_pwm1, abs_pwm2, abs_pwm3;
arayhan878 0:1d81f3296468 37 double dx, dy, dz;
arayhan878 0:1d81f3296468 38
arayhan878 1:edd7f45256f9 39 //////////////////////inverse///////////////////////
arayhan878 1:edd7f45256f9 40 void atur(float x, float y, float the)
arayhan878 0:1d81f3296468 41 {
arayhan878 0:1d81f3296468 42 double a = sqrt(3.);
arayhan878 0:1d81f3296468 43 roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the;
arayhan878 0:1d81f3296468 44 roda2 = 1 * ((-0.5 * x) + (a / 2 * y)) + the;
arayhan878 0:1d81f3296468 45 roda3 = 1 * (x) + the;
arayhan878 0:1d81f3296468 46
arayhan878 0:1d81f3296468 47 if (roda1 > 255)
arayhan878 0:1d81f3296468 48 {pwma1 = 255;}
arayhan878 0:1d81f3296468 49 else if (roda1 < -255)
arayhan878 0:1d81f3296468 50 {pwma1 = -255;}
arayhan878 0:1d81f3296468 51 else pwma1 = roda1;
arayhan878 0:1d81f3296468 52
arayhan878 0:1d81f3296468 53 if (roda2 > 255)
arayhan878 0:1d81f3296468 54 {pwma2 = 255;}
arayhan878 0:1d81f3296468 55 else if (roda2 < -255)
arayhan878 0:1d81f3296468 56 {pwma2 = -255;}
arayhan878 0:1d81f3296468 57 else pwma2 = roda2;
arayhan878 0:1d81f3296468 58
arayhan878 0:1d81f3296468 59 if (roda3 > 255)
arayhan878 0:1d81f3296468 60 {pwma3 = 255;}
arayhan878 0:1d81f3296468 61 else if (roda3 < -255)
arayhan878 0:1d81f3296468 62 {pwma3 = -255;}
arayhan878 0:1d81f3296468 63 else pwma3 = roda3;
arayhan878 0:1d81f3296468 64
arayhan878 0:1d81f3296468 65 if (pwma1 < 0) {
arayhan878 0:1d81f3296468 66 abs_pwm1 = abs(pwma1);
arayhan878 0:1d81f3296468 67 pw1=(float) abs_pwm1/255;
arayhan878 0:1d81f3296468 68 dir1 = 1;
arayhan878 0:1d81f3296468 69 dir2 = 0;
arayhan878 0:1d81f3296468 70 }
arayhan878 0:1d81f3296468 71 else {
arayhan878 0:1d81f3296468 72 abs_pwm1 = pwma1;
arayhan878 0:1d81f3296468 73 pw1=(float) abs_pwm1/255;
arayhan878 0:1d81f3296468 74 dir1 = 0;
arayhan878 0:1d81f3296468 75 dir2 = 1;
arayhan878 0:1d81f3296468 76 }
arayhan878 0:1d81f3296468 77 if (pwma2 < 0) {
arayhan878 0:1d81f3296468 78 abs_pwm2 = abs(pwma2);
arayhan878 0:1d81f3296468 79 pw2=(float) abs_pwm2/255;
arayhan878 0:1d81f3296468 80 dir3 = 1;
arayhan878 0:1d81f3296468 81 dir4 = 0;
arayhan878 0:1d81f3296468 82 }
arayhan878 0:1d81f3296468 83 else {
arayhan878 0:1d81f3296468 84 abs_pwm2 = pwma2;
arayhan878 0:1d81f3296468 85 pw2=(float) abs_pwm2/255;
arayhan878 0:1d81f3296468 86 dir3 = 0;
arayhan878 0:1d81f3296468 87 dir4 = 1;
arayhan878 0:1d81f3296468 88 }
arayhan878 0:1d81f3296468 89 if (pwma3 < 0) {
arayhan878 0:1d81f3296468 90 abs_pwm3 = abs(pwma3);
arayhan878 0:1d81f3296468 91 pw3=(float) abs_pwm3/255;
arayhan878 1:edd7f45256f9 92 dir5 = 1;
arayhan878 1:edd7f45256f9 93 dir6 = 0;
arayhan878 0:1d81f3296468 94 }
arayhan878 0:1d81f3296468 95 else {
arayhan878 0:1d81f3296468 96 abs_pwm3 = pwma3;
arayhan878 0:1d81f3296468 97 pw3=(float) abs_pwm3/255;
arayhan878 1:edd7f45256f9 98 dir5 = 0;
arayhan878 1:edd7f45256f9 99 dir6 = 1;
arayhan878 0:1d81f3296468 100 }
arayhan878 0:1d81f3296468 101 }
arayhan878 0:1d81f3296468 102
arayhan878 0:1d81f3296468 103 void berhenti() {
arayhan878 0:1d81f3296468 104 pwm1 = 0.5;
arayhan878 0:1d81f3296468 105 pwm2 = 0.5;
arayhan878 0:1d81f3296468 106 pwm3 = 0.5;
arayhan878 0:1d81f3296468 107 dir1 = 1;
arayhan878 0:1d81f3296468 108 dir2 = 1;
arayhan878 0:1d81f3296468 109 dir3 = 1;
arayhan878 0:1d81f3296468 110 dir4 = 1;
arayhan878 0:1d81f3296468 111 dir5 = 1;
arayhan878 0:1d81f3296468 112 dir6 = 1;
arayhan878 0:1d81f3296468 113 }
arayhan878 1:edd7f45256f9 114 ////////////////////////////////////////////////
arayhan878 0:1d81f3296468 115
arayhan878 0:1d81f3296468 116 int main()
arayhan878 0:1d81f3296468 117 {
arayhan878 0:1d81f3296468 118 pc.baud(9600);
arayhan878 0:1d81f3296468 119 bt.baud(9600);
arayhan878 1:edd7f45256f9 120 pc.printf("starttttttt\n");
arayhan878 1:edd7f45256f9 121
arayhan878 0:1d81f3296468 122 /////////////////////////////////PID///////////////////////////////
arayhan878 0:1d81f3296468 123 preError = 0;
arayhan878 0:1d81f3296468 124 integral = 0;
arayhan878 1:edd7f45256f9 125 ////////////////////////////////////////////////////////////////////
arayhan878 0:1d81f3296468 126
arayhan878 0:1d81f3296468 127 while(1) {
arayhan878 1:edd7f45256f9 128
arayhan878 0:1d81f3296468 129 if(bt.readable())
arayhan878 0:1d81f3296468 130 {
arayhan878 0:1d81f3296468 131 ch1 = bt.getc();
arayhan878 1:edd7f45256f9 132 ch = ch1*2 - 240;
arayhan878 1:edd7f45256f9 133 if(ch != -240)
arayhan878 0:1d81f3296468 134 {
arayhan878 1:edd7f45256f9 135 pc.printf("yapppp ");
arayhan878 1:edd7f45256f9 136 pc.printf(" sudut : %d",ch);
arayhan878 0:1d81f3296468 137
arayhan878 1:edd7f45256f9 138 //////////////////////////////////PID///////////////////////////////
arayhan878 1:edd7f45256f9 139 e = setpoint - ch;
arayhan878 1:edd7f45256f9 140 integral = integral + (e * Dt);
arayhan878 1:edd7f45256f9 141 derivative = (e - preError) / Dt;
arayhan878 1:edd7f45256f9 142 output = (Kp * e) + (Ki * integral) + (Kd * derivative);
arayhan878 1:edd7f45256f9 143 preError = e;
arayhan878 1:edd7f45256f9 144 //////////////////////////////////PID///////////////////////////////
arayhan878 0:1d81f3296468 145
arayhan878 1:edd7f45256f9 146 out = output*-1;
arayhan878 1:edd7f45256f9 147 atur(0, 0, out);
arayhan878 1:edd7f45256f9 148 pwm1.write(pw1);
arayhan878 1:edd7f45256f9 149 pwm2.write(pw2);
arayhan878 1:edd7f45256f9 150 pwm3.write(pw3);
arayhan878 1:edd7f45256f9 151 pc.printf(" pre : %f",preError);
arayhan878 1:edd7f45256f9 152 pc.printf(" out : %f",out);
arayhan878 1:edd7f45256f9 153 pc.printf(" pw1 : %f",pw1);
arayhan878 1:edd7f45256f9 154 pc.printf(" pw2 : %f",pw2);
arayhan878 1:edd7f45256f9 155 pc.printf(" pw3 : %f",pw3);
arayhan878 1:edd7f45256f9 156
arayhan878 1:edd7f45256f9 157 pc.printf(" I : %f",integral);
arayhan878 1:edd7f45256f9 158 pc.printf(" D : %f",derivative);
arayhan878 1:edd7f45256f9 159 pc.printf(" error : %f",e);
arayhan878 1:edd7f45256f9 160 //pc.printf(" pwm : %d",out);
arayhan878 1:edd7f45256f9 161 pc.printf(" rod1 = %0.2f ", roda1 );
arayhan878 1:edd7f45256f9 162 pc.printf(" rod2 = %0.2f ", roda2 );
arayhan878 1:edd7f45256f9 163 pc.printf(" rod3 = %0.2f\n ", roda3 );
arayhan878 1:edd7f45256f9 164
arayhan878 1:edd7f45256f9 165 // wait(1);
arayhan878 0:1d81f3296468 166 }
arayhan878 0:1d81f3296468 167 else
arayhan878 0:1d81f3296468 168 {
arayhan878 1:edd7f45256f9 169 pc.printf("nope \n");
arayhan878 1:edd7f45256f9 170 atur(0, 0, 0);
arayhan878 1:edd7f45256f9 171 pwm1.write(pw1);
arayhan878 1:edd7f45256f9 172 pwm2.write(pw2);
arayhan878 1:edd7f45256f9 173 pwm3.write(pw3);
arayhan878 0:1d81f3296468 174 }
arayhan878 0:1d81f3296468 175 }
arayhan878 0:1d81f3296468 176
arayhan878 0:1d81f3296468 177 //atur(0, 150, 0); //mundur
arayhan878 0:1d81f3296468 178 //pc.printf("Mundur");
arayhan878 0:1d81f3296468 179 // pc.printf("Kiri");
arayhan878 0:1d81f3296468 180 // atur(-150, 0, 0); //kiri
arayhan878 0:1d81f3296468 181 // pwm1.write(pw1);
arayhan878 0:1d81f3296468 182 // pwm2.write(pw2);
arayhan878 0:1d81f3296468 183 // pwm3.write(pw3);
arayhan878 0:1d81f3296468 184
arayhan878 0:1d81f3296468 185 //}
arayhan878 0:1d81f3296468 186 //else if (data == 'g') {
arayhan878 0:1d81f3296468 187 // pc.printf("Kiri");
arayhan878 0:1d81f3296468 188 // atur(-60, 0, 0); //kiri
arayhan878 0:1d81f3296468 189 //}
arayhan878 0:1d81f3296468 190 //else if (data == 'i') {
arayhan878 0:1d81f3296468 191 // pc.printf("Kanan");
arayhan878 0:1d81f3296468 192 // atur(60, 0, 0); //kanan
arayhan878 0:1d81f3296468 193 //}
arayhan878 0:1d81f3296468 194
arayhan878 0:1d81f3296468 195 //else if (data == 'l')
arayhan878 0:1d81f3296468 196 //{
arayhan878 0:1d81f3296468 197 // atur(0, 0, -45); //Putar
arayhan878 0:1d81f3296468 198 // pc.printf("Putar Kanan");
arayhan878 0:1d81f3296468 199 // data = 0;
arayhan878 0:1d81f3296468 200 //}
arayhan878 0:1d81f3296468 201 //else if (data == 'j')
arayhan878 0:1d81f3296468 202 //{
arayhan878 0:1d81f3296468 203 // atur(0, 0, 45); //Putar
arayhan878 0:1d81f3296468 204 // pc.printf("Putar Kiri");
arayhan878 0:1d81f3296468 205 // data = 0;
arayhan878 0:1d81f3296468 206 //}
arayhan878 0:1d81f3296468 207
arayhan878 0:1d81f3296468 208 }
arayhan878 0:1d81f3296468 209 }