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@6:0e159860e5c6, 2016-02-19 (annotated)

Committer:
nibrosul_umam
Date:
Fri Feb 19 14:18:01 2016 +0000
Revision:
6:0e159860e5c6
Parent:
5:34be90fa6d27
Child:
7:4d6a73d924ff
oke

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
nibrosul_umam 6:0e159860e5c6 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
nibrosul_umam 6:0e159860e5c6 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);
nibrosul_umam 6:0e159860e5c6 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 /*********************************************************************************************/
nibrosul_umam 6:0e159860e5c6 115 void init_sensor()
nibrosul_umam 6:0e159860e5c6 116 {
nibrosul_umam 6:0e159860e5c6 117 if(ps2.read(PS_PAD::PAD_SQUARE)==1)
nibrosul_umam 6:0e159860e5c6 118 {
nibrosul_umam 6:0e159860e5c6 119 calibrate=0;
nibrosul_umam 6:0e159860e5c6 120 }
nibrosul_umam 6:0e159860e5c6 121 else
nibrosul_umam 6:0e159860e5c6 122 {
nibrosul_umam 6:0e159860e5c6 123 calibrate=1;
nibrosul_umam 6:0e159860e5c6 124 }
nibrosul_umam 6:0e159860e5c6 125
nibrosul_umam 6:0e159860e5c6 126 sensor[0]=(S1.read()==1);
nibrosul_umam 6:0e159860e5c6 127 sensor[1]=(S2.read()==1);
nibrosul_umam 6:0e159860e5c6 128 sensor[2]=(S3.read()==1);
nibrosul_umam 6:0e159860e5c6 129 sensor[3]=(S4.read()==1);
nibrosul_umam 6:0e159860e5c6 130 sensor[4]=(S5.read()==1);
nibrosul_umam 6:0e159860e5c6 131 sensor[5]=(S6.read()==1);
nibrosul_umam 6:0e159860e5c6 132 sensor[6]=(S7.read()==1);
nibrosul_umam 6:0e159860e5c6 133 sensor[7]=(S8.read()==1);
nibrosul_umam 6:0e159860e5c6 134 sensor[8]=(S9.read()==1);
nibrosul_umam 6:0e159860e5c6 135 sensor[9]=(S10.read()==1);
nibrosul_umam 6:0e159860e5c6 136 sensor[10]=(S11.read()==1);
nibrosul_umam 6:0e159860e5c6 137 sensor[11]=(S12.read()==1);
nibrosul_umam 6:0e159860e5c6 138 sensor[12]=(S13.read()==1);
nibrosul_umam 6:0e159860e5c6 139 }
rizqicahyo 0:ac7353383a8e 140
nibrosul_umam 6:0e159860e5c6 141 void PIDrunning() //menjalankan perintah untuk line follower
nibrosul_umam 6:0e159860e5c6 142 {
nibrosul_umam 6:0e159860e5c6 143 int pv;
nibrosul_umam 6:0e159860e5c6 144 int over=0;
nibrosul_umam 6:0e159860e5c6 145 float speedR,speedL;
nibrosul_umam 6:0e159860e5c6 146
nibrosul_umam 6:0e159860e5c6 147 //init_sensor();
nibrosul_umam 6:0e159860e5c6 148 //////////////////logic dari PV (present Value)/////////////////////////
nibrosul_umam 6:0e159860e5c6 149 if(sensor[0]){
nibrosul_umam 6:0e159860e5c6 150 pv = -12;
nibrosul_umam 6:0e159860e5c6 151 over=1;
nibrosul_umam 6:0e159860e5c6 152 }
nibrosul_umam 6:0e159860e5c6 153 else if(sensor[12]){
nibrosul_umam 6:0e159860e5c6 154 pv = 12;
nibrosul_umam 6:0e159860e5c6 155 over=2;
nibrosul_umam 6:0e159860e5c6 156 }
nibrosul_umam 6:0e159860e5c6 157 else if(sensor[0] && sensor[1]){
nibrosul_umam 6:0e159860e5c6 158 pv = -10;
nibrosul_umam 6:0e159860e5c6 159 }
nibrosul_umam 6:0e159860e5c6 160 else if(sensor[11] && sensor[12]){
nibrosul_umam 6:0e159860e5c6 161 pv = 10;
nibrosul_umam 6:0e159860e5c6 162 }
nibrosul_umam 6:0e159860e5c6 163 else if(sensor[1]){
nibrosul_umam 6:0e159860e5c6 164 pv = -9;
nibrosul_umam 6:0e159860e5c6 165 }
nibrosul_umam 6:0e159860e5c6 166 else if(sensor[11]){
nibrosul_umam 6:0e159860e5c6 167 pv = 9;
nibrosul_umam 6:0e159860e5c6 168 }
nibrosul_umam 6:0e159860e5c6 169 else if(sensor[1] && sensor[2]){
nibrosul_umam 6:0e159860e5c6 170 pv = -8;
nibrosul_umam 6:0e159860e5c6 171 }
nibrosul_umam 6:0e159860e5c6 172 else if(sensor[10] && sensor[11]){
nibrosul_umam 6:0e159860e5c6 173 pv = 8;
nibrosul_umam 6:0e159860e5c6 174 }
nibrosul_umam 6:0e159860e5c6 175 else if(sensor[2]){
nibrosul_umam 6:0e159860e5c6 176 pv = -7;
nibrosul_umam 6:0e159860e5c6 177 }
nibrosul_umam 6:0e159860e5c6 178 else if(sensor[10]){
nibrosul_umam 6:0e159860e5c6 179 pv = 7;
nibrosul_umam 6:0e159860e5c6 180 }
nibrosul_umam 6:0e159860e5c6 181 else if(sensor[2] && sensor[3]){
nibrosul_umam 6:0e159860e5c6 182 pv = -6;
nibrosul_umam 6:0e159860e5c6 183 }
nibrosul_umam 6:0e159860e5c6 184 else if(sensor[9] && sensor[10]){
nibrosul_umam 6:0e159860e5c6 185 pv = 6;
nibrosul_umam 6:0e159860e5c6 186 }
nibrosul_umam 6:0e159860e5c6 187 else if(sensor[3]){
nibrosul_umam 6:0e159860e5c6 188 pv = -5;
nibrosul_umam 6:0e159860e5c6 189 }
nibrosul_umam 6:0e159860e5c6 190 else if(sensor[9]){
nibrosul_umam 6:0e159860e5c6 191 pv = 5;
nibrosul_umam 6:0e159860e5c6 192 }
nibrosul_umam 6:0e159860e5c6 193 else if(sensor[3] && sensor[4]){
nibrosul_umam 6:0e159860e5c6 194 pv = -4;
nibrosul_umam 6:0e159860e5c6 195 }
nibrosul_umam 6:0e159860e5c6 196 else if(sensor[8] && sensor[9]){
nibrosul_umam 6:0e159860e5c6 197 pv = 4;
nibrosul_umam 6:0e159860e5c6 198 }
nibrosul_umam 6:0e159860e5c6 199 else if(sensor[4]){
nibrosul_umam 6:0e159860e5c6 200 pv = -3;
nibrosul_umam 6:0e159860e5c6 201 }
nibrosul_umam 6:0e159860e5c6 202 else if(sensor[8]){
nibrosul_umam 6:0e159860e5c6 203 pv = 3;
nibrosul_umam 6:0e159860e5c6 204 }
nibrosul_umam 6:0e159860e5c6 205 else if(sensor[4] && sensor[5]){
nibrosul_umam 6:0e159860e5c6 206 pv = -2;
nibrosul_umam 6:0e159860e5c6 207 }
nibrosul_umam 6:0e159860e5c6 208 else if(sensor[7] && sensor[8]){
nibrosul_umam 6:0e159860e5c6 209 pv = 2;
nibrosul_umam 6:0e159860e5c6 210 }
nibrosul_umam 6:0e159860e5c6 211 else if(sensor[5]){
nibrosul_umam 6:0e159860e5c6 212 pv = -1;
nibrosul_umam 6:0e159860e5c6 213 }
nibrosul_umam 6:0e159860e5c6 214 else if(sensor[7]){
nibrosul_umam 6:0e159860e5c6 215 pv = 1;
nibrosul_umam 6:0e159860e5c6 216 }
nibrosul_umam 6:0e159860e5c6 217 else if(sensor[5] && sensor[6]){
nibrosul_umam 6:0e159860e5c6 218 pv = -0.5;
nibrosul_umam 6:0e159860e5c6 219 }
nibrosul_umam 6:0e159860e5c6 220 else if(sensor[6] && sensor[7]){
nibrosul_umam 6:0e159860e5c6 221 pv = 0.5;
nibrosul_umam 6:0e159860e5c6 222 }
nibrosul_umam 6:0e159860e5c6 223 else if (sensor[6]){
nibrosul_umam 6:0e159860e5c6 224 pv = 0;
nibrosul_umam 6:0e159860e5c6 225 }
nibrosul_umam 6:0e159860e5c6 226 ///////////////// robot bergerak keluar dari sensor/////////////////////
nibrosul_umam 6:0e159860e5c6 227 if(over==1){
nibrosul_umam 6:0e159860e5c6 228 /*if(speed_ka > 0){
nibrosul_umam 6:0e159860e5c6 229 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 230 motor2.speed(-speed_ka);
nibrosul_umam 6:0e159860e5c6 231 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 232 }
nibrosul_umam 6:0e159860e5c6 233 else{
nibrosul_umam 6:0e159860e5c6 234 motor2.speed(speed_ka);
nibrosul_umam 6:0e159860e5c6 235 }
nibrosul_umam 6:0e159860e5c6 236 wait_ms(10);*/
nibrosul_umam 6:0e159860e5c6 237
nibrosul_umam 6:0e159860e5c6 238 motor1.brake(1);
nibrosul_umam 6:0e159860e5c6 239 //wait_ms(100);
nibrosul_umam 6:0e159860e5c6 240
nibrosul_umam 6:0e159860e5c6 241 }
nibrosul_umam 6:0e159860e5c6 242 else if(over==2){
nibrosul_umam 6:0e159860e5c6 243 /*if(speed_ki > 0){
nibrosul_umam 6:0e159860e5c6 244 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 245 motor1.speed(-speed_ki);
nibrosul_umam 6:0e159860e5c6 246 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 247 }
nibrosul_umam 6:0e159860e5c6 248 else{
nibrosul_umam 6:0e159860e5c6 249 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 250 motor1.speed(speed_ki);
nibrosul_umam 6:0e159860e5c6 251 wait_ms(10);
nibrosul_umam 6:0e159860e5c6 252 }
nibrosul_umam 6:0e159860e5c6 253 wait_ms(10);*/
nibrosul_umam 6:0e159860e5c6 254 motor2.brake(1);
nibrosul_umam 6:0e159860e5c6 255 //wait_ms(100);
nibrosul_umam 6:0e159860e5c6 256 }
nibrosul_umam 6:0e159860e5c6 257
nibrosul_umam 6:0e159860e5c6 258 PID.setProcessValue(pv);
nibrosul_umam 6:0e159860e5c6 259 PID.setSetPoint(0);
nibrosul_umam 6:0e159860e5c6 260
nibrosul_umam 6:0e159860e5c6 261 // memulai perhitungan PID
nibrosul_umam 6:0e159860e5c6 262
nibrosul_umam 6:0e159860e5c6 263 speedR = gMax_speed - PID.compute();
nibrosul_umam 6:0e159860e5c6 264 if(speedR > gMax_speed){
nibrosul_umam 6:0e159860e5c6 265 speedR = gMax_speed;
nibrosul_umam 6:0e159860e5c6 266 }
nibrosul_umam 6:0e159860e5c6 267 else if(speedR < gMin_speed)
nibrosul_umam 6:0e159860e5c6 268 speedR = gMin_speed;
nibrosul_umam 6:0e159860e5c6 269 motor2.speed(speedR);
nibrosul_umam 6:0e159860e5c6 270
nibrosul_umam 6:0e159860e5c6 271 speedL = gMax_speed + PID.compute();
nibrosul_umam 6:0e159860e5c6 272 if(speedL > gMax_speed)
nibrosul_umam 6:0e159860e5c6 273 speedL = gMax_speed;
nibrosul_umam 6:0e159860e5c6 274 else if(speedL < gMin_speed)
nibrosul_umam 6:0e159860e5c6 275 speedL = gMin_speed;
nibrosul_umam 6:0e159860e5c6 276 motor1.speed(speedL);
nibrosul_umam 6:0e159860e5c6 277 }
rizqicahyo 0:ac7353383a8e 278
rizqicahyo 0:ac7353383a8e 279 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 280 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 281 /** PROGRAM UTAMA **/
rizqicahyo 0:ac7353383a8e 282 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 283 /*********************************************************************************************/
Najib_irvani 2:2f7bed7fb055 284
Najib_irvani 5:34be90fa6d27 285
Najib_irvani 3:d3708c3ed288 286 void ambil(int c){
Najib_irvani 3:d3708c3ed288 287 switch (c)
Najib_irvani 3:d3708c3ed288 288 {
Najib_irvani 3:d3708c3ed288 289 case 1:
Najib_irvani 4:65d65a108b68 290 servo_gripper.position(170);
Najib_irvani 4:65d65a108b68 291 pc.printf("grip lost");
Najib_irvani 4:65d65a108b68 292 wait_ms(25);
Najib_irvani 5:34be90fa6d27 293 pnuematik_lengan=0;
Najib_irvani 3:d3708c3ed288 294 wait_ms(500);
Najib_irvani 4:65d65a108b68 295
Najib_irvani 3:d3708c3ed288 296 break;
Najib_irvani 3:d3708c3ed288 297 case 2:
Najib_irvani 4:65d65a108b68 298 pnuematik_gripper=0;
Najib_irvani 4:65d65a108b68 299 wait_ms(600);
Najib_irvani 4:65d65a108b68 300 servo_gripper.position(50);
Najib_irvani 4:65d65a108b68 301 pc.printf("grip get");
Najib_irvani 4:65d65a108b68 302 wait_ms(25);
Najib_irvani 3:d3708c3ed288 303 wait_ms(1000);
Najib_irvani 3:d3708c3ed288 304 pnuematik_gripper=1;
Najib_irvani 3:d3708c3ed288 305 wait_ms(800);
Najib_irvani 5:34be90fa6d27 306 pnuematik_lengan=1;
Najib_irvani 3:d3708c3ed288 307 wait_ms(200);
Najib_irvani 3:d3708c3ed288 308 break;
Najib_irvani 3:d3708c3ed288 309 }
Najib_irvani 3:d3708c3ed288 310 }
Najib_irvani 3:d3708c3ed288 311
Najib_irvani 2:2f7bed7fb055 312 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 313 if (i){
Najib_irvani 2:2f7bed7fb055 314 if (sudut>90){
Najib_irvani 2:2f7bed7fb055 315 sudut=90;
Najib_irvani 2:2f7bed7fb055 316 }
Najib_irvani 2:2f7bed7fb055 317 if (sudut<0){
Najib_irvani 2:2f7bed7fb055 318 sudut=0;
Najib_irvani 2:2f7bed7fb055 319 }
Najib_irvani 2:2f7bed7fb055 320 } else {
Najib_irvani 2:2f7bed7fb055 321
Najib_irvani 2:2f7bed7fb055 322 if (sudut>0){
Najib_irvani 2:2f7bed7fb055 323 sudut=0;
Najib_irvani 2:2f7bed7fb055 324 }
Najib_irvani 2:2f7bed7fb055 325 if (sudut<-90){
Najib_irvani 2:2f7bed7fb055 326 sudut=-90;
Najib_irvani 2:2f7bed7fb055 327 }
Najib_irvani 2:2f7bed7fb055 328 }
Najib_irvani 2:2f7bed7fb055 329 }
Najib_irvani 2:2f7bed7fb055 330
Najib_irvani 2:2f7bed7fb055 331 void init_pwm (){
Najib_irvani 2:2f7bed7fb055 332 if (pwm>max){
Najib_irvani 2:2f7bed7fb055 333 pwm = max;
Najib_irvani 2:2f7bed7fb055 334 }
Najib_irvani 2:2f7bed7fb055 335
Najib_irvani 2:2f7bed7fb055 336 if (pwm<min){
Najib_irvani 2:2f7bed7fb055 337 pwm = min;
Najib_irvani 2:2f7bed7fb055 338 }
Najib_irvani 2:2f7bed7fb055 339 }
Najib_irvani 2:2f7bed7fb055 340
Najib_irvani 3:d3708c3ed288 341 int count=1;
Najib_irvani 3:d3708c3ed288 342
rizqicahyo 0:ac7353383a8e 343 int main(void){
rizqicahyo 0:ac7353383a8e 344 //inisialisasi joystick
rizqicahyo 0:ac7353383a8e 345 ps2.init();
rizqicahyo 0:ac7353383a8e 346
nibrosul_umam 6:0e159860e5c6 347 //inisialisasi PID
nibrosul_umam 6:0e159860e5c6 348 PID.setInputLimits(-15,15);
nibrosul_umam 6:0e159860e5c6 349 PID.setOutputLimits(-1.0,1.0);
nibrosul_umam 6:0e159860e5c6 350 PID.setMode(1.0);
nibrosul_umam 6:0e159860e5c6 351 PID.setBias(0.0);
nibrosul_umam 6:0e159860e5c6 352 PID.reset();
nibrosul_umam 6:0e159860e5c6 353
nibrosul_umam 6:0e159860e5c6 354
Najib_irvani 2:2f7bed7fb055 355 //tambahan power
Najib_irvani 4:65d65a108b68 356 //ESC edf(PB_9,20); //p wm esc pin PC_7
Najib_irvani 2:2f7bed7fb055 357 Servo myservo(PB_8); //pwm servo pin PA_8
Najib_irvani 2:2f7bed7fb055 358 //set inisialisasi servo pada posisi 0
Najib_irvani 2:2f7bed7fb055 359 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 360 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
Najib_irvani 4:65d65a108b68 361 //edf.setThrottle(pwm);
Najib_irvani 4:65d65a108b68 362 //edf.pulse();
rizqicahyo 0:ac7353383a8e 363
rizqicahyo 0:ac7353383a8e 364 pc.baud(115200);
Najib_irvani 2:2f7bed7fb055 365 float speed;
rizqicahyo 0:ac7353383a8e 366
Najib_irvani 3:d3708c3ed288 367 servo_gripper.position(140);
Najib_irvani 5:34be90fa6d27 368 pnuematik_lengan=1;
Najib_irvani 4:65d65a108b68 369 pnuematik_gripper=1;
Najib_irvani 5:34be90fa6d27 370 pnuematik_atas = state_atas;
Najib_irvani 5:34be90fa6d27 371 pnuematik_bawah = state_bawah;
Najib_irvani 3:d3708c3ed288 372
rizqicahyo 0:ac7353383a8e 373 while(1)
rizqicahyo 0:ac7353383a8e 374 {
Najib_irvani 2:2f7bed7fb055 375 ps2.poll();
Najib_irvani 2:2f7bed7fb055 376
Najib_irvani 2:2f7bed7fb055 377 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
Najib_irvani 2:2f7bed7fb055 378 //MOTOR DEPAN
Najib_irvani 2:2f7bed7fb055 379 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 380
Najib_irvani 3:d3708c3ed288 381 motor1.speed(speed-gTuning);
Najib_irvani 2:2f7bed7fb055 382 motor2.speed(speed);
Najib_irvani 2:2f7bed7fb055 383 pc.printf("maju \n");
Najib_irvani 2:2f7bed7fb055 384 }
nibrosul_umam 6:0e159860e5c6 385 else if((ps2.read(PS_PAD::PAD_X)==1)&&(ps2.read(PS_PAD::PAD_TOP)==1)){
nibrosul_umam 6:0e159860e5c6 386 PIDrunning();
nibrosul_umam 6:0e159860e5c6 387 pc.printf("PID \t %f \t ",PID.compute());
nibrosul_umam 6:0e159860e5c6 388
nibrosul_umam 6:0e159860e5c6 389 for(i=0;i<13;i++)
nibrosul_umam 6:0e159860e5c6 390 {
nibrosul_umam 6:0e159860e5c6 391 pc.printf("%i \t",sensor[i]);
nibrosul_umam 6:0e159860e5c6 392 }
nibrosul_umam 6:0e159860e5c6 393 pc.printf("\n");
nibrosul_umam 6:0e159860e5c6 394 }
Najib_irvani 2:2f7bed7fb055 395 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 396 //MOTOR BELAKANG
Najib_irvani 2:2f7bed7fb055 397 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 398
Najib_irvani 5:34be90fa6d27 399 motor1.speed(-speed);
Najib_irvani 2:2f7bed7fb055 400 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 401 pc.printf("mundur \n");
Najib_irvani 2:2f7bed7fb055 402 }
Najib_irvani 2:2f7bed7fb055 403 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 404 //pivot kiri
Najib_irvani 2:2f7bed7fb055 405 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 406
Najib_irvani 3:d3708c3ed288 407 motor1.speed(-(speed*0.9-gTuning));
Najib_irvani 3:d3708c3ed288 408 motor2.speed(speed*0.9);
Najib_irvani 2:2f7bed7fb055 409 pc.printf("kiri \n");
Najib_irvani 2:2f7bed7fb055 410 }
Najib_irvani 2:2f7bed7fb055 411 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 412 //pivot kanan
Najib_irvani 2:2f7bed7fb055 413 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 414
Najib_irvani 3:d3708c3ed288 415 motor1.speed(speed*0.9-gTuning);
Najib_irvani 3:d3708c3ed288 416 motor2.speed(-speed*0.9 );
Najib_irvani 2:2f7bed7fb055 417 pc.printf("kanan \n");
Najib_irvani 2:2f7bed7fb055 418 }
Najib_irvani 3:d3708c3ed288 419 else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
Najib_irvani 3:d3708c3ed288 420 {
Najib_irvani 4:65d65a108b68 421 //mekanisme ambil gripper
Najib_irvani 4:65d65a108b68 422 pc.printf("mekanisme gripper");
Najib_irvani 3:d3708c3ed288 423 if (count==1){
Najib_irvani 4:65d65a108b68 424 pc.printf("ambil 1");
Najib_irvani 3:d3708c3ed288 425 ambil(1);
Najib_irvani 3:d3708c3ed288 426 count=2;
Najib_irvani 3:d3708c3ed288 427 wait_ms(400);
Najib_irvani 3:d3708c3ed288 428 }
Najib_irvani 3:d3708c3ed288 429 else
Najib_irvani 3:d3708c3ed288 430 {
Najib_irvani 3:d3708c3ed288 431 ambil(2);
Najib_irvani 3:d3708c3ed288 432 count=1;
Najib_irvani 4:65d65a108b68 433 pc.printf("ambil 2");
Najib_irvani 3:d3708c3ed288 434 wait_ms(400);
Najib_irvani 3:d3708c3ed288 435 }
Najib_irvani 3:d3708c3ed288 436 }
Najib_irvani 2:2f7bed7fb055 437 else
rizqicahyo 1:c9f11055fb12 438 {
Najib_irvani 2:2f7bed7fb055 439 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 440 motor2.brake(1);
Najib_irvani 2:2f7bed7fb055 441 }
Najib_irvani 2:2f7bed7fb055 442
Najib_irvani 2:2f7bed7fb055 443 if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
Najib_irvani 2:2f7bed7fb055 444 //SLIDER KIRI
Najib_irvani 2:2f7bed7fb055 445 pc.printf("slide kiri \n");
Najib_irvani 2:2f7bed7fb055 446 }
Najib_irvani 2:2f7bed7fb055 447 else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
Najib_irvani 2:2f7bed7fb055 448 //SLIDER KANAN
Najib_irvani 2:2f7bed7fb055 449 pc.printf("slide kanan \n");
rizqicahyo 1:c9f11055fb12 450 }
Najib_irvani 2:2f7bed7fb055 451 else
Najib_irvani 2:2f7bed7fb055 452 {
Najib_irvani 2:2f7bed7fb055 453 pc.printf("slide diam \n");
Najib_irvani 2:2f7bed7fb055 454 }
Najib_irvani 2:2f7bed7fb055 455
Najib_irvani 3:d3708c3ed288 456 if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
Najib_irvani 2:2f7bed7fb055 457 //POWER WINDOW ATAS
Najib_irvani 5:34be90fa6d27 458
Najib_irvani 5:34be90fa6d27 459 gripper.speed(gripper_up);
Najib_irvani 2:2f7bed7fb055 460 pc.printf("up \n");
Najib_irvani 2:2f7bed7fb055 461 }
Najib_irvani 2:2f7bed7fb055 462 else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
Najib_irvani 5:34be90fa6d27 463 //POWER WINDOW BAWAH
Najib_irvani 5:34be90fa6d27 464 if ((state_atas==1) || (state_bawah==1))
Najib_irvani 5:34be90fa6d27 465 {
Najib_irvani 5:34be90fa6d27 466 gripper_down = 1;
Najib_irvani 5:34be90fa6d27 467 }
Najib_irvani 5:34be90fa6d27 468 else
Najib_irvani 5:34be90fa6d27 469 {
Najib_irvani 5:34be90fa6d27 470 gripper_down = 0.2;
Najib_irvani 5:34be90fa6d27 471 }
Najib_irvani 5:34be90fa6d27 472
Najib_irvani 5:34be90fa6d27 473 gripper.speed(-gripper_down);
Najib_irvani 2:2f7bed7fb055 474 pc.printf("down \n");
Najib_irvani 2:2f7bed7fb055 475 }
Najib_irvani 2:2f7bed7fb055 476 else
Najib_irvani 2:2f7bed7fb055 477 {
Najib_irvani 2:2f7bed7fb055 478 gripper.brake(1);
Najib_irvani 2:2f7bed7fb055 479 pc.printf("power diam \n");
Najib_irvani 3:d3708c3ed288 480 }
Najib_irvani 2:2f7bed7fb055 481
Najib_irvani 2:2f7bed7fb055 482 if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
Najib_irvani 2:2f7bed7fb055 483 //PWM ++
Najib_irvani 2:2f7bed7fb055 484 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 485 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 486 }
Najib_irvani 2:2f7bed7fb055 487
Najib_irvani 2:2f7bed7fb055 488 if(ps2.read(PS_PAD::ANALOG_RY)>=100){
Najib_irvani 2:2f7bed7fb055 489 //PWM--
Najib_irvani 2:2f7bed7fb055 490 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 491 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 492 }
Najib_irvani 2:2f7bed7fb055 493
Najib_irvani 2:2f7bed7fb055 494 if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
Najib_irvani 2:2f7bed7fb055 495 //SERVO --
Najib_irvani 2:2f7bed7fb055 496 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 497 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 498 }
Najib_irvani 2:2f7bed7fb055 499
Najib_irvani 2:2f7bed7fb055 500 if(ps2.read(PS_PAD::ANALOG_RX)>=100){
Najib_irvani 2:2f7bed7fb055 501 //SERVO ++
Najib_irvani 2:2f7bed7fb055 502 sudut += 3;
Najib_irvani 2:2f7bed7fb055 503 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 504 }
Najib_irvani 5:34be90fa6d27 505
Najib_irvani 5:34be90fa6d27 506 //gripper pole
Najib_irvani 5:34be90fa6d27 507
Najib_irvani 5:34be90fa6d27 508 if ((ps2.read(PS_PAD::PAD_L2)==1))
Najib_irvani 5:34be90fa6d27 509 {
Najib_irvani 3:d3708c3ed288 510
Najib_irvani 5:34be90fa6d27 511 state_atas = !state_atas;
Najib_irvani 5:34be90fa6d27 512 pnuematik_atas= state_atas;
Najib_irvani 5:34be90fa6d27 513 wait_ms(300);
Najib_irvani 5:34be90fa6d27 514 }
Najib_irvani 5:34be90fa6d27 515 else if ((ps2.read(PS_PAD::PAD_R2)==1))
Najib_irvani 5:34be90fa6d27 516 {
Najib_irvani 5:34be90fa6d27 517 state_bawah = !state_bawah;
Najib_irvani 5:34be90fa6d27 518 pnuematik_bawah = state_bawah;
Najib_irvani 5:34be90fa6d27 519 wait_ms(300);
Najib_irvani 5:34be90fa6d27 520 }
Najib_irvani 5:34be90fa6d27 521
Najib_irvani 5:34be90fa6d27 522
Najib_irvani 2:2f7bed7fb055 523 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 524 init_pwm();
Najib_irvani 2:2f7bed7fb055 525 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 526 wait_ms(25);
Najib_irvani 4:65d65a108b68 527 //edf.setThrottle(pwm);
Najib_irvani 4:65d65a108b68 528 //edf.pulse();
Najib_irvani 5:34be90fa6d27 529 //wait_ms(25);
rizqicahyo 0:ac7353383a8e 530 }
rizqicahyo 0:ac7353383a8e 531 }