KRAI 2016 / Mbed 2 deprecated TunningLF2

tunninglf beta2

Dependencies:   ESC Motor PID PS_PAD hadah mbed

Fork of Ultimate_Hybrid by KRAI 2016

main.cpp@3:5f9fbb53c8d5, 2016-04-22 (annotated)

Committer:
Najib_irvani
Date:
Fri Apr 22 23:58:19 2016 +0000
Revision:
3:5f9fbb53c8d5
Parent:
2:df6c49846367 
tunningLinefollowerversion2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:edddd373a163 1 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 2 /** FILE HEADER **/
rizqicahyo 0:edddd373a163 3 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 4 #include "mbed.h"
rizqicahyo 0:edddd373a163 5 #include "Motor.h"
rizqicahyo 0:edddd373a163 6 #include "PS_PAD.h"
rizqicahyo 0:edddd373a163 7 #include "PID.h"
rizqicahyo 0:edddd373a163 8 #include "esc.h"
rizqicahyo 0:edddd373a163 9 #include "Servo.h"
rizqicahyo 0:edddd373a163 10
rizqicahyo 0:edddd373a163 11 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 12 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:edddd373a163 13 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 14 //serial
rizqicahyo 0:edddd373a163 15 Serial pc(USBTX,USBRX); //debug
rizqicahyo 0:edddd373a163 16 Serial com(PA_0,PA_1); //sensor
rizqicahyo 0:edddd373a163 17
rizqicahyo 0:edddd373a163 18 //joystick PS2
rizqicahyo 0:edddd373a163 19 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
rizqicahyo 0:edddd373a163 20
rizqicahyo 0:edddd373a163 21 //motor (PWM, forward, reverse)
rizqicahyo 0:edddd373a163 22 Motor motor1(PA_8, PB_0, PC_15);
rizqicahyo 0:edddd373a163 23 Motor motor2(PA_11, PA_6, PA_5);
Najib_irvani 3:5f9fbb53c8d5 24 //Motor motor4(PB_6, PA_7, PB_12);
rizqicahyo 0:edddd373a163 25 Motor motor3(PA_9, PC_2, PC_3);
Najib_irvani 3:5f9fbb53c8d5 26 Motor motorS(PB_7, PA_14, PA_15);
rizqicahyo 0:edddd373a163 27 Motor motorC1(PB_9, PA_12, PC_5);
rizqicahyo 0:edddd373a163 28 Motor motorC2(PB_8, PB_1, PA_13);
Najib_irvani 3:5f9fbb53c8d5 29 Motor motor4(PA_10, PB_5, PB_4);
rizqicahyo 0:edddd373a163 30
Najib_irvani 1:fc1535231c0d 31 /* coba coba
Najib_irvani 1:fc1535231c0d 32 //motor (PWM, forward, reverse)
Najib_irvani 1:fc1535231c0d 33 Motor motor1(PA_8, PB_0, PC_15);
Najib_irvani 1:fc1535231c0d 34 Motor motorC1(PA_11, PA_6, PA_5);
Najib_irvani 1:fc1535231c0d 35 //Motor motor3(PB_6, PA_7, PB_12);
Najib_irvani 1:fc1535231c0d 36 Motor motorC2(PA_9, PC_2, PC_3);
Najib_irvani 1:fc1535231c0d 37 Motor motor4(PB_7, PA_14, PA_15);
Najib_irvani 1:fc1535231c0d 38 Motor motor2(PB_9, PA_12, PC_5);
Najib_irvani 1:fc1535231c0d 39 Motor motor3(PB_8, PB_1, PA_13);
Najib_irvani 1:fc1535231c0d 40 Motor motorS(PA_10, PB_5, PB_4);
Najib_irvani 1:fc1535231c0d 41 */
Najib_irvani 1:fc1535231c0d 42
rizqicahyo 0:edddd373a163 43 //pnuematik
rizqicahyo 0:edddd373a163 44 DigitalOut pnuematik1(PC_11);
rizqicahyo 0:edddd373a163 45 DigitalOut pnuematik2(PC_10);
rizqicahyo 0:edddd373a163 46 DigitalOut pnuematik3(PD_2);
rizqicahyo 0:edddd373a163 47 DigitalOut pnuematik4(PC_12);
rizqicahyo 0:edddd373a163 48
rizqicahyo 0:edddd373a163 49 //Limit Switch
rizqicahyo 0:edddd373a163 50 DigitalIn limit1(PC_13 ,PullUp);
rizqicahyo 0:edddd373a163 51 DigitalIn limit2(PC_14 ,PullUp);
rizqicahyo 0:edddd373a163 52 DigitalIn limit3(PC_1 ,PullUp);
rizqicahyo 0:edddd373a163 53 DigitalIn limit4(PC_0 ,PullUp);
rizqicahyo 0:edddd373a163 54
rizqicahyo 0:edddd373a163 55 //PID line follower(P, I, D, Time Sampling)
Najib_irvani 3:5f9fbb53c8d5 56 PID PID(0.32,0.000,0.1,0.001);
rizqicahyo 0:edddd373a163 57
rizqicahyo 0:edddd373a163 58 //servo(PWM)
rizqicahyo 0:edddd373a163 59 Servo servoEDF(PC_8);
rizqicahyo 0:edddd373a163 60
rizqicahyo 0:edddd373a163 61 //EDF(PWM, timer)
rizqicahyo 0:edddd373a163 62 ESC edf(PC_6,20);
rizqicahyo 0:edddd373a163 63
rizqicahyo 0:edddd373a163 64 //Timer Pnuematik
rizqicahyo 2:df6c49846367 65 //Timer timer;
rizqicahyo 2:df6c49846367 66
rizqicahyo 2:df6c49846367 67
rizqicahyo 0:edddd373a163 68
rizqicahyo 0:edddd373a163 69 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 70 /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
rizqicahyo 0:edddd373a163 71 /*********************************************************************************************/
rizqicahyo 2:df6c49846367 72 const float gMax_speed = 0.8;
rizqicahyo 2:df6c49846367 73 const float gMin_speed = 0.1;
rizqicahyo 2:df6c49846367 74 /*const float tuning1 = 0.0;
rizqicahyo 2:df6c49846367 75 const float tuning2 = 0.2;
rizqicahyo 2:df6c49846367 76 const float tuning3 = 0.06;
rizqicahyo 2:df6c49846367 77 const float tuning4 = 0.24;*/
rizqicahyo 2:df6c49846367 78 const float v0 = 0.35;
rizqicahyo 2:df6c49846367 79 const float ax = 0.0006;
rizqicahyo 2:df6c49846367 80
rizqicahyo 2:df6c49846367 81 const float tuning1 = 0.0;
Najib_irvani 3:5f9fbb53c8d5 82 const float tuning2 = 0.04;
Najib_irvani 3:5f9fbb53c8d5 83 const float tuning3 = 0.0;
Najib_irvani 3:5f9fbb53c8d5 84 const float tuning4 = 0.04;
rizqicahyo 2:df6c49846367 85
rizqicahyo 2:df6c49846367 86
rizqicahyo 2:df6c49846367 87 float vcurr = v0;
rizqicahyo 0:edddd373a163 88
rizqicahyo 0:edddd373a163 89 // inisialisasi pwm awal servo
rizqicahyo 0:edddd373a163 90 double pwm = 0.00;
rizqicahyo 0:edddd373a163 91
rizqicahyo 0:edddd373a163 92 // inisialisasi sudut awal
rizqicahyo 0:edddd373a163 93 double sudut = -85;
rizqicahyo 0:edddd373a163 94
rizqicahyo 0:edddd373a163 95 // variabel kondisi pnuematik
rizqicahyo 0:edddd373a163 96 int g = 1;
rizqicahyo 0:edddd373a163 97
rizqicahyo 2:df6c49846367 98 //slider auto
rizqicahyo 2:df6c49846367 99 int c =0;
rizqicahyo 2:df6c49846367 100 int batas_delay = 270;
rizqicahyo 2:df6c49846367 101
rizqicahyo 2:df6c49846367 102 //data sensor garis dan line following
rizqicahyo 2:df6c49846367 103 int datasensor[6];
Najib_irvani 3:5f9fbb53c8d5 104 int g_flag;
rizqicahyo 2:df6c49846367 105
rizqicahyo 0:edddd373a163 106 ///////
rizqicahyo 0:edddd373a163 107 void aktuator();
rizqicahyo 0:edddd373a163 108 void edf_servo();
rizqicahyo 2:df6c49846367 109 void init_sensor();
rizqicahyo 2:df6c49846367 110 void line(float speed);
rizqicahyo 2:df6c49846367 111
Najib_irvani 3:5f9fbb53c8d5 112 void majulf(float i, float j, float k, float l);
Najib_irvani 3:5f9fbb53c8d5 113 void kirilf(float e, float f, float g, float h);
Najib_irvani 3:5f9fbb53c8d5 114 void kananlf(float a, float b, float c, float d);
Najib_irvani 3:5f9fbb53c8d5 115
rizqicahyo 0:edddd373a163 116 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 117 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 118 /** PROGRAM UTAMA **/
rizqicahyo 0:edddd373a163 119 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 120 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 121 int main() {
rizqicahyo 0:edddd373a163 122 //inisiasi serial
rizqicahyo 0:edddd373a163 123 pc.baud(115200);
rizqicahyo 0:edddd373a163 124 com.baud(115200);
rizqicahyo 0:edddd373a163 125
rizqicahyo 0:edddd373a163 126 //inisiasi joystick
rizqicahyo 0:edddd373a163 127 ps2.init();
rizqicahyo 0:edddd373a163 128
rizqicahyo 0:edddd373a163 129 //set inisiasi servo pada posisi 0
rizqicahyo 0:edddd373a163 130 servoEDF.position(sudut);
rizqicahyo 0:edddd373a163 131
rizqicahyo 0:edddd373a163 132 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
rizqicahyo 0:edddd373a163 133 edf.setThrottle(0);
rizqicahyo 0:edddd373a163 134 edf.pulse();
rizqicahyo 0:edddd373a163 135
rizqicahyo 0:edddd373a163 136 //inisiasi pnuematik
rizqicahyo 0:edddd373a163 137 pnuematik1 = 1;
rizqicahyo 0:edddd373a163 138 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 139 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 140 pnuematik4 = 1;
rizqicahyo 0:edddd373a163 141
rizqicahyo 0:edddd373a163 142 //inisiasi PID sensor
rizqicahyo 0:edddd373a163 143 PID.setInputLimits(-15,15);
rizqicahyo 0:edddd373a163 144 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 0:edddd373a163 145 PID.setMode(1.0);
rizqicahyo 0:edddd373a163 146 PID.setBias(0.0);
rizqicahyo 0:edddd373a163 147 PID.reset();
rizqicahyo 0:edddd373a163 148
rizqicahyo 0:edddd373a163 149 //inisisasi TIMER
rizqicahyo 2:df6c49846367 150 //timer.start();
rizqicahyo 0:edddd373a163 151
rizqicahyo 0:edddd373a163 152 //kondisi robot
rizqicahyo 2:df6c49846367 153 bool manual=true;
Najib_irvani 3:5f9fbb53c8d5 154
rizqicahyo 2:df6c49846367 155 while(1)
rizqicahyo 2:df6c49846367 156 {
rizqicahyo 2:df6c49846367 157 ps2.poll();
rizqicahyo 2:df6c49846367 158 init_sensor();
rizqicahyo 2:df6c49846367 159
rizqicahyo 2:df6c49846367 160 if(ps2.read(PS_PAD::PAD_X)==1){
Najib_irvani 3:5f9fbb53c8d5 161 line(0.35);
rizqicahyo 2:df6c49846367 162 }
rizqicahyo 2:df6c49846367 163 else{
rizqicahyo 2:df6c49846367 164 motor1.brake(1);
rizqicahyo 2:df6c49846367 165 motor2.brake(1);
rizqicahyo 2:df6c49846367 166 motor3.brake(1);
rizqicahyo 2:df6c49846367 167 motor4.brake(1);
rizqicahyo 2:df6c49846367 168
rizqicahyo 2:df6c49846367 169 //v =0.1;
rizqicahyo 2:df6c49846367 170 }
rizqicahyo 2:df6c49846367 171 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 172 pc.printf("%d ",datasensor[i]);
rizqicahyo 2:df6c49846367 173 }
rizqicahyo 2:df6c49846367 174 pc.printf("\n");
rizqicahyo 2:df6c49846367 175
Najib_irvani 3:5f9fbb53c8d5 176 }
rizqicahyo 0:edddd373a163 177
rizqicahyo 0:edddd373a163 178 while(manual){
rizqicahyo 0:edddd373a163 179
rizqicahyo 0:edddd373a163 180 ps2.poll();
rizqicahyo 0:edddd373a163 181
rizqicahyo 0:edddd373a163 182 aktuator();
rizqicahyo 0:edddd373a163 183 edf_servo();
rizqicahyo 0:edddd373a163 184
rizqicahyo 0:edddd373a163 185 if(limit3==0) manual = false;
rizqicahyo 0:edddd373a163 186
rizqicahyo 0:edddd373a163 187 }
Najib_irvani 3:5f9fbb53c8d5 188 /*
Najib_irvani 3:5f9fbb53c8d5 189 while(1){
Najib_irvani 3:5f9fbb53c8d5 190 kananlf(0.1,0.1,0.1,0.1);
Najib_irvani 3:5f9fbb53c8d5 191 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 192 motor1.brake(1);
Najib_irvani 3:5f9fbb53c8d5 193 motor2.brake(1);
Najib_irvani 3:5f9fbb53c8d5 194 motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 195 motor4.brake(1);
Najib_irvani 3:5f9fbb53c8d5 196 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 197 kirilf(0.1,0.1,0.1,0.1);
Najib_irvani 3:5f9fbb53c8d5 198 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 199 motor1.brake(1);
Najib_irvani 3:5f9fbb53c8d5 200 motor2.brake(1);
Najib_irvani 3:5f9fbb53c8d5 201 motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 202 motor4.brake(1);
Najib_irvani 3:5f9fbb53c8d5 203 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 204 majulf(0.1,0.1,0.1,0.1);
Najib_irvani 3:5f9fbb53c8d5 205 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 206 motor1.brake(1);
Najib_irvani 3:5f9fbb53c8d5 207 motor2.brake(1);
Najib_irvani 3:5f9fbb53c8d5 208 motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 209 motor4.brake(1);
Najib_irvani 3:5f9fbb53c8d5 210 wait_ms(3000);
Najib_irvani 3:5f9fbb53c8d5 211 }*/
Najib_irvani 3:5f9fbb53c8d5 212
Najib_irvani 3:5f9fbb53c8d5 213 //bool lf = true;
Najib_irvani 3:5f9fbb53c8d5 214 //int v_sensor;
Najib_irvani 3:5f9fbb53c8d5 215
rizqicahyo 0:edddd373a163 216
rizqicahyo 0:edddd373a163 217 motor1.brake(1);
rizqicahyo 0:edddd373a163 218 motor2.brake(1);
rizqicahyo 0:edddd373a163 219 motor3.brake(1);
rizqicahyo 0:edddd373a163 220 motor4.brake(1);
rizqicahyo 0:edddd373a163 221
rizqicahyo 0:edddd373a163 222 pnuematik1=0;
rizqicahyo 0:edddd373a163 223 wait_ms(1500);
rizqicahyo 0:edddd373a163 224
rizqicahyo 0:edddd373a163 225 while(limit4!=0){
rizqicahyo 0:edddd373a163 226 motorC1.speed(1);
rizqicahyo 0:edddd373a163 227 motorC2.speed(-1);
rizqicahyo 0:edddd373a163 228 }
rizqicahyo 0:edddd373a163 229
rizqicahyo 0:edddd373a163 230 motorC1.brake(1);
rizqicahyo 0:edddd373a163 231 motorC2.brake(1);
rizqicahyo 0:edddd373a163 232
rizqicahyo 0:edddd373a163 233 pnuematik3 = 0;
rizqicahyo 0:edddd373a163 234 wait_ms(1500);
rizqicahyo 0:edddd373a163 235 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 236 wait_ms(500);
rizqicahyo 0:edddd373a163 237 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 238
rizqicahyo 0:edddd373a163 239 return 0;
rizqicahyo 0:edddd373a163 240 }
rizqicahyo 0:edddd373a163 241
rizqicahyo 0:edddd373a163 242 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 243 /** ALGORITMA PROSEDUR DAN FUNGSI **/
rizqicahyo 0:edddd373a163 244 /*********************************************************************************************/
Najib_irvani 3:5f9fbb53c8d5 245
rizqicahyo 0:edddd373a163 246 void aktuator(){
rizqicahyo 2:df6c49846367 247 float speed = vcurr;
rizqicahyo 2:df6c49846367 248
rizqicahyo 2:df6c49846367 249 if(vcurr >= gMax_speed) vcurr = gMax_speed;
Najib_irvani 1:fc1535231c0d 250
rizqicahyo 0:edddd373a163 251 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 0:edddd373a163 252 //pivot kiri
Najib_irvani 3:5f9fbb53c8d5 253 motor2.speed((float)0.5*(speed-tuning2));
Najib_irvani 3:5f9fbb53c8d5 254 motor4.speed((float)-0.5*(speed-tuning4));
Najib_irvani 3:5f9fbb53c8d5 255 motor1.speed((float)0.5*(speed-tuning1));
Najib_irvani 3:5f9fbb53c8d5 256 motor3.speed((float)-0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 257 pc.printf("pivot kiri \n");
rizqicahyo 0:edddd373a163 258
rizqicahyo 2:df6c49846367 259 vcurr += ax;
rizqicahyo 0:edddd373a163 260 }
rizqicahyo 0:edddd373a163 261 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 0:edddd373a163 262 //pivot kanan
Najib_irvani 3:5f9fbb53c8d5 263 motor2.speed((float)-0.5*(speed-tuning2));
Najib_irvani 3:5f9fbb53c8d5 264 motor4.speed((float)0.5*(speed-tuning4));
Najib_irvani 3:5f9fbb53c8d5 265 motor1.speed((float)-0.5*(speed-tuning1));
Najib_irvani 3:5f9fbb53c8d5 266 motor3.speed((float)0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 267 pc.printf("pivot kanan \n");
rizqicahyo 0:edddd373a163 268
rizqicahyo 2:df6c49846367 269 vcurr += ax;
rizqicahyo 0:edddd373a163 270 }
rizqicahyo 0:edddd373a163 271 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 0:edddd373a163 272 //serong atas kanan
Najib_irvani 1:fc1535231c0d 273 motor2.speed(speed-tuning2);
rizqicahyo 0:edddd373a163 274 motor4.brake(1);
rizqicahyo 0:edddd373a163 275 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 276 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 277 pc.printf("serong atas kanan \n");
rizqicahyo 0:edddd373a163 278
rizqicahyo 2:df6c49846367 279 vcurr += ax;
rizqicahyo 0:edddd373a163 280 }
rizqicahyo 0:edddd373a163 281 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 282 //serong atas kiri
rizqicahyo 0:edddd373a163 283 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 284 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 285 motor1.speed(-(speed-tuning1));
rizqicahyo 0:edddd373a163 286 motor3.brake(1);
rizqicahyo 0:edddd373a163 287 pc.printf("serong atas kiri \n");
rizqicahyo 0:edddd373a163 288
rizqicahyo 2:df6c49846367 289 vcurr += ax;
rizqicahyo 0:edddd373a163 290 }
rizqicahyo 0:edddd373a163 291 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 0:edddd373a163 292 //serong bawah kiri
Najib_irvani 1:fc1535231c0d 293 motor2.speed(-(speed-tuning2));
rizqicahyo 0:edddd373a163 294 motor4.brake(1);
rizqicahyo 0:edddd373a163 295 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 296 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 297 pc.printf("serong bawah kiri \n");
rizqicahyo 0:edddd373a163 298
rizqicahyo 2:df6c49846367 299 vcurr += ax;
rizqicahyo 0:edddd373a163 300 }
rizqicahyo 0:edddd373a163 301 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 0:edddd373a163 302 //serong bawah kanan
rizqicahyo 0:edddd373a163 303 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 304 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 305 motor1.speed(speed-tuning1);
rizqicahyo 0:edddd373a163 306 motor3.brake(1);
rizqicahyo 0:edddd373a163 307 pc.printf("serong bawah kanan \n");
rizqicahyo 0:edddd373a163 308
rizqicahyo 2:df6c49846367 309 vcurr += ax;
rizqicahyo 0:edddd373a163 310 }
rizqicahyo 0:edddd373a163 311 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 0:edddd373a163 312 //maju
Najib_irvani 1:fc1535231c0d 313 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 314 motor3.speed(speed-tuning3);
Najib_irvani 1:fc1535231c0d 315 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 316 motor4.speed(-(speed-tuning4));
rizqicahyo 0:edddd373a163 317 pc.printf("maju \n");
rizqicahyo 0:edddd373a163 318
rizqicahyo 2:df6c49846367 319 vcurr += ax;
rizqicahyo 0:edddd373a163 320 }
rizqicahyo 0:edddd373a163 321 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 0:edddd373a163 322 //mundur
Najib_irvani 1:fc1535231c0d 323 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 324 motor3.speed(-(speed-tuning3));
Najib_irvani 1:fc1535231c0d 325 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 326 motor4.speed(speed-tuning4);
rizqicahyo 0:edddd373a163 327 pc.printf("mundur \n");
rizqicahyo 0:edddd373a163 328
rizqicahyo 2:df6c49846367 329 vcurr += ax;
rizqicahyo 0:edddd373a163 330 }
rizqicahyo 0:edddd373a163 331 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 0:edddd373a163 332 //kanan
Najib_irvani 1:fc1535231c0d 333 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 334 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 335 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 336 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 337 pc.printf("kanan \n");
rizqicahyo 0:edddd373a163 338
rizqicahyo 2:df6c49846367 339 vcurr += ax;
rizqicahyo 0:edddd373a163 340 }
rizqicahyo 0:edddd373a163 341 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 342 //kiri
Najib_irvani 1:fc1535231c0d 343 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 344 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 345 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 346 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 347 pc.printf("kiri \n");
rizqicahyo 0:edddd373a163 348
rizqicahyo 2:df6c49846367 349 vcurr += ax;
rizqicahyo 0:edddd373a163 350 }
rizqicahyo 0:edddd373a163 351 else{
rizqicahyo 0:edddd373a163 352 motor1.brake(1);
rizqicahyo 0:edddd373a163 353 motor3.brake(1);
rizqicahyo 0:edddd373a163 354 motor2.brake(1);
rizqicahyo 0:edddd373a163 355 motor4.brake(1);
rizqicahyo 0:edddd373a163 356 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 357
rizqicahyo 2:df6c49846367 358 vcurr = v0;
rizqicahyo 0:edddd373a163 359 }
rizqicahyo 0:edddd373a163 360
rizqicahyo 0:edddd373a163 361 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
rizqicahyo 0:edddd373a163 362 //POWER WINDOW ATAS
rizqicahyo 0:edddd373a163 363 motorS.speed(1);
rizqicahyo 2:df6c49846367 364
rizqicahyo 2:df6c49846367 365 if (limit1 == 0){
rizqicahyo 2:df6c49846367 366 motorS.brake(1);
rizqicahyo 2:df6c49846367 367 }
rizqicahyo 2:df6c49846367 368
rizqicahyo 2:df6c49846367 369
rizqicahyo 0:edddd373a163 370 pc.printf("up \n");
rizqicahyo 2:df6c49846367 371 c++;
rizqicahyo 0:edddd373a163 372 }
rizqicahyo 0:edddd373a163 373 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
rizqicahyo 0:edddd373a163 374 //POWER WINDOW BAWAH
rizqicahyo 2:df6c49846367 375 motorS.speed(-0.5);
rizqicahyo 2:df6c49846367 376
rizqicahyo 2:df6c49846367 377 if (limit2 ==0){
rizqicahyo 2:df6c49846367 378 motorS.brake(1);
rizqicahyo 2:df6c49846367 379 }
rizqicahyo 2:df6c49846367 380
rizqicahyo 0:edddd373a163 381 pc.printf("down \n");
rizqicahyo 2:df6c49846367 382 c--;
rizqicahyo 0:edddd373a163 383 }
rizqicahyo 0:edddd373a163 384 else{
rizqicahyo 0:edddd373a163 385 motorS.brake(1);
rizqicahyo 2:df6c49846367 386 if ((c <= batas_delay) && (c>=-batas_delay)){
rizqicahyo 2:df6c49846367 387 c=0;
rizqicahyo 2:df6c49846367 388 }
rizqicahyo 0:edddd373a163 389
rizqicahyo 2:df6c49846367 390 pc.printf("diam \n");
rizqicahyo 2:df6c49846367 391 }
rizqicahyo 2:df6c49846367 392 if((c > batas_delay) && (limit1 == 0)){
rizqicahyo 2:df6c49846367 393 c = 0;
rizqicahyo 2:df6c49846367 394 motorS.brake(1);
rizqicahyo 2:df6c49846367 395 }
rizqicahyo 2:df6c49846367 396 else if((c < -batas_delay) && (limit2 == 0)){
rizqicahyo 2:df6c49846367 397 c = 0;
rizqicahyo 2:df6c49846367 398 motorS.brake(1);
rizqicahyo 2:df6c49846367 399 }
rizqicahyo 2:df6c49846367 400 else if( (c > batas_delay) && (limit1 != 0)){
rizqicahyo 2:df6c49846367 401 motorS.speed(1);
rizqicahyo 2:df6c49846367 402 }
rizqicahyo 2:df6c49846367 403 else if ((c<-batas_delay) && (limit2 != 0)){
rizqicahyo 2:df6c49846367 404 motorS.speed(-0.7);
rizqicahyo 2:df6c49846367 405 }
rizqicahyo 2:df6c49846367 406
rizqicahyo 0:edddd373a163 407
rizqicahyo 0:edddd373a163 408 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 0:edddd373a163 409 {
rizqicahyo 0:edddd373a163 410 //mekanisme ambil gripper
rizqicahyo 0:edddd373a163 411 pc.printf("mekanisme gripper");
rizqicahyo 0:edddd373a163 412 if (g==1){
rizqicahyo 0:edddd373a163 413 pc.printf("ambil 1");
rizqicahyo 0:edddd373a163 414 pnuematik2 = 0;
rizqicahyo 0:edddd373a163 415 g=2;
rizqicahyo 0:edddd373a163 416 wait_ms(400);
rizqicahyo 0:edddd373a163 417 }
rizqicahyo 0:edddd373a163 418 else
rizqicahyo 0:edddd373a163 419 {
rizqicahyo 0:edddd373a163 420 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 421 wait_ms(400);
rizqicahyo 0:edddd373a163 422 g=1;
rizqicahyo 0:edddd373a163 423 }
rizqicahyo 0:edddd373a163 424 }
rizqicahyo 0:edddd373a163 425 }
rizqicahyo 0:edddd373a163 426
Najib_irvani 3:5f9fbb53c8d5 427
rizqicahyo 0:edddd373a163 428 void edf_servo(){
rizqicahyo 0:edddd373a163 429 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 0:edddd373a163 430 //PWM ++
rizqicahyo 2:df6c49846367 431 pwm += 0.0007;
rizqicahyo 2:df6c49846367 432 if( pwm > 0.7) pwm = 0.8;
rizqicahyo 0:edddd373a163 433 pc.printf("gaspol \n");
rizqicahyo 0:edddd373a163 434 }
rizqicahyo 0:edddd373a163 435 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
rizqicahyo 0:edddd373a163 436 //PWM--
rizqicahyo 2:df6c49846367 437 pwm -= 0.0007;
rizqicahyo 0:edddd373a163 438
rizqicahyo 0:edddd373a163 439 if(pwm < 0) pwm = 0.0;
rizqicahyo 0:edddd373a163 440 pc.printf("rem ndeng \n");
rizqicahyo 0:edddd373a163 441 }
rizqicahyo 0:edddd373a163 442
rizqicahyo 0:edddd373a163 443 if(ps2.read(PS_PAD::PAD_R2)==1){
rizqicahyo 0:edddd373a163 444 //SERVO --
rizqicahyo 0:edddd373a163 445 sudut += 0.5;
rizqicahyo 0:edddd373a163 446
rizqicahyo 0:edddd373a163 447 if(sudut > 90) sudut = 90;
rizqicahyo 0:edddd373a163 448 pc.printf("servo max \n");
rizqicahyo 0:edddd373a163 449 }
rizqicahyo 0:edddd373a163 450 else if(ps2.read(PS_PAD::PAD_L2)==1){
rizqicahyo 0:edddd373a163 451 //SERVO ++
rizqicahyo 0:edddd373a163 452 sudut -= 0.5;
rizqicahyo 0:edddd373a163 453
rizqicahyo 0:edddd373a163 454 if(sudut < -90) sudut = -90;
rizqicahyo 0:edddd373a163 455 pc.printf("servo min \n");
rizqicahyo 0:edddd373a163 456 }
rizqicahyo 0:edddd373a163 457
rizqicahyo 0:edddd373a163 458 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 0:edddd373a163 459
rizqicahyo 0:edddd373a163 460 sudut = 0;
rizqicahyo 0:edddd373a163 461 pwm = 0.25;
rizqicahyo 0:edddd373a163 462 }
rizqicahyo 0:edddd373a163 463
rizqicahyo 0:edddd373a163 464 servoEDF.position((float)sudut);
rizqicahyo 0:edddd373a163 465 edf.setThrottle((float)pwm);
rizqicahyo 0:edddd373a163 466 edf.pulse();
rizqicahyo 2:df6c49846367 467 }
rizqicahyo 2:df6c49846367 468
rizqicahyo 2:df6c49846367 469 void init_sensor(){
rizqicahyo 2:df6c49846367 470 char data;
rizqicahyo 2:df6c49846367 471 if(com.readable()){
rizqicahyo 2:df6c49846367 472 data = com.getc();
rizqicahyo 2:df6c49846367 473
rizqicahyo 2:df6c49846367 474 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 475 datasensor[i] = (data >> i) & 1;
rizqicahyo 2:df6c49846367 476 }
rizqicahyo 2:df6c49846367 477 }
rizqicahyo 2:df6c49846367 478 }
rizqicahyo 2:df6c49846367 479
rizqicahyo 2:df6c49846367 480 void line(float speed){
rizqicahyo 2:df6c49846367 481 int pv;
rizqicahyo 2:df6c49846367 482 float speed1,speed2,speed3,speed4;
rizqicahyo 2:df6c49846367 483
rizqicahyo 2:df6c49846367 484 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 2:df6c49846367 485 if(datasensor[0]){
rizqicahyo 2:df6c49846367 486 pv = -5;
rizqicahyo 2:df6c49846367 487 over=1;
rizqicahyo 2:df6c49846367 488 }
rizqicahyo 2:df6c49846367 489 else if(datasensor[5]){
rizqicahyo 2:df6c49846367 490 pv = 5;
rizqicahyo 2:df6c49846367 491 over=2;
rizqicahyo 2:df6c49846367 492 }
rizqicahyo 2:df6c49846367 493 else if(datasensor[0] && datasensor[1]){
Najib_irvani 3:5f9fbb53c8d5 494 pv = -3.5;
rizqicahyo 2:df6c49846367 495 }
rizqicahyo 2:df6c49846367 496 else if(datasensor[4] && datasensor[5]){
Najib_irvani 3:5f9fbb53c8d5 497 pv = 3.5;
rizqicahyo 2:df6c49846367 498 }
rizqicahyo 2:df6c49846367 499 else if(datasensor[1]){
Najib_irvani 3:5f9fbb53c8d5 500 pv = -2;
rizqicahyo 2:df6c49846367 501 }
rizqicahyo 2:df6c49846367 502 else if(datasensor[4]){
Najib_irvani 3:5f9fbb53c8d5 503 pv = 2;
rizqicahyo 2:df6c49846367 504 }
rizqicahyo 2:df6c49846367 505 else if(datasensor[1] && datasensor[2]){
Najib_irvani 3:5f9fbb53c8d5 506 pv = -1;
rizqicahyo 2:df6c49846367 507 }
rizqicahyo 2:df6c49846367 508 else if(datasensor[3] && datasensor[4]){
Najib_irvani 3:5f9fbb53c8d5 509 pv = 1;
rizqicahyo 2:df6c49846367 510 }
rizqicahyo 2:df6c49846367 511 else if(datasensor[2]){
Najib_irvani 3:5f9fbb53c8d5 512 pv = -0.5;
rizqicahyo 2:df6c49846367 513 }
rizqicahyo 2:df6c49846367 514 else if(datasensor[3]){
Najib_irvani 3:5f9fbb53c8d5 515 pv = 0.5;
rizqicahyo 2:df6c49846367 516 }
rizqicahyo 2:df6c49846367 517 else if(datasensor[2] && datasensor[3]){
rizqicahyo 2:df6c49846367 518 pv = 0;
rizqicahyo 2:df6c49846367 519 }
rizqicahyo 2:df6c49846367 520 else
rizqicahyo 2:df6c49846367 521 {
rizqicahyo 2:df6c49846367 522 ///////////////// robot bergerak keluar dari sensor/////////////////////
rizqicahyo 2:df6c49846367 523 if(over==1){
rizqicahyo 2:df6c49846367 524
Najib_irvani 3:5f9fbb53c8d5 525 //motor1.brake(1);
Najib_irvani 3:5f9fbb53c8d5 526 //motor4.brake(1);
rizqicahyo 2:df6c49846367 527 //wait_ms(100);
rizqicahyo 2:df6c49846367 528
rizqicahyo 2:df6c49846367 529 }
rizqicahyo 2:df6c49846367 530 else if(over==2){
Najib_irvani 3:5f9fbb53c8d5 531
Najib_irvani 3:5f9fbb53c8d5 532 //motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 533 //motor2.brake(1);
rizqicahyo 2:df6c49846367 534 //wait_ms(100);
rizqicahyo 2:df6c49846367 535 }
rizqicahyo 2:df6c49846367 536 }
Najib_irvani 3:5f9fbb53c8d5 537 /*
Najib_irvani 3:5f9fbb53c8d5 538 if(pv > 1){
Najib_irvani 3:5f9fbb53c8d5 539 //kanan
Najib_irvani 3:5f9fbb53c8d5 540 speed1 = -(0.4-tuning1);
Najib_irvani 3:5f9fbb53c8d5 541 speed2 = 0;
Najib_irvani 3:5f9fbb53c8d5 542 speed3 = 0;
Najib_irvani 3:5f9fbb53c8d5 543 speed4 = -(0.5-tuning4);
Najib_irvani 3:5f9fbb53c8d5 544 motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 545 motor2.brake(1);
Najib_irvani 3:5f9fbb53c8d5 546 motor1.speed(speed1);
Najib_irvani 3:5f9fbb53c8d5 547 motor4.speed(speed4);
Najib_irvani 3:5f9fbb53c8d5 548 }
Najib_irvani 3:5f9fbb53c8d5 549 else if(pv < -1){
Najib_irvani 3:5f9fbb53c8d5 550 //kiri
Najib_irvani 3:5f9fbb53c8d5 551 speed1 = 0;
Najib_irvani 3:5f9fbb53c8d5 552 speed2 = -(0.4-tuning2);
Najib_irvani 3:5f9fbb53c8d5 553 speed3 = -(0.4-tuning3);
Najib_irvani 3:5f9fbb53c8d5 554 speed4 = 0;
Najib_irvani 3:5f9fbb53c8d5 555
Najib_irvani 3:5f9fbb53c8d5 556 motor3.brake(1);
Najib_irvani 3:5f9fbb53c8d5 557 motor2.speed(speed2);
Najib_irvani 3:5f9fbb53c8d5 558 motor1.speed(speed1);
Najib_irvani 3:5f9fbb53c8d5 559 motor4.brake(1);
Najib_irvani 3:5f9fbb53c8d5 560 }
Najib_irvani 3:5f9fbb53c8d5 561 else{
Najib_irvani 3:5f9fbb53c8d5 562 //gaspoll
Najib_irvani 3:5f9fbb53c8d5 563 speed1 = -(0.7-tuning1);
Najib_irvani 3:5f9fbb53c8d5 564 speed2 = -(0.7-tuning2);
Najib_irvani 3:5f9fbb53c8d5 565 speed3 = -(0.7-tuning3);
Najib_irvani 3:5f9fbb53c8d5 566 speed4 = -(0.7-tuning4);
Najib_irvani 3:5f9fbb53c8d5 567
Najib_irvani 3:5f9fbb53c8d5 568 motor3.speed(speed3);
Najib_irvani 3:5f9fbb53c8d5 569 motor2.speed(speed2);
Najib_irvani 3:5f9fbb53c8d5 570 motor1.speed(speed1);
Najib_irvani 3:5f9fbb53c8d5 571 motor4.speed(speed4);
Najib_irvani 3:5f9fbb53c8d5 572 } */
rizqicahyo 2:df6c49846367 573 PID.setProcessValue(pv);
rizqicahyo 2:df6c49846367 574 PID.setSetPoint(0);
rizqicahyo 2:df6c49846367 575
rizqicahyo 2:df6c49846367 576 // memulai perhitungan PID
Najib_irvani 3:5f9fbb53c8d5 577 //speed1 = speed - tuning1 + PID.compute();
Najib_irvani 3:5f9fbb53c8d5 578 //speed2 = speed - tuning2 - PID.compute();
Najib_irvani 3:5f9fbb53c8d5 579 //speed3 = speed - tuning3 - PID.compute();
Najib_irvani 3:5f9fbb53c8d5 580 //speed4 = speed - tuning4 + PID.compute();
rizqicahyo 2:df6c49846367 581
Najib_irvani 3:5f9fbb53c8d5 582 speed1 = -0.74;
Najib_irvani 3:5f9fbb53c8d5 583 speed2 = -0.74;
Najib_irvani 3:5f9fbb53c8d5 584 speed3 = -0.8;
Najib_irvani 3:5f9fbb53c8d5 585 speed4 = -0.8;
Najib_irvani 3:5f9fbb53c8d5 586
Najib_irvani 3:5f9fbb53c8d5 587 /*if(speed1 > speed) speed1 = speed;
rizqicahyo 2:df6c49846367 588 else if(speed1 < gMin_speed) speed1 = gMin_speed;
rizqicahyo 2:df6c49846367 589
rizqicahyo 2:df6c49846367 590 if(speed2 > speed) speed2 = speed;
rizqicahyo 2:df6c49846367 591 else if(speed2 < gMin_speed) speed2 = gMin_speed;
rizqicahyo 2:df6c49846367 592
rizqicahyo 2:df6c49846367 593 if(speed3 > speed) speed3 = speed;
rizqicahyo 2:df6c49846367 594 else if(speed3 < gMin_speed) speed3 = gMin_speed;
rizqicahyo 2:df6c49846367 595
rizqicahyo 2:df6c49846367 596 if(speed4 > speed) speed4 = speed;
rizqicahyo 2:df6c49846367 597 else if(speed4 < gMin_speed) speed4 = gMin_speed;
Najib_irvani 3:5f9fbb53c8d5 598 */
rizqicahyo 2:df6c49846367 599
Najib_irvani 3:5f9fbb53c8d5 600 //motor3.speed(-speed3);
Najib_irvani 3:5f9fbb53c8d5 601 //motor2.speed(-speed2);
Najib_irvani 3:5f9fbb53c8d5 602 //motor1.speed(-speed1);
Najib_irvani 3:5f9fbb53c8d5 603 //motor4.speed(-speed4);
Najib_irvani 3:5f9fbb53c8d5 604
Najib_irvani 3:5f9fbb53c8d5 605 motor3.speed(speed3);
Najib_irvani 3:5f9fbb53c8d5 606 motor2.speed(speed2);
Najib_irvani 3:5f9fbb53c8d5 607 motor1.speed(speed1);
Najib_irvani 3:5f9fbb53c8d5 608 motor4.speed(speed4);
Najib_irvani 3:5f9fbb53c8d5 609 }