KRAI 2016 / Mbed 2 deprecated Base_Hybrid_V2

base hybrid board baru

Dependencies:   Motor NewTextLCD PID PS_PAD mbed millis ESC base_rekam_darurat

main.cpp@5:34be90fa6d27, 2016-02-19 (annotated)

Committer:
Najib_irvani
Date:
Fri Feb 19 12:49:34 2016 +0000
Revision:
5:34be90fa6d27
Parent:
4:65d65a108b68 
base darurat V1

Who changed what in which revision?

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