KRAI 2016 / Mbed 2 deprecated base_rekam_darurat_v1

darurat

Dependencies:   ESC Motor NewTextLCD PID PS_PAD base_rekam_darurat mbed millis

Fork of Base_Hybrid_V2 by KRAI 2016

main.cpp@3:d3708c3ed288, 2016-02-17 (annotated)

Committer:
Najib_irvani
Date:
Wed Feb 17 17:21:18 2016 +0000
Revision:
3:d3708c3ed288
Parent:
2:2f7bed7fb055
Child:
4:65d65a108b68
Hybrid Evolution

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:ac7353383a8e 1 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 2 /** GARUDAGO-ITB (KRAI2016) **/
rizqicahyo 0:ac7353383a8e 3 /** #ROADTOBANGKOK! **/
rizqicahyo 0:ac7353383a8e 4 /** **/
rizqicahyo 0:ac7353383a8e 5 /** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
rizqicahyo 0:ac7353383a8e 6 /** **/
rizqicahyo 0:ac7353383a8e 7 /** **/
rizqicahyo 0:ac7353383a8e 8 /** Created by : **/
rizqicahyo 0:ac7353383a8e 9 /** Rizqi Cahyo Yuwono **/
rizqicahyo 0:ac7353383a8e 10 /** EL'14 - 13214090 **/
rizqicahyo 0:ac7353383a8e 11 /** **/
rizqicahyo 0:ac7353383a8e 12 /** Last Update : 19 Desember 2015, 06.10 WIB **/
rizqicahyo 0:ac7353383a8e 13 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 14
rizqicahyo 0:ac7353383a8e 15 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 16 /** FILE HEADER **/
rizqicahyo 0:ac7353383a8e 17 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 18 #include "mbed.h"
rizqicahyo 0:ac7353383a8e 19 #include "Motor.h"
rizqicahyo 0:ac7353383a8e 20 #include "NewTextLCD.h"
rizqicahyo 0:ac7353383a8e 21 #include "PS_PAD.h"
rizqicahyo 0:ac7353383a8e 22 #include "PID.h"
rizqicahyo 1:c9f11055fb12 23 #include "millis.h"
Najib_irvani 2:2f7bed7fb055 24 #include "esc.h"
Najib_irvani 2:2f7bed7fb055 25 #include "Servo.h"
rizqicahyo 0:ac7353383a8e 26
Najib_irvani 3:d3708c3ed288 27
rizqicahyo 0:ac7353383a8e 28 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 29 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:ac7353383a8e 30 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 31 // serial pc
rizqicahyo 0:ac7353383a8e 32 Serial pc(USBTX,USBRX);
rizqicahyo 0:ac7353383a8e 33
rizqicahyo 0:ac7353383a8e 34 // LCD 20x4
Najib_irvani 2:2f7bed7fb055 35 //TextLCD lcd(PC_5, PB_1, PA_7, PC_4, PA_5, PA_6, TextLCD::LCD20x4); //(rs,e,d4,d5,d6,d7)
rizqicahyo 0:ac7353383a8e 36
rizqicahyo 0:ac7353383a8e 37 // joystick PS2
rizqicahyo 0:ac7353383a8e 38 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
rizqicahyo 0:ac7353383a8e 39
Najib_irvani 2:2f7bed7fb055 40
Najib_irvani 2:2f7bed7fb055 41 // tambahan power
Najib_irvani 2:2f7bed7fb055 42 // inisialisasi pwm awal servo
Najib_irvani 2:2f7bed7fb055 43 float pwm = 0.00;
Najib_irvani 2:2f7bed7fb055 44
Najib_irvani 2:2f7bed7fb055 45 // inisialisasi sudut awal
Najib_irvani 2:2f7bed7fb055 46 int sudut = 0;
Najib_irvani 2:2f7bed7fb055 47
Najib_irvani 2:2f7bed7fb055 48 //membatasi nilai brushless pada edf
Najib_irvani 2:2f7bed7fb055 49 float min=0;
Najib_irvani 2:2f7bed7fb055 50 float max=0.50;
Najib_irvani 2:2f7bed7fb055 51
rizqicahyo 0:ac7353383a8e 52 // PID sensor garis
Najib_irvani 2:2f7bed7fb055 53 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rizqicahyo 0:ac7353383a8e 54
rizqicahyo 0:ac7353383a8e 55 // Motor(pwm, fwd, rev)
Najib_irvani 3:d3708c3ed288 56 //Motor motor2(PC_6, PC_8, PC_9); //gripper
Najib_irvani 3:d3708c3ed288 57 //Motor motor1(PA_15, PA_14, PA_13); //motor3
Najib_irvani 3:d3708c3ed288 58 //Motor gripper(PB_5, PA_11, PA_12);
Najib_irvani 2:2f7bed7fb055 59 //Motor motor2(PA_3, PC_15, PC_14); //kanan
Najib_irvani 2:2f7bed7fb055 60 //Motor motor1(PA_1, PH_0, PH_1); //kiri
rizqicahyo 0:ac7353383a8e 61
Najib_irvani 3:d3708c3ed288 62 Motor gripper(PA_10, PB_4, PB_5); // pwm, fwd, rev
Najib_irvani 3:d3708c3ed288 63 Motor motor2(PA_0, PA_6, PA_7); // pwm, fwd, rev
Najib_irvani 3:d3708c3ed288 64 //Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
Najib_irvani 3:d3708c3ed288 65 Motor motor1(PA_1, PC_9, PC_8); // pwm, fwd, rev
Najib_irvani 3:d3708c3ed288 66
Najib_irvani 3:d3708c3ed288 67 DigitalOut pnuematik_lengan(PC_10);
Najib_irvani 3:d3708c3ed288 68 DigitalOut pnuematik_gripper(PC_11);
Najib_irvani 3:d3708c3ed288 69
Najib_irvani 3:d3708c3ed288 70 Servo servo_gripper(PC_7);
Najib_irvani 3:d3708c3ed288 71
Najib_irvani 2:2f7bed7fb055 72 /*
rizqicahyo 0:ac7353383a8e 73 // Sensor
rizqicahyo 0:ac7353383a8e 74 DigitalIn S1(PC_0);
rizqicahyo 0:ac7353383a8e 75 DigitalIn S2(PC_1);
rizqicahyo 0:ac7353383a8e 76 DigitalIn S3(PC_2);
rizqicahyo 0:ac7353383a8e 77 DigitalIn S4(PC_3);
rizqicahyo 0:ac7353383a8e 78 DigitalIn S5(PA_0);
rizqicahyo 0:ac7353383a8e 79 DigitalIn S6(PA_1);
rizqicahyo 0:ac7353383a8e 80 DigitalIn S7(PA_4);
rizqicahyo 0:ac7353383a8e 81 DigitalIn S8(PB_0);
rizqicahyo 0:ac7353383a8e 82 DigitalIn S9(PB_2);
rizqicahyo 0:ac7353383a8e 83 DigitalIn S10(PB_10);
rizqicahyo 0:ac7353383a8e 84 DigitalIn S11(PA_10);
rizqicahyo 0:ac7353383a8e 85 DigitalIn S12(PA_11);
rizqicahyo 0:ac7353383a8e 86 DigitalIn S13(PA_12);
rizqicahyo 0:ac7353383a8e 87 DigitalOut calibrate(PA_15);
Najib_irvani 2:2f7bed7fb055 88 */
rizqicahyo 0:ac7353383a8e 89
rizqicahyo 0:ac7353383a8e 90 DigitalIn button(USER_BUTTON);
rizqicahyo 0:ac7353383a8e 91
Najib_irvani 2:2f7bed7fb055 92
Najib_irvani 2:2f7bed7fb055 93 //bool sensor[13];
rizqicahyo 0:ac7353383a8e 94
rizqicahyo 0:ac7353383a8e 95 //DigitalIn limit_switch1(A0);
rizqicahyo 0:ac7353383a8e 96 //DigitalIn limit_switch2(A1);
rizqicahyo 0:ac7353383a8e 97
rizqicahyo 0:ac7353383a8e 98
rizqicahyo 0:ac7353383a8e 99 // Multitasker
Najib_irvani 2:2f7bed7fb055 100 //Ticker timer;
rizqicahyo 0:ac7353383a8e 101
rizqicahyo 0:ac7353383a8e 102
rizqicahyo 0:ac7353383a8e 103 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 104 /** DEKLARASI VARIABEL GLOBAL **/
rizqicahyo 0:ac7353383a8e 105 /*********************************************************************************************/
Najib_irvani 3:d3708c3ed288 106 float gMax_speed=0.7; //nilai maksimum kecepatan motor
rizqicahyo 0:ac7353383a8e 107 float gMin_speed=-0.05; //nilai minimum kecepatan motor
Najib_irvani 3:d3708c3ed288 108 float gTuning = 0.16;
rizqicahyo 0:ac7353383a8e 109
rizqicahyo 0:ac7353383a8e 110 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
rizqicahyo 0:ac7353383a8e 111 unsigned char gCase=0; //variabel keadaan proses
rizqicahyo 0:ac7353383a8e 112
rizqicahyo 0:ac7353383a8e 113 unsigned char i; // variabel iterasi
rizqicahyo 1:c9f11055fb12 114 int over=0;
Najib_irvani 2:2f7bed7fb055 115 int lapangan = 1;
rizqicahyo 0:ac7353383a8e 116
rizqicahyo 0:ac7353383a8e 117
rizqicahyo 0:ac7353383a8e 118
rizqicahyo 0:ac7353383a8e 119 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 120 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 0:ac7353383a8e 121 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 122
rizqicahyo 0:ac7353383a8e 123
rizqicahyo 0:ac7353383a8e 124 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 125 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 126 /** PROGRAM UTAMA **/
rizqicahyo 0:ac7353383a8e 127 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 128 /*********************************************************************************************/
Najib_irvani 2:2f7bed7fb055 129
Najib_irvani 3:d3708c3ed288 130 void ambil(int c){
Najib_irvani 3:d3708c3ed288 131 switch (c)
Najib_irvani 3:d3708c3ed288 132 {
Najib_irvani 3:d3708c3ed288 133 case 1:
Najib_irvani 3:d3708c3ed288 134 servo_gripper.position(140);
Najib_irvani 3:d3708c3ed288 135 pnuematik_lengan=1;
Najib_irvani 3:d3708c3ed288 136 wait_ms(500);
Najib_irvani 3:d3708c3ed288 137 pnuematik_gripper=0;
Najib_irvani 3:d3708c3ed288 138 wait_ms(100);
Najib_irvani 3:d3708c3ed288 139 break;
Najib_irvani 3:d3708c3ed288 140 case 2:
Najib_irvani 3:d3708c3ed288 141 wait_ms(300);
Najib_irvani 3:d3708c3ed288 142 servo_gripper.position(40);
Najib_irvani 3:d3708c3ed288 143 wait_ms(1000);
Najib_irvani 3:d3708c3ed288 144 pnuematik_gripper=1;
Najib_irvani 3:d3708c3ed288 145 wait_ms(800);
Najib_irvani 3:d3708c3ed288 146 pnuematik_lengan=0;
Najib_irvani 3:d3708c3ed288 147 wait_ms(200);
Najib_irvani 3:d3708c3ed288 148 break;
Najib_irvani 3:d3708c3ed288 149 }
Najib_irvani 3:d3708c3ed288 150 }
Najib_irvani 3:d3708c3ed288 151
Najib_irvani 2:2f7bed7fb055 152 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 153 if (i){
Najib_irvani 2:2f7bed7fb055 154 if (sudut>90){
Najib_irvani 2:2f7bed7fb055 155 sudut=90;
Najib_irvani 2:2f7bed7fb055 156 }
Najib_irvani 2:2f7bed7fb055 157 if (sudut<0){
Najib_irvani 2:2f7bed7fb055 158 sudut=0;
Najib_irvani 2:2f7bed7fb055 159 }
Najib_irvani 2:2f7bed7fb055 160 } else {
Najib_irvani 2:2f7bed7fb055 161
Najib_irvani 2:2f7bed7fb055 162 if (sudut>0){
Najib_irvani 2:2f7bed7fb055 163 sudut=0;
Najib_irvani 2:2f7bed7fb055 164 }
Najib_irvani 2:2f7bed7fb055 165 if (sudut<-90){
Najib_irvani 2:2f7bed7fb055 166 sudut=-90;
Najib_irvani 2:2f7bed7fb055 167 }
Najib_irvani 2:2f7bed7fb055 168 }
Najib_irvani 2:2f7bed7fb055 169 }
Najib_irvani 2:2f7bed7fb055 170
Najib_irvani 2:2f7bed7fb055 171 void init_pwm (){
Najib_irvani 2:2f7bed7fb055 172 if (pwm>max){
Najib_irvani 2:2f7bed7fb055 173 pwm = max;
Najib_irvani 2:2f7bed7fb055 174 }
Najib_irvani 2:2f7bed7fb055 175
Najib_irvani 2:2f7bed7fb055 176 if (pwm<min){
Najib_irvani 2:2f7bed7fb055 177 pwm = min;
Najib_irvani 2:2f7bed7fb055 178 }
Najib_irvani 2:2f7bed7fb055 179 }
Najib_irvani 2:2f7bed7fb055 180
Najib_irvani 3:d3708c3ed288 181 int count=1;
Najib_irvani 3:d3708c3ed288 182
rizqicahyo 0:ac7353383a8e 183 int main(void){
rizqicahyo 0:ac7353383a8e 184 //inisialisasi joystick
rizqicahyo 0:ac7353383a8e 185 ps2.init();
rizqicahyo 0:ac7353383a8e 186
Najib_irvani 2:2f7bed7fb055 187 //tambahan power
Najib_irvani 2:2f7bed7fb055 188 ESC edf(PB_9,20); //p wm esc pin PC_7
Najib_irvani 2:2f7bed7fb055 189 Servo myservo(PB_8); //pwm servo pin PA_8
Najib_irvani 2:2f7bed7fb055 190 //set inisialisasi servo pada posisi 0
Najib_irvani 2:2f7bed7fb055 191 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 192 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
Najib_irvani 2:2f7bed7fb055 193 edf.setThrottle(pwm);
Najib_irvani 2:2f7bed7fb055 194 edf.pulse();
rizqicahyo 0:ac7353383a8e 195
rizqicahyo 0:ac7353383a8e 196 pc.baud(115200);
Najib_irvani 2:2f7bed7fb055 197 float speed;
rizqicahyo 0:ac7353383a8e 198
Najib_irvani 3:d3708c3ed288 199 servo_gripper.position(140);
Najib_irvani 3:d3708c3ed288 200 pnuematik_lengan=0;
Najib_irvani 3:d3708c3ed288 201
rizqicahyo 0:ac7353383a8e 202 while(1)
rizqicahyo 0:ac7353383a8e 203 {
Najib_irvani 2:2f7bed7fb055 204 ps2.poll();
Najib_irvani 2:2f7bed7fb055 205
Najib_irvani 2:2f7bed7fb055 206 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
Najib_irvani 2:2f7bed7fb055 207 //MOTOR DEPAN
Najib_irvani 2:2f7bed7fb055 208 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 209
Najib_irvani 3:d3708c3ed288 210 motor1.speed(speed-gTuning);
Najib_irvani 2:2f7bed7fb055 211 motor2.speed(speed);
Najib_irvani 2:2f7bed7fb055 212 pc.printf("maju \n");
Najib_irvani 2:2f7bed7fb055 213 }
Najib_irvani 2:2f7bed7fb055 214 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 215 //MOTOR BELAKANG
Najib_irvani 2:2f7bed7fb055 216 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 217
Najib_irvani 3:d3708c3ed288 218 motor1.speed(-(speed-gTuning));
Najib_irvani 2:2f7bed7fb055 219 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 220 pc.printf("mundur \n");
Najib_irvani 2:2f7bed7fb055 221 }
Najib_irvani 2:2f7bed7fb055 222 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 223 //pivot kiri
Najib_irvani 2:2f7bed7fb055 224 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 225
Najib_irvani 3:d3708c3ed288 226 motor1.speed(-(speed*0.9-gTuning));
Najib_irvani 3:d3708c3ed288 227 motor2.speed(speed*0.9);
Najib_irvani 2:2f7bed7fb055 228 pc.printf("kiri \n");
Najib_irvani 2:2f7bed7fb055 229 }
Najib_irvani 2:2f7bed7fb055 230 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 231 //pivot kanan
Najib_irvani 2:2f7bed7fb055 232 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 233
Najib_irvani 3:d3708c3ed288 234 motor1.speed(speed*0.9-gTuning);
Najib_irvani 3:d3708c3ed288 235 motor2.speed(-speed*0.9 );
Najib_irvani 2:2f7bed7fb055 236 pc.printf("kanan \n");
Najib_irvani 2:2f7bed7fb055 237 }
Najib_irvani 3:d3708c3ed288 238 else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
Najib_irvani 3:d3708c3ed288 239 {
Najib_irvani 3:d3708c3ed288 240 if (count==1){
Najib_irvani 3:d3708c3ed288 241 ambil(1);
Najib_irvani 3:d3708c3ed288 242 count=2;
Najib_irvani 3:d3708c3ed288 243 wait_ms(400);
Najib_irvani 3:d3708c3ed288 244 }
Najib_irvani 3:d3708c3ed288 245 else
Najib_irvani 3:d3708c3ed288 246 {
Najib_irvani 3:d3708c3ed288 247 ambil(2);
Najib_irvani 3:d3708c3ed288 248 count=1;
Najib_irvani 3:d3708c3ed288 249 wait_ms(400);
Najib_irvani 3:d3708c3ed288 250 }
Najib_irvani 3:d3708c3ed288 251 }
Najib_irvani 2:2f7bed7fb055 252 else
rizqicahyo 1:c9f11055fb12 253 {
Najib_irvani 2:2f7bed7fb055 254 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 255 motor2.brake(1);
Najib_irvani 2:2f7bed7fb055 256 }
Najib_irvani 2:2f7bed7fb055 257
Najib_irvani 2:2f7bed7fb055 258 if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
Najib_irvani 2:2f7bed7fb055 259 //SLIDER KIRI
Najib_irvani 2:2f7bed7fb055 260 pc.printf("slide kiri \n");
Najib_irvani 2:2f7bed7fb055 261 }
Najib_irvani 2:2f7bed7fb055 262 else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
Najib_irvani 2:2f7bed7fb055 263 //SLIDER KANAN
Najib_irvani 2:2f7bed7fb055 264 pc.printf("slide kanan \n");
rizqicahyo 1:c9f11055fb12 265 }
Najib_irvani 2:2f7bed7fb055 266 else
Najib_irvani 2:2f7bed7fb055 267 {
Najib_irvani 2:2f7bed7fb055 268 pc.printf("slide diam \n");
Najib_irvani 2:2f7bed7fb055 269 }
Najib_irvani 2:2f7bed7fb055 270
Najib_irvani 3:d3708c3ed288 271 if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
Najib_irvani 2:2f7bed7fb055 272 //POWER WINDOW ATAS
Najib_irvani 3:d3708c3ed288 273 gripper.speed(1);
Najib_irvani 2:2f7bed7fb055 274 pc.printf("up \n");
Najib_irvani 2:2f7bed7fb055 275 }
Najib_irvani 2:2f7bed7fb055 276 else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
Najib_irvani 2:2f7bed7fb055 277 //POWER WINDOW BAWAH
Najib_irvani 2:2f7bed7fb055 278 gripper.speed(-0.2);
Najib_irvani 2:2f7bed7fb055 279 pc.printf("down \n");
Najib_irvani 2:2f7bed7fb055 280 }
Najib_irvani 2:2f7bed7fb055 281 else
Najib_irvani 2:2f7bed7fb055 282 {
Najib_irvani 2:2f7bed7fb055 283 gripper.brake(1);
Najib_irvani 2:2f7bed7fb055 284 pc.printf("power diam \n");
Najib_irvani 3:d3708c3ed288 285 }
Najib_irvani 2:2f7bed7fb055 286
Najib_irvani 2:2f7bed7fb055 287 if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
Najib_irvani 2:2f7bed7fb055 288 //PWM ++
Najib_irvani 2:2f7bed7fb055 289 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 290 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 291 }
Najib_irvani 2:2f7bed7fb055 292
Najib_irvani 2:2f7bed7fb055 293 if(ps2.read(PS_PAD::ANALOG_RY)>=100){
Najib_irvani 2:2f7bed7fb055 294 //PWM--
Najib_irvani 2:2f7bed7fb055 295 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 296 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 297 }
Najib_irvani 2:2f7bed7fb055 298
Najib_irvani 2:2f7bed7fb055 299 if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
Najib_irvani 2:2f7bed7fb055 300 //SERVO --
Najib_irvani 2:2f7bed7fb055 301 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 302 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 303 }
Najib_irvani 2:2f7bed7fb055 304
Najib_irvani 2:2f7bed7fb055 305 if(ps2.read(PS_PAD::ANALOG_RX)>=100){
Najib_irvani 2:2f7bed7fb055 306 //SERVO ++
Najib_irvani 2:2f7bed7fb055 307 sudut += 3;
Najib_irvani 2:2f7bed7fb055 308 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 309 }
Najib_irvani 3:d3708c3ed288 310
Najib_irvani 2:2f7bed7fb055 311 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 312 init_pwm();
Najib_irvani 2:2f7bed7fb055 313 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 314 wait_ms(25);
Najib_irvani 2:2f7bed7fb055 315 edf.setThrottle(pwm);
Najib_irvani 2:2f7bed7fb055 316 edf.pulse();
Najib_irvani 2:2f7bed7fb055 317 wait_ms(25);
rizqicahyo 0:ac7353383a8e 318 }
rizqicahyo 0:ac7353383a8e 319 }