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

Committer:
rizqicahyo
Date:
Tue Apr 19 02:47:31 2016 +0000
Revision:
0:edddd373a163
Child:
1:fc1535231c0d
hadah

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