KRAI 2016 / Mbed 2 deprecated Ultimate_Hybrid_LF

base ultimate + line following HYBRID

Dependencies:   ESC Motor PS_PAD hadah mbed Ping

Fork of Ultimate_Hybrid by KRAI 2016

main.cpp@2:df6c49846367, 2016-04-22 (annotated)

Committer:
rizqicahyo
Date:
Fri Apr 22 08:39:32 2016 +0000
Revision:
2:df6c49846367
Parent:
1:fc1535231c0d
Child:
3:43d4cb3ece1b
update tuning

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);
rizqicahyo 2:df6c49846367 24 //Motor gripper(PB_6, PA_7, PB_12);
rizqicahyo 0:edddd373a163 25 Motor motor3(PA_9, PC_2, PC_3);
rizqicahyo 0:edddd373a163 26 Motor motor4(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);
rizqicahyo 0:edddd373a163 29 Motor motorS(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)
rizqicahyo 2:df6c49846367 56 PID PID(0.92,0.000,0.61,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;
rizqicahyo 2:df6c49846367 82 const float tuning2 = 0.16;
rizqicahyo 2:df6c49846367 83 const float tuning3 = 0.03;
rizqicahyo 2:df6c49846367 84 const float tuning4 = 0.22;
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];
rizqicahyo 2:df6c49846367 104 int over;
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
rizqicahyo 0:edddd373a163 112 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 113 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 114 /** PROGRAM UTAMA **/
rizqicahyo 0:edddd373a163 115 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 116 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 117 int main() {
rizqicahyo 0:edddd373a163 118 //inisiasi serial
rizqicahyo 0:edddd373a163 119 pc.baud(115200);
rizqicahyo 0:edddd373a163 120 com.baud(115200);
rizqicahyo 0:edddd373a163 121
rizqicahyo 0:edddd373a163 122 //inisiasi joystick
rizqicahyo 0:edddd373a163 123 ps2.init();
rizqicahyo 0:edddd373a163 124
rizqicahyo 0:edddd373a163 125 //set inisiasi servo pada posisi 0
rizqicahyo 0:edddd373a163 126 servoEDF.position(sudut);
rizqicahyo 0:edddd373a163 127
rizqicahyo 0:edddd373a163 128 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
rizqicahyo 0:edddd373a163 129 edf.setThrottle(0);
rizqicahyo 0:edddd373a163 130 edf.pulse();
rizqicahyo 0:edddd373a163 131
rizqicahyo 0:edddd373a163 132 //inisiasi pnuematik
rizqicahyo 0:edddd373a163 133 pnuematik1 = 1;
rizqicahyo 0:edddd373a163 134 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 135 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 136 pnuematik4 = 1;
rizqicahyo 0:edddd373a163 137
rizqicahyo 0:edddd373a163 138 //inisiasi PID sensor
rizqicahyo 0:edddd373a163 139 PID.setInputLimits(-15,15);
rizqicahyo 0:edddd373a163 140 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 0:edddd373a163 141 PID.setMode(1.0);
rizqicahyo 0:edddd373a163 142 PID.setBias(0.0);
rizqicahyo 0:edddd373a163 143 PID.reset();
rizqicahyo 0:edddd373a163 144
rizqicahyo 0:edddd373a163 145 //inisisasi TIMER
rizqicahyo 2:df6c49846367 146 //timer.start();
rizqicahyo 0:edddd373a163 147
rizqicahyo 0:edddd373a163 148 //kondisi robot
rizqicahyo 2:df6c49846367 149 bool manual=true;
rizqicahyo 2:df6c49846367 150 /*
rizqicahyo 2:df6c49846367 151 while(1)
rizqicahyo 2:df6c49846367 152 {
rizqicahyo 2:df6c49846367 153 ps2.poll();
rizqicahyo 2:df6c49846367 154 init_sensor();
rizqicahyo 2:df6c49846367 155
rizqicahyo 2:df6c49846367 156 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 2:df6c49846367 157 line(0.3);
rizqicahyo 2:df6c49846367 158 }
rizqicahyo 2:df6c49846367 159 else{
rizqicahyo 2:df6c49846367 160 motor1.brake(1);
rizqicahyo 2:df6c49846367 161 motor2.brake(1);
rizqicahyo 2:df6c49846367 162 motor3.brake(1);
rizqicahyo 2:df6c49846367 163 motor4.brake(1);
rizqicahyo 2:df6c49846367 164
rizqicahyo 2:df6c49846367 165 //v =0.1;
rizqicahyo 2:df6c49846367 166 }
rizqicahyo 2:df6c49846367 167 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 168 pc.printf("%d ",datasensor[i]);
rizqicahyo 2:df6c49846367 169 }
rizqicahyo 2:df6c49846367 170 pc.printf("\n");
rizqicahyo 2:df6c49846367 171
rizqicahyo 2:df6c49846367 172 }*/
rizqicahyo 0:edddd373a163 173
rizqicahyo 0:edddd373a163 174 while(manual){
rizqicahyo 0:edddd373a163 175
rizqicahyo 0:edddd373a163 176 ps2.poll();
rizqicahyo 0:edddd373a163 177
rizqicahyo 0:edddd373a163 178 aktuator();
rizqicahyo 0:edddd373a163 179 edf_servo();
rizqicahyo 0:edddd373a163 180
rizqicahyo 0:edddd373a163 181 if(limit3==0) manual = false;
rizqicahyo 0:edddd373a163 182
rizqicahyo 0:edddd373a163 183 }
rizqicahyo 0:edddd373a163 184
rizqicahyo 0:edddd373a163 185 motor1.brake(1);
rizqicahyo 0:edddd373a163 186 motor2.brake(1);
rizqicahyo 0:edddd373a163 187 motor3.brake(1);
rizqicahyo 0:edddd373a163 188 motor4.brake(1);
rizqicahyo 0:edddd373a163 189
rizqicahyo 0:edddd373a163 190 pnuematik1=0;
rizqicahyo 0:edddd373a163 191 wait_ms(1500);
rizqicahyo 0:edddd373a163 192
rizqicahyo 0:edddd373a163 193 while(limit4!=0){
rizqicahyo 0:edddd373a163 194 motorC1.speed(1);
rizqicahyo 0:edddd373a163 195 motorC2.speed(-1);
rizqicahyo 0:edddd373a163 196 }
rizqicahyo 0:edddd373a163 197
rizqicahyo 0:edddd373a163 198 motorC1.brake(1);
rizqicahyo 0:edddd373a163 199 motorC2.brake(1);
rizqicahyo 0:edddd373a163 200
rizqicahyo 0:edddd373a163 201 pnuematik3 = 0;
rizqicahyo 0:edddd373a163 202 wait_ms(1500);
rizqicahyo 0:edddd373a163 203 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 204 wait_ms(500);
rizqicahyo 0:edddd373a163 205 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 206
rizqicahyo 0:edddd373a163 207 return 0;
rizqicahyo 0:edddd373a163 208 }
rizqicahyo 0:edddd373a163 209
rizqicahyo 0:edddd373a163 210 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 211 /** ALGORITMA PROSEDUR DAN FUNGSI **/
rizqicahyo 0:edddd373a163 212 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 213 void aktuator(){
rizqicahyo 2:df6c49846367 214 float speed = vcurr;
rizqicahyo 2:df6c49846367 215
rizqicahyo 2:df6c49846367 216 if(vcurr >= gMax_speed) vcurr = gMax_speed;
Najib_irvani 1:fc1535231c0d 217
rizqicahyo 0:edddd373a163 218 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 0:edddd373a163 219 //pivot kiri
rizqicahyo 2:df6c49846367 220 motor2.speed((float)0.2*(speed-tuning2));
rizqicahyo 2:df6c49846367 221 motor4.speed((float)-0.2*(speed-tuning4));
rizqicahyo 2:df6c49846367 222 motor1.speed((float)0.2*(speed-tuning1));
rizqicahyo 2:df6c49846367 223 motor3.speed((float)-0.2*(speed-tuning3));
rizqicahyo 0:edddd373a163 224 pc.printf("pivot kiri \n");
rizqicahyo 0:edddd373a163 225
rizqicahyo 2:df6c49846367 226 vcurr += ax;
rizqicahyo 0:edddd373a163 227 }
rizqicahyo 0:edddd373a163 228 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 0:edddd373a163 229 //pivot kanan
rizqicahyo 2:df6c49846367 230 motor2.speed((float)-0.2*(speed-tuning2));
rizqicahyo 2:df6c49846367 231 motor4.speed((float)0.2*(speed-tuning4));
rizqicahyo 2:df6c49846367 232 motor1.speed((float)-0.2*(speed-tuning1));
rizqicahyo 2:df6c49846367 233 motor3.speed((float)0.2*(speed-tuning3));
rizqicahyo 0:edddd373a163 234 pc.printf("pivot kanan \n");
rizqicahyo 0:edddd373a163 235
rizqicahyo 2:df6c49846367 236 vcurr += ax;
rizqicahyo 0:edddd373a163 237 }
rizqicahyo 0:edddd373a163 238 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 0:edddd373a163 239 //serong atas kanan
Najib_irvani 1:fc1535231c0d 240 motor2.speed(speed-tuning2);
rizqicahyo 0:edddd373a163 241 motor4.brake(1);
rizqicahyo 0:edddd373a163 242 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 243 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 244 pc.printf("serong atas kanan \n");
rizqicahyo 0:edddd373a163 245
rizqicahyo 2:df6c49846367 246 vcurr += ax;
rizqicahyo 0:edddd373a163 247 }
rizqicahyo 0:edddd373a163 248 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 249 //serong atas kiri
rizqicahyo 0:edddd373a163 250 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 251 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 252 motor1.speed(-(speed-tuning1));
rizqicahyo 0:edddd373a163 253 motor3.brake(1);
rizqicahyo 0:edddd373a163 254 pc.printf("serong atas kiri \n");
rizqicahyo 0:edddd373a163 255
rizqicahyo 2:df6c49846367 256 vcurr += ax;
rizqicahyo 0:edddd373a163 257 }
rizqicahyo 0:edddd373a163 258 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 0:edddd373a163 259 //serong bawah kiri
Najib_irvani 1:fc1535231c0d 260 motor2.speed(-(speed-tuning2));
rizqicahyo 0:edddd373a163 261 motor4.brake(1);
rizqicahyo 0:edddd373a163 262 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 263 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 264 pc.printf("serong bawah kiri \n");
rizqicahyo 0:edddd373a163 265
rizqicahyo 2:df6c49846367 266 vcurr += ax;
rizqicahyo 0:edddd373a163 267 }
rizqicahyo 0:edddd373a163 268 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 0:edddd373a163 269 //serong bawah kanan
rizqicahyo 0:edddd373a163 270 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 271 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 272 motor1.speed(speed-tuning1);
rizqicahyo 0:edddd373a163 273 motor3.brake(1);
rizqicahyo 0:edddd373a163 274 pc.printf("serong bawah kanan \n");
rizqicahyo 0:edddd373a163 275
rizqicahyo 2:df6c49846367 276 vcurr += ax;
rizqicahyo 0:edddd373a163 277 }
rizqicahyo 0:edddd373a163 278 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 0:edddd373a163 279 //maju
Najib_irvani 1:fc1535231c0d 280 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 281 motor3.speed(speed-tuning3);
Najib_irvani 1:fc1535231c0d 282 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 283 motor4.speed(-(speed-tuning4));
rizqicahyo 0:edddd373a163 284 pc.printf("maju \n");
rizqicahyo 0:edddd373a163 285
rizqicahyo 2:df6c49846367 286 vcurr += ax;
rizqicahyo 0:edddd373a163 287 }
rizqicahyo 0:edddd373a163 288 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 0:edddd373a163 289 //mundur
Najib_irvani 1:fc1535231c0d 290 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 291 motor3.speed(-(speed-tuning3));
Najib_irvani 1:fc1535231c0d 292 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 293 motor4.speed(speed-tuning4);
rizqicahyo 0:edddd373a163 294 pc.printf("mundur \n");
rizqicahyo 0:edddd373a163 295
rizqicahyo 2:df6c49846367 296 vcurr += ax;
rizqicahyo 0:edddd373a163 297 }
rizqicahyo 0:edddd373a163 298 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 0:edddd373a163 299 //kanan
Najib_irvani 1:fc1535231c0d 300 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 301 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 302 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 303 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 304 pc.printf("kanan \n");
rizqicahyo 0:edddd373a163 305
rizqicahyo 2:df6c49846367 306 vcurr += ax;
rizqicahyo 0:edddd373a163 307 }
rizqicahyo 0:edddd373a163 308 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 309 //kiri
Najib_irvani 1:fc1535231c0d 310 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 311 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 312 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 313 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 314 pc.printf("kiri \n");
rizqicahyo 0:edddd373a163 315
rizqicahyo 2:df6c49846367 316 vcurr += ax;
rizqicahyo 0:edddd373a163 317 }
rizqicahyo 0:edddd373a163 318 else{
rizqicahyo 0:edddd373a163 319 motor1.brake(1);
rizqicahyo 0:edddd373a163 320 motor3.brake(1);
rizqicahyo 0:edddd373a163 321 motor2.brake(1);
rizqicahyo 0:edddd373a163 322 motor4.brake(1);
rizqicahyo 0:edddd373a163 323 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 324
rizqicahyo 2:df6c49846367 325 vcurr = v0;
rizqicahyo 0:edddd373a163 326 }
rizqicahyo 0:edddd373a163 327
rizqicahyo 0:edddd373a163 328 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
rizqicahyo 0:edddd373a163 329 //POWER WINDOW ATAS
rizqicahyo 0:edddd373a163 330 motorS.speed(1);
rizqicahyo 2:df6c49846367 331
rizqicahyo 2:df6c49846367 332 if (limit1 == 0){
rizqicahyo 2:df6c49846367 333 motorS.brake(1);
rizqicahyo 2:df6c49846367 334 }
rizqicahyo 2:df6c49846367 335
rizqicahyo 2:df6c49846367 336
rizqicahyo 0:edddd373a163 337 pc.printf("up \n");
rizqicahyo 2:df6c49846367 338 c++;
rizqicahyo 0:edddd373a163 339 }
rizqicahyo 0:edddd373a163 340 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
rizqicahyo 0:edddd373a163 341 //POWER WINDOW BAWAH
rizqicahyo 2:df6c49846367 342 motorS.speed(-0.5);
rizqicahyo 2:df6c49846367 343
rizqicahyo 2:df6c49846367 344 if (limit2 ==0){
rizqicahyo 2:df6c49846367 345 motorS.brake(1);
rizqicahyo 2:df6c49846367 346 }
rizqicahyo 2:df6c49846367 347
rizqicahyo 0:edddd373a163 348 pc.printf("down \n");
rizqicahyo 2:df6c49846367 349 c--;
rizqicahyo 0:edddd373a163 350 }
rizqicahyo 0:edddd373a163 351 else{
rizqicahyo 0:edddd373a163 352 motorS.brake(1);
rizqicahyo 2:df6c49846367 353 if ((c <= batas_delay) && (c>=-batas_delay)){
rizqicahyo 2:df6c49846367 354 c=0;
rizqicahyo 2:df6c49846367 355 }
rizqicahyo 0:edddd373a163 356
rizqicahyo 2:df6c49846367 357 pc.printf("diam \n");
rizqicahyo 2:df6c49846367 358 }
rizqicahyo 2:df6c49846367 359 if((c > batas_delay) && (limit1 == 0)){
rizqicahyo 2:df6c49846367 360 c = 0;
rizqicahyo 2:df6c49846367 361 motorS.brake(1);
rizqicahyo 2:df6c49846367 362 }
rizqicahyo 2:df6c49846367 363 else if((c < -batas_delay) && (limit2 == 0)){
rizqicahyo 2:df6c49846367 364 c = 0;
rizqicahyo 2:df6c49846367 365 motorS.brake(1);
rizqicahyo 2:df6c49846367 366 }
rizqicahyo 2:df6c49846367 367 else if( (c > batas_delay) && (limit1 != 0)){
rizqicahyo 2:df6c49846367 368 motorS.speed(1);
rizqicahyo 2:df6c49846367 369 }
rizqicahyo 2:df6c49846367 370 else if ((c<-batas_delay) && (limit2 != 0)){
rizqicahyo 2:df6c49846367 371 motorS.speed(-0.7);
rizqicahyo 2:df6c49846367 372 }
rizqicahyo 2:df6c49846367 373
rizqicahyo 0:edddd373a163 374
rizqicahyo 0:edddd373a163 375 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 0:edddd373a163 376 {
rizqicahyo 0:edddd373a163 377 //mekanisme ambil gripper
rizqicahyo 0:edddd373a163 378 pc.printf("mekanisme gripper");
rizqicahyo 0:edddd373a163 379 if (g==1){
rizqicahyo 0:edddd373a163 380 pc.printf("ambil 1");
rizqicahyo 0:edddd373a163 381 pnuematik2 = 0;
rizqicahyo 0:edddd373a163 382 g=2;
rizqicahyo 0:edddd373a163 383 wait_ms(400);
rizqicahyo 0:edddd373a163 384 }
rizqicahyo 0:edddd373a163 385 else
rizqicahyo 0:edddd373a163 386 {
rizqicahyo 0:edddd373a163 387 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 388 wait_ms(400);
rizqicahyo 0:edddd373a163 389 g=1;
rizqicahyo 0:edddd373a163 390 }
rizqicahyo 0:edddd373a163 391 }
rizqicahyo 0:edddd373a163 392 }
rizqicahyo 0:edddd373a163 393
rizqicahyo 0:edddd373a163 394 void edf_servo(){
rizqicahyo 0:edddd373a163 395 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 0:edddd373a163 396 //PWM ++
rizqicahyo 2:df6c49846367 397 pwm += 0.0007;
rizqicahyo 2:df6c49846367 398 if( pwm > 0.7) pwm = 0.8;
rizqicahyo 0:edddd373a163 399 pc.printf("gaspol \n");
rizqicahyo 0:edddd373a163 400 }
rizqicahyo 0:edddd373a163 401 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
rizqicahyo 0:edddd373a163 402 //PWM--
rizqicahyo 2:df6c49846367 403 pwm -= 0.0007;
rizqicahyo 0:edddd373a163 404
rizqicahyo 0:edddd373a163 405 if(pwm < 0) pwm = 0.0;
rizqicahyo 0:edddd373a163 406 pc.printf("rem ndeng \n");
rizqicahyo 0:edddd373a163 407 }
rizqicahyo 0:edddd373a163 408
rizqicahyo 0:edddd373a163 409 if(ps2.read(PS_PAD::PAD_R2)==1){
rizqicahyo 0:edddd373a163 410 //SERVO --
rizqicahyo 0:edddd373a163 411 sudut += 0.5;
rizqicahyo 0:edddd373a163 412
rizqicahyo 0:edddd373a163 413 if(sudut > 90) sudut = 90;
rizqicahyo 0:edddd373a163 414 pc.printf("servo max \n");
rizqicahyo 0:edddd373a163 415 }
rizqicahyo 0:edddd373a163 416 else if(ps2.read(PS_PAD::PAD_L2)==1){
rizqicahyo 0:edddd373a163 417 //SERVO ++
rizqicahyo 0:edddd373a163 418 sudut -= 0.5;
rizqicahyo 0:edddd373a163 419
rizqicahyo 0:edddd373a163 420 if(sudut < -90) sudut = -90;
rizqicahyo 0:edddd373a163 421 pc.printf("servo min \n");
rizqicahyo 0:edddd373a163 422 }
rizqicahyo 0:edddd373a163 423
rizqicahyo 0:edddd373a163 424 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 0:edddd373a163 425
rizqicahyo 0:edddd373a163 426 sudut = 0;
rizqicahyo 0:edddd373a163 427 pwm = 0.25;
rizqicahyo 0:edddd373a163 428 }
rizqicahyo 0:edddd373a163 429
rizqicahyo 0:edddd373a163 430 servoEDF.position((float)sudut);
rizqicahyo 0:edddd373a163 431 edf.setThrottle((float)pwm);
rizqicahyo 0:edddd373a163 432 edf.pulse();
rizqicahyo 2:df6c49846367 433 }
rizqicahyo 2:df6c49846367 434
rizqicahyo 2:df6c49846367 435 void init_sensor(){
rizqicahyo 2:df6c49846367 436 char data;
rizqicahyo 2:df6c49846367 437 if(com.readable()){
rizqicahyo 2:df6c49846367 438 data = com.getc();
rizqicahyo 2:df6c49846367 439
rizqicahyo 2:df6c49846367 440 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 441 datasensor[i] = (data >> i) & 1;
rizqicahyo 2:df6c49846367 442 }
rizqicahyo 2:df6c49846367 443 }
rizqicahyo 2:df6c49846367 444 }
rizqicahyo 2:df6c49846367 445
rizqicahyo 2:df6c49846367 446 void line(float speed){
rizqicahyo 2:df6c49846367 447 int pv;
rizqicahyo 2:df6c49846367 448 float speed1,speed2,speed3,speed4;
rizqicahyo 2:df6c49846367 449
rizqicahyo 2:df6c49846367 450 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 2:df6c49846367 451 if(datasensor[0]){
rizqicahyo 2:df6c49846367 452 pv = -5;
rizqicahyo 2:df6c49846367 453 over=1;
rizqicahyo 2:df6c49846367 454 }
rizqicahyo 2:df6c49846367 455 else if(datasensor[5]){
rizqicahyo 2:df6c49846367 456 pv = 5;
rizqicahyo 2:df6c49846367 457 over=2;
rizqicahyo 2:df6c49846367 458 }
rizqicahyo 2:df6c49846367 459 else if(datasensor[0] && datasensor[1]){
rizqicahyo 2:df6c49846367 460 pv = -4;
rizqicahyo 2:df6c49846367 461 }
rizqicahyo 2:df6c49846367 462 else if(datasensor[4] && datasensor[5]){
rizqicahyo 2:df6c49846367 463 pv = 4;
rizqicahyo 2:df6c49846367 464 }
rizqicahyo 2:df6c49846367 465 else if(datasensor[1]){
rizqicahyo 2:df6c49846367 466 pv = -3;
rizqicahyo 2:df6c49846367 467 }
rizqicahyo 2:df6c49846367 468 else if(datasensor[4]){
rizqicahyo 2:df6c49846367 469 pv = 3;
rizqicahyo 2:df6c49846367 470 }
rizqicahyo 2:df6c49846367 471 else if(datasensor[1] && datasensor[2]){
rizqicahyo 2:df6c49846367 472 pv = -2;
rizqicahyo 2:df6c49846367 473 }
rizqicahyo 2:df6c49846367 474 else if(datasensor[3] && datasensor[4]){
rizqicahyo 2:df6c49846367 475 pv = 2;
rizqicahyo 2:df6c49846367 476 }
rizqicahyo 2:df6c49846367 477 else if(datasensor[2]){
rizqicahyo 2:df6c49846367 478 pv = -1;
rizqicahyo 2:df6c49846367 479 }
rizqicahyo 2:df6c49846367 480 else if(datasensor[3]){
rizqicahyo 2:df6c49846367 481 pv = 1;
rizqicahyo 2:df6c49846367 482 }
rizqicahyo 2:df6c49846367 483 else if(datasensor[2] && datasensor[3]){
rizqicahyo 2:df6c49846367 484 pv = 0;
rizqicahyo 2:df6c49846367 485 }
rizqicahyo 2:df6c49846367 486 else
rizqicahyo 2:df6c49846367 487 {
rizqicahyo 2:df6c49846367 488 ///////////////// robot bergerak keluar dari sensor/////////////////////
rizqicahyo 2:df6c49846367 489 if(over==1){
rizqicahyo 2:df6c49846367 490 /*if(speed_ka > 0){
rizqicahyo 2:df6c49846367 491 wait_ms(10);
rizqicahyo 2:df6c49846367 492 motor2.speed(-speed_ka);
rizqicahyo 2:df6c49846367 493 wait_ms(10);
rizqicahyo 2:df6c49846367 494 }
rizqicahyo 2:df6c49846367 495 else{
rizqicahyo 2:df6c49846367 496 motor2.speed(speed_ka);
rizqicahyo 2:df6c49846367 497 }
rizqicahyo 2:df6c49846367 498 wait_ms(10);*/
rizqicahyo 2:df6c49846367 499
rizqicahyo 2:df6c49846367 500 motor1.brake(1);
rizqicahyo 2:df6c49846367 501 motor4.brake(1);
rizqicahyo 2:df6c49846367 502 //wait_ms(100);
rizqicahyo 2:df6c49846367 503
rizqicahyo 2:df6c49846367 504 }
rizqicahyo 2:df6c49846367 505 else if(over==2){
rizqicahyo 2:df6c49846367 506 /*if(speed_ki > 0){
rizqicahyo 2:df6c49846367 507 wait_ms(10);
rizqicahyo 2:df6c49846367 508 motor1.speed(-speed_ki);
rizqicahyo 2:df6c49846367 509 wait_ms(10);
rizqicahyo 2:df6c49846367 510 }
rizqicahyo 2:df6c49846367 511 else{
rizqicahyo 2:df6c49846367 512 wait_ms(10);
rizqicahyo 2:df6c49846367 513 motor1.speed(speed_ki);
rizqicahyo 2:df6c49846367 514 wait_ms(10);
rizqicahyo 2:df6c49846367 515 }
rizqicahyo 2:df6c49846367 516 wait_ms(10);*/
rizqicahyo 2:df6c49846367 517 motor3.brake(1);
rizqicahyo 2:df6c49846367 518 motor2.brake(1);
rizqicahyo 2:df6c49846367 519 //wait_ms(100);
rizqicahyo 2:df6c49846367 520 }
rizqicahyo 2:df6c49846367 521 }
rizqicahyo 2:df6c49846367 522 PID.setProcessValue(pv);
rizqicahyo 2:df6c49846367 523 PID.setSetPoint(0);
rizqicahyo 2:df6c49846367 524
rizqicahyo 2:df6c49846367 525 // memulai perhitungan PID
rizqicahyo 2:df6c49846367 526
rizqicahyo 2:df6c49846367 527
rizqicahyo 2:df6c49846367 528 speed2 = speed - 0.0 + PID.compute();
rizqicahyo 2:df6c49846367 529 speed1 = speed - 0.0 + PID.compute();
rizqicahyo 2:df6c49846367 530 speed3 = speed - 0.2 - PID.compute();
rizqicahyo 2:df6c49846367 531 speed4 = speed - 0.3 - PID.compute();
rizqicahyo 2:df6c49846367 532
rizqicahyo 2:df6c49846367 533 if(speed1 > speed) speed1 = speed;
rizqicahyo 2:df6c49846367 534 else if(speed1 < gMin_speed) speed1 = gMin_speed;
rizqicahyo 2:df6c49846367 535
rizqicahyo 2:df6c49846367 536 if(speed2 > speed) speed2 = speed;
rizqicahyo 2:df6c49846367 537 else if(speed2 < gMin_speed) speed2 = gMin_speed;
rizqicahyo 2:df6c49846367 538
rizqicahyo 2:df6c49846367 539 if(speed3 > speed) speed3 = speed;
rizqicahyo 2:df6c49846367 540 else if(speed3 < gMin_speed) speed3 = gMin_speed;
rizqicahyo 2:df6c49846367 541
rizqicahyo 2:df6c49846367 542 if(speed4 > speed) speed4 = speed;
rizqicahyo 2:df6c49846367 543 else if(speed4 < gMin_speed) speed4 = gMin_speed;
rizqicahyo 2:df6c49846367 544
rizqicahyo 2:df6c49846367 545 motor3.speed(-(speed3));
rizqicahyo 2:df6c49846367 546 motor2.speed(-(speed2));
rizqicahyo 2:df6c49846367 547
rizqicahyo 2:df6c49846367 548 motor1.speed(-(speed1));
rizqicahyo 2:df6c49846367 549 motor4.speed(-(speed4));
rizqicahyo 0:edddd373a163 550 }