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@1:fc1535231c0d, 2016-04-19 (annotated)

Committer:
Najib_irvani
Date:
Tue Apr 19 13:32:26 2016 +0000
Revision:
1:fc1535231c0d
Parent:
0:edddd373a163
Child:
2:df6c49846367
update tunning

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 0:edddd373a163 24 //Motor motor3(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 0:edddd373a163 56 PID PID(0.992,0.000,0.81,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 0:edddd373a163 65 Timer timer;
rizqicahyo 0:edddd373a163 66
rizqicahyo 0:edddd373a163 67 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 68 /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
rizqicahyo 0:edddd373a163 69 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 70 float gMax_speed = 1;
rizqicahyo 0:edddd373a163 71 float v0 = 0.6;
Najib_irvani 1:fc1535231c0d 72 float ax = 0.0005;
rizqicahyo 0:edddd373a163 73
rizqicahyo 0:edddd373a163 74 // inisialisasi pwm awal servo
rizqicahyo 0:edddd373a163 75 double pwm = 0.00;
rizqicahyo 0:edddd373a163 76
rizqicahyo 0:edddd373a163 77 // inisialisasi sudut awal
rizqicahyo 0:edddd373a163 78 double sudut = -85;
rizqicahyo 0:edddd373a163 79
rizqicahyo 0:edddd373a163 80 // variabel kondisi pnuematik
rizqicahyo 0:edddd373a163 81 int g = 1;
rizqicahyo 0:edddd373a163 82
rizqicahyo 0:edddd373a163 83 ///////
rizqicahyo 0:edddd373a163 84 void aktuator();
rizqicahyo 0:edddd373a163 85 void edf_servo();
rizqicahyo 0:edddd373a163 86 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 87 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 88 /** PROGRAM UTAMA **/
rizqicahyo 0:edddd373a163 89 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 90 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 91 int main() {
rizqicahyo 0:edddd373a163 92 //inisiasi serial
rizqicahyo 0:edddd373a163 93 pc.baud(115200);
rizqicahyo 0:edddd373a163 94 com.baud(115200);
rizqicahyo 0:edddd373a163 95
rizqicahyo 0:edddd373a163 96 //inisiasi joystick
rizqicahyo 0:edddd373a163 97 ps2.init();
rizqicahyo 0:edddd373a163 98
rizqicahyo 0:edddd373a163 99 //set inisiasi servo pada posisi 0
rizqicahyo 0:edddd373a163 100 servoEDF.position(sudut);
rizqicahyo 0:edddd373a163 101
rizqicahyo 0:edddd373a163 102 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
rizqicahyo 0:edddd373a163 103 edf.setThrottle(0);
rizqicahyo 0:edddd373a163 104 edf.pulse();
rizqicahyo 0:edddd373a163 105
rizqicahyo 0:edddd373a163 106 //inisiasi pnuematik
rizqicahyo 0:edddd373a163 107 pnuematik1 = 1;
rizqicahyo 0:edddd373a163 108 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 109 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 110 pnuematik4 = 1;
rizqicahyo 0:edddd373a163 111
rizqicahyo 0:edddd373a163 112 //inisiasi PID sensor
rizqicahyo 0:edddd373a163 113 PID.setInputLimits(-15,15);
rizqicahyo 0:edddd373a163 114 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 0:edddd373a163 115 PID.setMode(1.0);
rizqicahyo 0:edddd373a163 116 PID.setBias(0.0);
rizqicahyo 0:edddd373a163 117 PID.reset();
rizqicahyo 0:edddd373a163 118
rizqicahyo 0:edddd373a163 119 //inisisasi TIMER
rizqicahyo 0:edddd373a163 120 timer.start();
rizqicahyo 0:edddd373a163 121
rizqicahyo 0:edddd373a163 122 //kondisi robot
rizqicahyo 0:edddd373a163 123 bool manual=true;
rizqicahyo 0:edddd373a163 124
rizqicahyo 0:edddd373a163 125
rizqicahyo 0:edddd373a163 126 while(manual){
rizqicahyo 0:edddd373a163 127
rizqicahyo 0:edddd373a163 128 ps2.poll();
rizqicahyo 0:edddd373a163 129
rizqicahyo 0:edddd373a163 130 aktuator();
rizqicahyo 0:edddd373a163 131 edf_servo();
rizqicahyo 0:edddd373a163 132
rizqicahyo 0:edddd373a163 133 if(limit3==0) manual = false;
rizqicahyo 0:edddd373a163 134
rizqicahyo 0:edddd373a163 135 }
rizqicahyo 0:edddd373a163 136
rizqicahyo 0:edddd373a163 137 motor1.brake(1);
rizqicahyo 0:edddd373a163 138 motor2.brake(1);
rizqicahyo 0:edddd373a163 139 motor3.brake(1);
rizqicahyo 0:edddd373a163 140 motor4.brake(1);
rizqicahyo 0:edddd373a163 141
rizqicahyo 0:edddd373a163 142 pnuematik1=0;
rizqicahyo 0:edddd373a163 143 wait_ms(1500);
rizqicahyo 0:edddd373a163 144
rizqicahyo 0:edddd373a163 145 while(limit4!=0){
rizqicahyo 0:edddd373a163 146 motorC1.speed(1);
rizqicahyo 0:edddd373a163 147 motorC2.speed(-1);
rizqicahyo 0:edddd373a163 148 }
rizqicahyo 0:edddd373a163 149
rizqicahyo 0:edddd373a163 150 motorC1.brake(1);
rizqicahyo 0:edddd373a163 151 motorC2.brake(1);
rizqicahyo 0:edddd373a163 152
rizqicahyo 0:edddd373a163 153 pnuematik3 = 0;
rizqicahyo 0:edddd373a163 154 wait_ms(1500);
rizqicahyo 0:edddd373a163 155 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 156 wait_ms(500);
rizqicahyo 0:edddd373a163 157 pnuematik3 = 1;
rizqicahyo 0:edddd373a163 158
rizqicahyo 0:edddd373a163 159
rizqicahyo 0:edddd373a163 160
rizqicahyo 0:edddd373a163 161 return 0;
rizqicahyo 0:edddd373a163 162 }
rizqicahyo 0:edddd373a163 163
rizqicahyo 0:edddd373a163 164 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 165 /** ALGORITMA PROSEDUR DAN FUNGSI **/
rizqicahyo 0:edddd373a163 166 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 167 void aktuator(){
rizqicahyo 0:edddd373a163 168 float speed = v0;
Najib_irvani 1:fc1535231c0d 169 float tuning1 = 0.0;
Najib_irvani 1:fc1535231c0d 170 float tuning2 = 0.23;
Najib_irvani 1:fc1535231c0d 171 float tuning3 = 0.1;
Najib_irvani 1:fc1535231c0d 172 float tuning4 = 0.26;
Najib_irvani 1:fc1535231c0d 173
Najib_irvani 1:fc1535231c0d 174 if(v0 >= gMax_speed) v0 = gMax_speed;
Najib_irvani 1:fc1535231c0d 175
rizqicahyo 0:edddd373a163 176 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 0:edddd373a163 177 //pivot kiri
Najib_irvani 1:fc1535231c0d 178 motor2.speed((float)0.6*(speed-tuning2));
Najib_irvani 1:fc1535231c0d 179 motor4.speed((float)-0.6*(speed-tuning4));
Najib_irvani 1:fc1535231c0d 180 motor1.speed((float)0.6*(speed-tuning1));
Najib_irvani 1:fc1535231c0d 181 motor3.speed((float)-0.6*(speed-tuning3));
rizqicahyo 0:edddd373a163 182 pc.printf("pivot kiri \n");
rizqicahyo 0:edddd373a163 183
rizqicahyo 0:edddd373a163 184 v0 += ax;
rizqicahyo 0:edddd373a163 185 }
rizqicahyo 0:edddd373a163 186 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 0:edddd373a163 187 //pivot kanan
Najib_irvani 1:fc1535231c0d 188 motor2.speed((float)-0.6*(speed-tuning2));
Najib_irvani 1:fc1535231c0d 189 motor4.speed((float)0.6*(speed-tuning4));
Najib_irvani 1:fc1535231c0d 190 motor1.speed((float)-0.6*(speed-tuning1));
Najib_irvani 1:fc1535231c0d 191 motor3.speed((float)0.6*(speed-tuning3));
rizqicahyo 0:edddd373a163 192 pc.printf("pivot kanan \n");
rizqicahyo 0:edddd373a163 193
rizqicahyo 0:edddd373a163 194 v0 += ax;
rizqicahyo 0:edddd373a163 195 }
rizqicahyo 0:edddd373a163 196 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 0:edddd373a163 197 //serong atas kanan
Najib_irvani 1:fc1535231c0d 198 motor2.speed(speed-tuning2);
rizqicahyo 0:edddd373a163 199 motor4.brake(1);
rizqicahyo 0:edddd373a163 200 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 201 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 202 pc.printf("serong atas kanan \n");
rizqicahyo 0:edddd373a163 203
rizqicahyo 0:edddd373a163 204 v0 += ax;
rizqicahyo 0:edddd373a163 205 }
rizqicahyo 0:edddd373a163 206 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 207 //serong atas kiri
rizqicahyo 0:edddd373a163 208 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 209 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 210 motor1.speed(-(speed-tuning1));
rizqicahyo 0:edddd373a163 211 motor3.brake(1);
rizqicahyo 0:edddd373a163 212 pc.printf("serong atas kiri \n");
rizqicahyo 0:edddd373a163 213
rizqicahyo 0:edddd373a163 214 v0 += ax;
rizqicahyo 0:edddd373a163 215 }
rizqicahyo 0:edddd373a163 216 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 0:edddd373a163 217 //serong bawah kiri
Najib_irvani 1:fc1535231c0d 218 motor2.speed(-(speed-tuning2));
rizqicahyo 0:edddd373a163 219 motor4.brake(1);
rizqicahyo 0:edddd373a163 220 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 221 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 222 pc.printf("serong bawah kiri \n");
rizqicahyo 0:edddd373a163 223
rizqicahyo 0:edddd373a163 224 v0 += ax;
rizqicahyo 0:edddd373a163 225 }
rizqicahyo 0:edddd373a163 226 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 0:edddd373a163 227 //serong bawah kanan
rizqicahyo 0:edddd373a163 228 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 229 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 230 motor1.speed(speed-tuning1);
rizqicahyo 0:edddd373a163 231 motor3.brake(1);
rizqicahyo 0:edddd373a163 232 pc.printf("serong bawah kanan \n");
rizqicahyo 0:edddd373a163 233
rizqicahyo 0:edddd373a163 234 v0 += ax;
rizqicahyo 0:edddd373a163 235 }
rizqicahyo 0:edddd373a163 236 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 0:edddd373a163 237 //maju
Najib_irvani 1:fc1535231c0d 238 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 239 motor3.speed(speed-tuning3);
Najib_irvani 1:fc1535231c0d 240 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 241 motor4.speed(-(speed-tuning4));
rizqicahyo 0:edddd373a163 242 pc.printf("maju \n");
rizqicahyo 0:edddd373a163 243
rizqicahyo 0:edddd373a163 244 v0 += ax;
rizqicahyo 0:edddd373a163 245 }
rizqicahyo 0:edddd373a163 246 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 0:edddd373a163 247 //mundur
Najib_irvani 1:fc1535231c0d 248 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 249 motor3.speed(-(speed-tuning3));
Najib_irvani 1:fc1535231c0d 250 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 251 motor4.speed(speed-tuning4);
rizqicahyo 0:edddd373a163 252 pc.printf("mundur \n");
rizqicahyo 0:edddd373a163 253
rizqicahyo 0:edddd373a163 254 v0 += ax;
rizqicahyo 0:edddd373a163 255 }
rizqicahyo 0:edddd373a163 256 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 0:edddd373a163 257 //kanan
Najib_irvani 1:fc1535231c0d 258 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 259 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 260 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 261 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 262 pc.printf("kanan \n");
rizqicahyo 0:edddd373a163 263
rizqicahyo 0:edddd373a163 264 v0 += ax;
rizqicahyo 0:edddd373a163 265 }
rizqicahyo 0:edddd373a163 266 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 267 //kiri
Najib_irvani 1:fc1535231c0d 268 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 269 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 270 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 271 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 272 pc.printf("kiri \n");
rizqicahyo 0:edddd373a163 273
rizqicahyo 0:edddd373a163 274 v0 += ax;
rizqicahyo 0:edddd373a163 275 }
rizqicahyo 0:edddd373a163 276 else{
rizqicahyo 0:edddd373a163 277 motor1.brake(1);
rizqicahyo 0:edddd373a163 278 motor3.brake(1);
rizqicahyo 0:edddd373a163 279 motor2.brake(1);
rizqicahyo 0:edddd373a163 280 motor4.brake(1);
rizqicahyo 0:edddd373a163 281 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 282
rizqicahyo 0:edddd373a163 283 v0 = 0.6;
rizqicahyo 0:edddd373a163 284 }
rizqicahyo 0:edddd373a163 285
rizqicahyo 0:edddd373a163 286 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
rizqicahyo 0:edddd373a163 287 //POWER WINDOW ATAS
rizqicahyo 0:edddd373a163 288 motorS.speed(1);
rizqicahyo 0:edddd373a163 289 pc.printf("up \n");
rizqicahyo 0:edddd373a163 290 }
rizqicahyo 0:edddd373a163 291 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
rizqicahyo 0:edddd373a163 292 //POWER WINDOW BAWAH
rizqicahyo 0:edddd373a163 293 motorS.speed(-1);
rizqicahyo 0:edddd373a163 294 pc.printf("down \n");
rizqicahyo 0:edddd373a163 295 }
rizqicahyo 0:edddd373a163 296 else{
rizqicahyo 0:edddd373a163 297 motorS.brake(1);
rizqicahyo 0:edddd373a163 298 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 299
rizqicahyo 0:edddd373a163 300 }
rizqicahyo 0:edddd373a163 301
rizqicahyo 0:edddd373a163 302 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 0:edddd373a163 303 {
rizqicahyo 0:edddd373a163 304 //mekanisme ambil gripper
rizqicahyo 0:edddd373a163 305 pc.printf("mekanisme gripper");
rizqicahyo 0:edddd373a163 306 if (g==1){
rizqicahyo 0:edddd373a163 307 pc.printf("ambil 1");
rizqicahyo 0:edddd373a163 308 pnuematik2 = 0;
rizqicahyo 0:edddd373a163 309 g=2;
rizqicahyo 0:edddd373a163 310 wait_ms(400);
rizqicahyo 0:edddd373a163 311 }
rizqicahyo 0:edddd373a163 312 else
rizqicahyo 0:edddd373a163 313 {
rizqicahyo 0:edddd373a163 314 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 315 wait_ms(400);
rizqicahyo 0:edddd373a163 316 g=1;
rizqicahyo 0:edddd373a163 317 }
rizqicahyo 0:edddd373a163 318 }
rizqicahyo 0:edddd373a163 319 }
rizqicahyo 0:edddd373a163 320
rizqicahyo 0:edddd373a163 321 void edf_servo(){
rizqicahyo 0:edddd373a163 322 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 0:edddd373a163 323 //PWM ++
rizqicahyo 0:edddd373a163 324 pwm += 0.002;
rizqicahyo 0:edddd373a163 325 if( pwm > 0.7) pwm = 0.7;
rizqicahyo 0:edddd373a163 326 pc.printf("gaspol \n");
rizqicahyo 0:edddd373a163 327 }
rizqicahyo 0:edddd373a163 328 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
rizqicahyo 0:edddd373a163 329 //PWM--
rizqicahyo 0:edddd373a163 330 pwm -= 0.002;
rizqicahyo 0:edddd373a163 331
rizqicahyo 0:edddd373a163 332 if(pwm < 0) pwm = 0.0;
rizqicahyo 0:edddd373a163 333 pc.printf("rem ndeng \n");
rizqicahyo 0:edddd373a163 334 }
rizqicahyo 0:edddd373a163 335
rizqicahyo 0:edddd373a163 336 if(ps2.read(PS_PAD::PAD_R2)==1){
rizqicahyo 0:edddd373a163 337 //SERVO --
rizqicahyo 0:edddd373a163 338 sudut += 0.5;
rizqicahyo 0:edddd373a163 339
rizqicahyo 0:edddd373a163 340 if(sudut > 90) sudut = 90;
rizqicahyo 0:edddd373a163 341 pc.printf("servo max \n");
rizqicahyo 0:edddd373a163 342 }
rizqicahyo 0:edddd373a163 343 else if(ps2.read(PS_PAD::PAD_L2)==1){
rizqicahyo 0:edddd373a163 344 //SERVO ++
rizqicahyo 0:edddd373a163 345 sudut -= 0.5;
rizqicahyo 0:edddd373a163 346
rizqicahyo 0:edddd373a163 347 if(sudut < -90) sudut = -90;
rizqicahyo 0:edddd373a163 348 pc.printf("servo min \n");
rizqicahyo 0:edddd373a163 349 }
rizqicahyo 0:edddd373a163 350
rizqicahyo 0:edddd373a163 351 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 0:edddd373a163 352
rizqicahyo 0:edddd373a163 353 sudut = 0;
rizqicahyo 0:edddd373a163 354 pwm = 0.25;
rizqicahyo 0:edddd373a163 355 }
rizqicahyo 0:edddd373a163 356
rizqicahyo 0:edddd373a163 357 servoEDF.position((float)sudut);
rizqicahyo 0:edddd373a163 358 edf.setThrottle((float)pwm);
rizqicahyo 0:edddd373a163 359 edf.pulse();
rizqicahyo 0:edddd373a163 360 }