KATAPANG / Mbed 2 deprecated PID1

bla

Dependencies:   mbed

Fork of PID by KATAPANG

main.cpp@2:a214cf60ce58, 2018-11-01 (annotated)

Committer:
arayhan878
Date:
Thu Nov 01 17:22:26 2018 +0000
Revision:
2:a214cf60ce58
Parent:
1:edd7f45256f9 
yang baru

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 2:a214cf60ce58 8 float Ki = 0.024;
arayhan878 2:a214cf60ce58 9 float Kd = 0.0014;
arayhan878 2:a214cf60ce58 10 int setpoint = 120;
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 2:a214cf60ce58 67 pw1=(float) abs_pwm1/200;
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 2:a214cf60ce58 73 pw1=(float) abs_pwm1/200;
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 2:a214cf60ce58 79 pw2=(float) abs_pwm2/200;
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 2:a214cf60ce58 85 pw2=(float) abs_pwm2/200;
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 2:a214cf60ce58 91 pw3=(float) abs_pwm3/200;
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 2:a214cf60ce58 97 pw3=(float) abs_pwm3/200;
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 2:a214cf60ce58 132 ch = ch1;
arayhan878 2:a214cf60ce58 133 if(ch != 0)
arayhan878 0:1d81f3296468 134 {
arayhan878 2:a214cf60ce58 135
arayhan878 1:edd7f45256f9 136 //////////////////////////////////PID///////////////////////////////
arayhan878 1:edd7f45256f9 137 e = setpoint - ch;
arayhan878 1:edd7f45256f9 138 integral = integral + (e * Dt);
arayhan878 1:edd7f45256f9 139 derivative = (e - preError) / Dt;
arayhan878 1:edd7f45256f9 140 output = (Kp * e) + (Ki * integral) + (Kd * derivative);
arayhan878 1:edd7f45256f9 141 preError = e;
arayhan878 1:edd7f45256f9 142 //////////////////////////////////PID///////////////////////////////
arayhan878 2:a214cf60ce58 143
arayhan878 2:a214cf60ce58 144 out = (output*-1);
arayhan878 2:a214cf60ce58 145
arayhan878 2:a214cf60ce58 146 if (e>20 || e<-20)
arayhan878 2:a214cf60ce58 147 {
arayhan878 2:a214cf60ce58 148 pc.printf(" PID");
arayhan878 2:a214cf60ce58 149 pc.printf(" yapppp ");
arayhan878 2:a214cf60ce58 150 pc.printf(" sudut : %d",ch);
arayhan878 2:a214cf60ce58 151 pc.printf(" out : %f",out);
arayhan878 2:a214cf60ce58 152 pc.printf(" e : %f\n",e);
arayhan878 2:a214cf60ce58 153 atur(0, 0, out);
arayhan878 2:a214cf60ce58 154 pwm1.write(pw1);
arayhan878 2:a214cf60ce58 155 pwm2.write(pw2);
arayhan878 2:a214cf60ce58 156 pwm3.write(pw3);
arayhan878 2:a214cf60ce58 157 if (out > -10 && out < -1)
arayhan878 2:a214cf60ce58 158 {
arayhan878 2:a214cf60ce58 159 atur(0, 0, -10);
arayhan878 2:a214cf60ce58 160 pwm1.write(pw1);
arayhan878 2:a214cf60ce58 161 pwm2.write(pw2);
arayhan878 2:a214cf60ce58 162 pwm3.write(pw3);
arayhan878 2:a214cf60ce58 163 wait(0.1);
arayhan878 2:a214cf60ce58 164 }
arayhan878 2:a214cf60ce58 165 if (out < 10 && out > 1)
arayhan878 2:a214cf60ce58 166 {
arayhan878 2:a214cf60ce58 167 atur(0, 0, 10);
arayhan878 2:a214cf60ce58 168 pwm1.write(pw1);
arayhan878 2:a214cf60ce58 169 pwm2.write(pw2);
arayhan878 2:a214cf60ce58 170 pwm3.write(pw3);
arayhan878 2:a214cf60ce58 171 wait(0.1);
arayhan878 2:a214cf60ce58 172 }
arayhan878 2:a214cf60ce58 173
arayhan878 2:a214cf60ce58 174 }
arayhan878 2:a214cf60ce58 175 if (e <20 && e>-20)
arayhan878 2:a214cf60ce58 176 {
arayhan878 2:a214cf60ce58 177 atur(0, -50, 0);
arayhan878 2:a214cf60ce58 178 pc.printf(" Maju");
arayhan878 2:a214cf60ce58 179 pc.printf(" yapppp ");
arayhan878 2:a214cf60ce58 180 pc.printf(" sudut : %d",ch);
arayhan878 2:a214cf60ce58 181 pc.printf(" e : %f\n",e);
arayhan878 2:a214cf60ce58 182 pwm1.write(pw1);
arayhan878 2:a214cf60ce58 183 pwm2.write(pw2);
arayhan878 2:a214cf60ce58 184 pwm3.write(pw3);
arayhan878 2:a214cf60ce58 185 }
arayhan878 2:a214cf60ce58 186 // pc.printf(" out : %f",out);
arayhan878 2:a214cf60ce58 187 // pc.printf(" pw1 : %f",pw1);
arayhan878 2:a214cf60ce58 188 // pc.printf(" pw2 : %f",pw2);
arayhan878 2:a214cf60ce58 189 // pc.printf(" pw3 : %f",pw3);
arayhan878 0:1d81f3296468 190
arayhan878 2:a214cf60ce58 191 // pc.printf(" I : %f",integral);
arayhan878 2:a214cf60ce58 192 // pc.printf(" D : %f",derivative);
arayhan878 2:a214cf60ce58 193 // pc.printf(" error : %f",e);
arayhan878 1:edd7f45256f9 194 //pc.printf(" pwm : %d",out);
arayhan878 2:a214cf60ce58 195 // pc.printf(" rod1 = %0.2f ", roda1 );
arayhan878 2:a214cf60ce58 196 // pc.printf(" rod2 = %0.2f ", roda2 );
arayhan878 2:a214cf60ce58 197 // pc.printf(" rod3 = %0.2f\n ", roda3 );
arayhan878 1:edd7f45256f9 198
arayhan878 1:edd7f45256f9 199 // wait(1);
arayhan878 0:1d81f3296468 200 }
arayhan878 0:1d81f3296468 201 else
arayhan878 0:1d81f3296468 202 {
arayhan878 1:edd7f45256f9 203 pc.printf("nope \n");
arayhan878 1:edd7f45256f9 204 atur(0, 0, 0);
arayhan878 1:edd7f45256f9 205 pwm1.write(pw1);
arayhan878 1:edd7f45256f9 206 pwm2.write(pw2);
arayhan878 1:edd7f45256f9 207 pwm3.write(pw3);
arayhan878 0:1d81f3296468 208 }
arayhan878 0:1d81f3296468 209 }
arayhan878 0:1d81f3296468 210
arayhan878 0:1d81f3296468 211 //atur(0, 150, 0); //mundur
arayhan878 0:1d81f3296468 212 //pc.printf("Mundur");
arayhan878 0:1d81f3296468 213 // pc.printf("Kiri");
arayhan878 0:1d81f3296468 214 // atur(-150, 0, 0); //kiri
arayhan878 0:1d81f3296468 215 // pwm1.write(pw1);
arayhan878 0:1d81f3296468 216 // pwm2.write(pw2);
arayhan878 0:1d81f3296468 217 // pwm3.write(pw3);
arayhan878 0:1d81f3296468 218
arayhan878 0:1d81f3296468 219 //}
arayhan878 0:1d81f3296468 220 //else if (data == 'g') {
arayhan878 0:1d81f3296468 221 // pc.printf("Kiri");
arayhan878 0:1d81f3296468 222 // atur(-60, 0, 0); //kiri
arayhan878 0:1d81f3296468 223 //}
arayhan878 0:1d81f3296468 224 //else if (data == 'i') {
arayhan878 0:1d81f3296468 225 // pc.printf("Kanan");
arayhan878 0:1d81f3296468 226 // atur(60, 0, 0); //kanan
arayhan878 0:1d81f3296468 227 //}
arayhan878 0:1d81f3296468 228
arayhan878 0:1d81f3296468 229 //else if (data == 'l')
arayhan878 0:1d81f3296468 230 //{
arayhan878 0:1d81f3296468 231 // atur(0, 0, -45); //Putar
arayhan878 0:1d81f3296468 232 // pc.printf("Putar Kanan");
arayhan878 0:1d81f3296468 233 // data = 0;
arayhan878 0:1d81f3296468 234 //}
arayhan878 0:1d81f3296468 235 //else if (data == 'j')
arayhan878 0:1d81f3296468 236 //{
arayhan878 0:1d81f3296468 237 // atur(0, 0, 45); //Putar
arayhan878 0:1d81f3296468 238 // pc.printf("Putar Kiri");
arayhan878 0:1d81f3296468 239 // data = 0;
arayhan878 0:1d81f3296468 240 //}
arayhan878 0:1d81f3296468 241
arayhan878 0:1d81f3296468 242 }
arayhan878 0:1d81f3296468 243 }