KRAI 2016 / Mbed 2 deprecated Base_Hybrid_Latihan_Ok_Hajar_servo_sensor

hadah. jajal

Dependencies:   ESC Motor NewTextLCD PID PS_PAD Ping Servo mbed millis

Fork of Base_Hybrid_Latihan_Ok_Hajar_servo_pwm by KRAI 2016

main.cpp@4:65d65a108b68, 2016-02-18 (annotated)

Committer:
Najib_irvani
Date:
Thu Feb 18 09:18:39 2016 +0000
Revision:
4:65d65a108b68
Parent:
3:d3708c3ed288
Child:
5:34be90fa6d27
hmmmmmmmmmmmmmm

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 4:65d65a108b68 70 Servo servo_gripper(PB_9);
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 4:65d65a108b68 134 servo_gripper.position(170);
Najib_irvani 4:65d65a108b68 135 pc.printf("grip lost");
Najib_irvani 4:65d65a108b68 136 wait_ms(25);
Najib_irvani 3:d3708c3ed288 137 pnuematik_lengan=1;
Najib_irvani 3:d3708c3ed288 138 wait_ms(500);
Najib_irvani 4:65d65a108b68 139
Najib_irvani 3:d3708c3ed288 140 break;
Najib_irvani 3:d3708c3ed288 141 case 2:
Najib_irvani 4:65d65a108b68 142 pnuematik_gripper=0;
Najib_irvani 4:65d65a108b68 143 wait_ms(600);
Najib_irvani 4:65d65a108b68 144 servo_gripper.position(50);
Najib_irvani 4:65d65a108b68 145 pc.printf("grip get");
Najib_irvani 4:65d65a108b68 146 wait_ms(25);
Najib_irvani 3:d3708c3ed288 147 wait_ms(1000);
Najib_irvani 3:d3708c3ed288 148 pnuematik_gripper=1;
Najib_irvani 3:d3708c3ed288 149 wait_ms(800);
Najib_irvani 3:d3708c3ed288 150 pnuematik_lengan=0;
Najib_irvani 3:d3708c3ed288 151 wait_ms(200);
Najib_irvani 3:d3708c3ed288 152 break;
Najib_irvani 3:d3708c3ed288 153 }
Najib_irvani 3:d3708c3ed288 154 }
Najib_irvani 3:d3708c3ed288 155
Najib_irvani 2:2f7bed7fb055 156 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 157 if (i){
Najib_irvani 2:2f7bed7fb055 158 if (sudut>90){
Najib_irvani 2:2f7bed7fb055 159 sudut=90;
Najib_irvani 2:2f7bed7fb055 160 }
Najib_irvani 2:2f7bed7fb055 161 if (sudut<0){
Najib_irvani 2:2f7bed7fb055 162 sudut=0;
Najib_irvani 2:2f7bed7fb055 163 }
Najib_irvani 2:2f7bed7fb055 164 } else {
Najib_irvani 2:2f7bed7fb055 165
Najib_irvani 2:2f7bed7fb055 166 if (sudut>0){
Najib_irvani 2:2f7bed7fb055 167 sudut=0;
Najib_irvani 2:2f7bed7fb055 168 }
Najib_irvani 2:2f7bed7fb055 169 if (sudut<-90){
Najib_irvani 2:2f7bed7fb055 170 sudut=-90;
Najib_irvani 2:2f7bed7fb055 171 }
Najib_irvani 2:2f7bed7fb055 172 }
Najib_irvani 2:2f7bed7fb055 173 }
Najib_irvani 2:2f7bed7fb055 174
Najib_irvani 2:2f7bed7fb055 175 void init_pwm (){
Najib_irvani 2:2f7bed7fb055 176 if (pwm>max){
Najib_irvani 2:2f7bed7fb055 177 pwm = max;
Najib_irvani 2:2f7bed7fb055 178 }
Najib_irvani 2:2f7bed7fb055 179
Najib_irvani 2:2f7bed7fb055 180 if (pwm<min){
Najib_irvani 2:2f7bed7fb055 181 pwm = min;
Najib_irvani 2:2f7bed7fb055 182 }
Najib_irvani 2:2f7bed7fb055 183 }
Najib_irvani 2:2f7bed7fb055 184
Najib_irvani 3:d3708c3ed288 185 int count=1;
Najib_irvani 3:d3708c3ed288 186
rizqicahyo 0:ac7353383a8e 187 int main(void){
rizqicahyo 0:ac7353383a8e 188 //inisialisasi joystick
rizqicahyo 0:ac7353383a8e 189 ps2.init();
rizqicahyo 0:ac7353383a8e 190
Najib_irvani 2:2f7bed7fb055 191 //tambahan power
Najib_irvani 4:65d65a108b68 192 //ESC edf(PB_9,20); //p wm esc pin PC_7
Najib_irvani 2:2f7bed7fb055 193 Servo myservo(PB_8); //pwm servo pin PA_8
Najib_irvani 2:2f7bed7fb055 194 //set inisialisasi servo pada posisi 0
Najib_irvani 2:2f7bed7fb055 195 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 196 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
Najib_irvani 4:65d65a108b68 197 //edf.setThrottle(pwm);
Najib_irvani 4:65d65a108b68 198 //edf.pulse();
rizqicahyo 0:ac7353383a8e 199
rizqicahyo 0:ac7353383a8e 200 pc.baud(115200);
Najib_irvani 2:2f7bed7fb055 201 float speed;
rizqicahyo 0:ac7353383a8e 202
Najib_irvani 3:d3708c3ed288 203 servo_gripper.position(140);
Najib_irvani 3:d3708c3ed288 204 pnuematik_lengan=0;
Najib_irvani 4:65d65a108b68 205 pnuematik_gripper=1;
Najib_irvani 3:d3708c3ed288 206
rizqicahyo 0:ac7353383a8e 207 while(1)
rizqicahyo 0:ac7353383a8e 208 {
Najib_irvani 2:2f7bed7fb055 209 ps2.poll();
Najib_irvani 2:2f7bed7fb055 210
Najib_irvani 2:2f7bed7fb055 211 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
Najib_irvani 2:2f7bed7fb055 212 //MOTOR DEPAN
Najib_irvani 2:2f7bed7fb055 213 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 214
Najib_irvani 3:d3708c3ed288 215 motor1.speed(speed-gTuning);
Najib_irvani 2:2f7bed7fb055 216 motor2.speed(speed);
Najib_irvani 2:2f7bed7fb055 217 pc.printf("maju \n");
Najib_irvani 2:2f7bed7fb055 218 }
Najib_irvani 2:2f7bed7fb055 219 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 220 //MOTOR BELAKANG
Najib_irvani 2:2f7bed7fb055 221 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 222
Najib_irvani 3:d3708c3ed288 223 motor1.speed(-(speed-gTuning));
Najib_irvani 2:2f7bed7fb055 224 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 225 pc.printf("mundur \n");
Najib_irvani 2:2f7bed7fb055 226 }
Najib_irvani 2:2f7bed7fb055 227 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 228 //pivot kiri
Najib_irvani 2:2f7bed7fb055 229 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 230
Najib_irvani 3:d3708c3ed288 231 motor1.speed(-(speed*0.9-gTuning));
Najib_irvani 3:d3708c3ed288 232 motor2.speed(speed*0.9);
Najib_irvani 2:2f7bed7fb055 233 pc.printf("kiri \n");
Najib_irvani 2:2f7bed7fb055 234 }
Najib_irvani 2:2f7bed7fb055 235 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 236 //pivot kanan
Najib_irvani 2:2f7bed7fb055 237 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 238
Najib_irvani 3:d3708c3ed288 239 motor1.speed(speed*0.9-gTuning);
Najib_irvani 3:d3708c3ed288 240 motor2.speed(-speed*0.9 );
Najib_irvani 2:2f7bed7fb055 241 pc.printf("kanan \n");
Najib_irvani 2:2f7bed7fb055 242 }
Najib_irvani 3:d3708c3ed288 243 else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
Najib_irvani 3:d3708c3ed288 244 {
Najib_irvani 4:65d65a108b68 245 //mekanisme ambil gripper
Najib_irvani 4:65d65a108b68 246 pc.printf("mekanisme gripper");
Najib_irvani 3:d3708c3ed288 247 if (count==1){
Najib_irvani 4:65d65a108b68 248 pc.printf("ambil 1");
Najib_irvani 3:d3708c3ed288 249 ambil(1);
Najib_irvani 3:d3708c3ed288 250 count=2;
Najib_irvani 3:d3708c3ed288 251 wait_ms(400);
Najib_irvani 3:d3708c3ed288 252 }
Najib_irvani 3:d3708c3ed288 253 else
Najib_irvani 3:d3708c3ed288 254 {
Najib_irvani 3:d3708c3ed288 255 ambil(2);
Najib_irvani 3:d3708c3ed288 256 count=1;
Najib_irvani 4:65d65a108b68 257 pc.printf("ambil 2");
Najib_irvani 3:d3708c3ed288 258 wait_ms(400);
Najib_irvani 3:d3708c3ed288 259 }
Najib_irvani 3:d3708c3ed288 260 }
Najib_irvani 2:2f7bed7fb055 261 else
rizqicahyo 1:c9f11055fb12 262 {
Najib_irvani 2:2f7bed7fb055 263 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 264 motor2.brake(1);
Najib_irvani 2:2f7bed7fb055 265 }
Najib_irvani 2:2f7bed7fb055 266
Najib_irvani 2:2f7bed7fb055 267 if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
Najib_irvani 2:2f7bed7fb055 268 //SLIDER KIRI
Najib_irvani 2:2f7bed7fb055 269 pc.printf("slide kiri \n");
Najib_irvani 2:2f7bed7fb055 270 }
Najib_irvani 2:2f7bed7fb055 271 else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
Najib_irvani 2:2f7bed7fb055 272 //SLIDER KANAN
Najib_irvani 2:2f7bed7fb055 273 pc.printf("slide kanan \n");
rizqicahyo 1:c9f11055fb12 274 }
Najib_irvani 2:2f7bed7fb055 275 else
Najib_irvani 2:2f7bed7fb055 276 {
Najib_irvani 2:2f7bed7fb055 277 pc.printf("slide diam \n");
Najib_irvani 2:2f7bed7fb055 278 }
Najib_irvani 2:2f7bed7fb055 279
Najib_irvani 3:d3708c3ed288 280 if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
Najib_irvani 2:2f7bed7fb055 281 //POWER WINDOW ATAS
Najib_irvani 3:d3708c3ed288 282 gripper.speed(1);
Najib_irvani 2:2f7bed7fb055 283 pc.printf("up \n");
Najib_irvani 2:2f7bed7fb055 284 }
Najib_irvani 2:2f7bed7fb055 285 else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
Najib_irvani 2:2f7bed7fb055 286 //POWER WINDOW BAWAH
Najib_irvani 2:2f7bed7fb055 287 gripper.speed(-0.2);
Najib_irvani 2:2f7bed7fb055 288 pc.printf("down \n");
Najib_irvani 2:2f7bed7fb055 289 }
Najib_irvani 2:2f7bed7fb055 290 else
Najib_irvani 2:2f7bed7fb055 291 {
Najib_irvani 2:2f7bed7fb055 292 gripper.brake(1);
Najib_irvani 2:2f7bed7fb055 293 pc.printf("power diam \n");
Najib_irvani 3:d3708c3ed288 294 }
Najib_irvani 2:2f7bed7fb055 295
Najib_irvani 2:2f7bed7fb055 296 if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
Najib_irvani 2:2f7bed7fb055 297 //PWM ++
Najib_irvani 2:2f7bed7fb055 298 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 299 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 300 }
Najib_irvani 2:2f7bed7fb055 301
Najib_irvani 2:2f7bed7fb055 302 if(ps2.read(PS_PAD::ANALOG_RY)>=100){
Najib_irvani 2:2f7bed7fb055 303 //PWM--
Najib_irvani 2:2f7bed7fb055 304 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 305 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 306 }
Najib_irvani 2:2f7bed7fb055 307
Najib_irvani 2:2f7bed7fb055 308 if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
Najib_irvani 2:2f7bed7fb055 309 //SERVO --
Najib_irvani 2:2f7bed7fb055 310 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 311 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 312 }
Najib_irvani 2:2f7bed7fb055 313
Najib_irvani 2:2f7bed7fb055 314 if(ps2.read(PS_PAD::ANALOG_RX)>=100){
Najib_irvani 2:2f7bed7fb055 315 //SERVO ++
Najib_irvani 2:2f7bed7fb055 316 sudut += 3;
Najib_irvani 2:2f7bed7fb055 317 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 318 }
Najib_irvani 3:d3708c3ed288 319
Najib_irvani 2:2f7bed7fb055 320 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 321 init_pwm();
Najib_irvani 2:2f7bed7fb055 322 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 323 wait_ms(25);
Najib_irvani 4:65d65a108b68 324 //edf.setThrottle(pwm);
Najib_irvani 4:65d65a108b68 325 //edf.pulse();
Najib_irvani 4:65d65a108b68 326 //wait_ms(25);
rizqicahyo 0:ac7353383a8e 327 }
rizqicahyo 0:ac7353383a8e 328 }