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@9:226d1137453e, 2016-04-18 (annotated)

Committer:
rizqicahyo
Date:
Mon Apr 18 05:14:51 2016 +0000
Revision:
9:226d1137453e
Parent:
8:3cc68df2bebf 
jajal

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"
Najib_irvani 8:3cc68df2bebf 12 #include "Ping.h"
rizqicahyo 0:ac7353383a8e 13
Najib_irvani 3:d3708c3ed288 14
nibrosul_umam 7:4d6a73d924ff 15
rizqicahyo 0:ac7353383a8e 16 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 17 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:ac7353383a8e 18 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 19 // serial pc
rizqicahyo 0:ac7353383a8e 20 Serial pc(USBTX,USBRX);
rizqicahyo 9:226d1137453e 21 Serial sensor(PA_0,PA_1);
rizqicahyo 0:ac7353383a8e 22
Najib_irvani 8:3cc68df2bebf 23 // joystick PS2
Najib_irvani 8:3cc68df2bebf 24 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12
rizqicahyo 9:226d1137453e 25
rizqicahyo 9:226d1137453e 26 // PID sensor garis
rizqicahyo 9:226d1137453e 27 PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rizqicahyo 9:226d1137453e 28
Najib_irvani 8:3cc68df2bebf 29 Motor gripper(PA_10, PB_3, PB_5); //PB_6, PB_8, PB_9
Najib_irvani 8:3cc68df2bebf 30 //Motor slider(PC_6, PC_9, PC_8);
Najib_irvani 8:3cc68df2bebf 31 Motor motor2(PA_11, PB_12, PA_7); //kanan
Najib_irvani 8:3cc68df2bebf 32 Motor motor1(PA_8, PB_4, PB_1); //kiri
Najib_irvani 8:3cc68df2bebf 33
Najib_irvani 8:3cc68df2bebf 34 DigitalOut limit0(PC_0,PullUp);
Najib_irvani 8:3cc68df2bebf 35 DigitalOut limit1(PC_1,PullUp);
rizqicahyo 0:ac7353383a8e 36
rizqicahyo 9:226d1137453e 37 DigitalOut pnuematik_lengan(PC_12);
rizqicahyo 9:226d1137453e 38 DigitalOut pnuematik_gripper(PC_11);
rizqicahyo 9:226d1137453e 39
rizqicahyo 9:226d1137453e 40 ESC edf(PC_6,20); //pwm esc PB_8
rizqicahyo 9:226d1137453e 41 Servo myservo(PC_8); //pwm servo PB_9
rizqicahyo 9:226d1137453e 42
rizqicahyo 9:226d1137453e 43 Timer timer;
rizqicahyo 0:ac7353383a8e 44
rizqicahyo 0:ac7353383a8e 45 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 46 /** DEKLARASI VARIABEL GLOBAL **/
rizqicahyo 0:ac7353383a8e 47 /*********************************************************************************************/
Najib_irvani 8:3cc68df2bebf 48 float gMax_speed=0.83; //nilai maksimum kecepatan motor
rizqicahyo 0:ac7353383a8e 49 float gMin_speed=-0.05; //nilai minimum kecepatan motor
Najib_irvani 8:3cc68df2bebf 50 float gTuning = 0.14;
rizqicahyo 0:ac7353383a8e 51
nibrosul_umam 7:4d6a73d924ff 52 // tambahan power
nibrosul_umam 7:4d6a73d924ff 53 // inisialisasi pwm awal servo
nibrosul_umam 7:4d6a73d924ff 54 float pwm = 0.00;
nibrosul_umam 7:4d6a73d924ff 55
nibrosul_umam 7:4d6a73d924ff 56 // inisialisasi sudut awal
nibrosul_umam 7:4d6a73d924ff 57 int sudut = 0;
nibrosul_umam 7:4d6a73d924ff 58 //membatasi nilai brushless pada edf
nibrosul_umam 7:4d6a73d924ff 59 float min=0;
nibrosul_umam 7:4d6a73d924ff 60 float max=0.70;
rizqicahyo 0:ac7353383a8e 61
rizqicahyo 0:ac7353383a8e 62 unsigned char i; // variabel iterasi
rizqicahyo 1:c9f11055fb12 63 int over=0;
Najib_irvani 8:3cc68df2bebf 64 int lapangan = 0;
rizqicahyo 0:ac7353383a8e 65
nibrosul_umam 7:4d6a73d924ff 66 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
nibrosul_umam 7:4d6a73d924ff 67 unsigned char gCase=0; //variabel keadaan proses
nibrosul_umam 7:4d6a73d924ff 68
rizqicahyo 9:226d1137453e 69 int c=0; //motor gripper auto
rizqicahyo 9:226d1137453e 70 int g=2; //pnuematik kondisi gripper
rizqicahyo 9:226d1137453e 71 int count=1;
nibrosul_umam 7:4d6a73d924ff 72
rizqicahyo 9:226d1137453e 73 float k;
rizqicahyo 9:226d1137453e 74 float speed;
rizqicahyo 9:226d1137453e 75
rizqicahyo 9:226d1137453e 76 int datasensor[6];
rizqicahyo 9:226d1137453e 77
rizqicahyo 0:ac7353383a8e 78 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 79 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 0:ac7353383a8e 80 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 81
rizqicahyo 0:ac7353383a8e 82 /*********************************************************************************************/
Najib_irvani 2:2f7bed7fb055 83 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 84 if (i){
Najib_irvani 8:3cc68df2bebf 85 if (sudut>60){
Najib_irvani 8:3cc68df2bebf 86 sudut=60;
Najib_irvani 2:2f7bed7fb055 87 }
Najib_irvani 8:3cc68df2bebf 88 if (sudut<-60){
Najib_irvani 8:3cc68df2bebf 89 sudut=-60;
Najib_irvani 2:2f7bed7fb055 90 }
Najib_irvani 2:2f7bed7fb055 91 } else {
Najib_irvani 2:2f7bed7fb055 92
Najib_irvani 8:3cc68df2bebf 93 if (sudut>60){
Najib_irvani 8:3cc68df2bebf 94 sudut=60;
Najib_irvani 2:2f7bed7fb055 95 }
Najib_irvani 8:3cc68df2bebf 96 if (sudut<-60){
Najib_irvani 8:3cc68df2bebf 97 sudut=-60;
nibrosul_umam 7:4d6a73d924ff 98 }
Najib_irvani 2:2f7bed7fb055 99 }
Najib_irvani 2:2f7bed7fb055 100 }
rizqicahyo 9:226d1137453e 101
Najib_irvani 2:2f7bed7fb055 102 void init_pwm (){
Najib_irvani 2:2f7bed7fb055 103 if (pwm>max){
Najib_irvani 2:2f7bed7fb055 104 pwm = max;
Najib_irvani 2:2f7bed7fb055 105 }
Najib_irvani 2:2f7bed7fb055 106
Najib_irvani 2:2f7bed7fb055 107 if (pwm<min){
Najib_irvani 2:2f7bed7fb055 108 pwm = min;
Najib_irvani 2:2f7bed7fb055 109 }
Najib_irvani 2:2f7bed7fb055 110 }
Najib_irvani 3:d3708c3ed288 111
Najib_irvani 8:3cc68df2bebf 112
rizqicahyo 9:226d1137453e 113 void motor_base(void){
rizqicahyo 9:226d1137453e 114 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
nibrosul_umam 7:4d6a73d924ff 115 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 116 motor1.brake(0.2);
nibrosul_umam 7:4d6a73d924ff 117 motor2.speed(speed-0.05);
nibrosul_umam 7:4d6a73d924ff 118 pc.printf("maju serong kiri\n");
nibrosul_umam 7:4d6a73d924ff 119
nibrosul_umam 7:4d6a73d924ff 120 }
nibrosul_umam 7:4d6a73d924ff 121 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
nibrosul_umam 7:4d6a73d924ff 122 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 123 motor1.speed(speed-gTuning-0.05);
Najib_irvani 8:3cc68df2bebf 124 motor2.brake(0.2);
nibrosul_umam 7:4d6a73d924ff 125 pc.printf("maju serong kanan\n");
nibrosul_umam 7:4d6a73d924ff 126
nibrosul_umam 7:4d6a73d924ff 127 }
nibrosul_umam 7:4d6a73d924ff 128 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
nibrosul_umam 7:4d6a73d924ff 129 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 130 motor2.brake(1);
nibrosul_umam 7:4d6a73d924ff 131 motor1.speed(-(speed-gTuning-0.2));
nibrosul_umam 7:4d6a73d924ff 132 pc.printf("mundur serong kanan\n");
nibrosul_umam 7:4d6a73d924ff 133
Najib_irvani 2:2f7bed7fb055 134 }
nibrosul_umam 7:4d6a73d924ff 135 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
nibrosul_umam 7:4d6a73d924ff 136 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 137 motor2.speed(-(speed-0.2));
nibrosul_umam 7:4d6a73d924ff 138 motor1.brake(1);
nibrosul_umam 7:4d6a73d924ff 139 pc.printf("mundur serong kiri\n");
nibrosul_umam 7:4d6a73d924ff 140
nibrosul_umam 7:4d6a73d924ff 141 }
nibrosul_umam 7:4d6a73d924ff 142 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
nibrosul_umam 7:4d6a73d924ff 143 //MOTOR DEPAN
nibrosul_umam 7:4d6a73d924ff 144
nibrosul_umam 7:4d6a73d924ff 145 speed = k;
nibrosul_umam 7:4d6a73d924ff 146
nibrosul_umam 7:4d6a73d924ff 147 if (k >= gMax_speed){
nibrosul_umam 7:4d6a73d924ff 148 k = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 149 }
nibrosul_umam 7:4d6a73d924ff 150
nibrosul_umam 7:4d6a73d924ff 151 motor1.speed(speed-gTuning);
nibrosul_umam 7:4d6a73d924ff 152 motor2.speed(speed);
nibrosul_umam 7:4d6a73d924ff 153 pc.printf("maju \n");
nibrosul_umam 7:4d6a73d924ff 154
nibrosul_umam 7:4d6a73d924ff 155 k += 0.1;
nibrosul_umam 7:4d6a73d924ff 156 }
Najib_irvani 2:2f7bed7fb055 157 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 158 //MOTOR BELAKANG
nibrosul_umam 7:4d6a73d924ff 159 speed = k;
nibrosul_umam 7:4d6a73d924ff 160
nibrosul_umam 7:4d6a73d924ff 161 if (k >= gMax_speed){
nibrosul_umam 7:4d6a73d924ff 162 k = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 163 }
nibrosul_umam 7:4d6a73d924ff 164
Najib_irvani 5:34be90fa6d27 165 motor1.speed(-speed);
Najib_irvani 2:2f7bed7fb055 166 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 167 pc.printf("mundur \n");
nibrosul_umam 7:4d6a73d924ff 168
nibrosul_umam 7:4d6a73d924ff 169 k += 0.1;
Najib_irvani 2:2f7bed7fb055 170 }
Najib_irvani 2:2f7bed7fb055 171 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 172 //pivot kiri
nibrosul_umam 7:4d6a73d924ff 173 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 174 motor1.speed(-(speed*0.95-gTuning));
Najib_irvani 8:3cc68df2bebf 175 motor2.speed(speed*0.95);
nibrosul_umam 7:4d6a73d924ff 176 pc.printf("pivot kiri \n");
Najib_irvani 2:2f7bed7fb055 177 }
Najib_irvani 2:2f7bed7fb055 178 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 179 //pivot kanan
nibrosul_umam 7:4d6a73d924ff 180 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 181 motor1.speed(speed*0.95-gTuning);
Najib_irvani 8:3cc68df2bebf 182 motor2.speed(-speed*0.95 );
nibrosul_umam 7:4d6a73d924ff 183 pc.printf("pivot kanan \n");
nibrosul_umam 7:4d6a73d924ff 184
Najib_irvani 2:2f7bed7fb055 185 }
nibrosul_umam 7:4d6a73d924ff 186 else{
Najib_irvani 2:2f7bed7fb055 187 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 188 motor2.brake(1);
nibrosul_umam 7:4d6a73d924ff 189
nibrosul_umam 7:4d6a73d924ff 190 k = 0.6;
Najib_irvani 2:2f7bed7fb055 191 }
rizqicahyo 9:226d1137453e 192 }
rizqicahyo 9:226d1137453e 193
rizqicahyo 9:226d1137453e 194 void motor_gripper(){
nibrosul_umam 7:4d6a73d924ff 195 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
Najib_irvani 2:2f7bed7fb055 196 //POWER WINDOW ATAS
rizqicahyo 9:226d1137453e 197 /* do{
rizqicahyo 9:226d1137453e 198 motor_base();
rizqicahyo 9:226d1137453e 199 gripper.speed(1);
rizqicahyo 9:226d1137453e 200 }while((limit0 != 0) && (c > 7));
rizqicahyo 9:226d1137453e 201 */
rizqicahyo 9:226d1137453e 202 gripper.speed(0.9);
Najib_irvani 5:34be90fa6d27 203
Najib_irvani 8:3cc68df2bebf 204 if (limit0 == 0){
Najib_irvani 8:3cc68df2bebf 205 gripper.brake(1);
Najib_irvani 8:3cc68df2bebf 206 }
rizqicahyo 9:226d1137453e 207
rizqicahyo 9:226d1137453e 208
Najib_irvani 2:2f7bed7fb055 209 pc.printf("up \n");
rizqicahyo 9:226d1137453e 210 c++;
Najib_irvani 2:2f7bed7fb055 211 }
nibrosul_umam 7:4d6a73d924ff 212 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
Najib_irvani 5:34be90fa6d27 213 //POWER WINDOW BAWAH
rizqicahyo 9:226d1137453e 214 /*do{
rizqicahyo 9:226d1137453e 215 motor_base();
rizqicahyo 9:226d1137453e 216 gripper.speed(-0.8);
rizqicahyo 9:226d1137453e 217 }while((limit1 != 0) && (c > 7));
rizqicahyo 9:226d1137453e 218 */
Najib_irvani 5:34be90fa6d27 219
rizqicahyo 9:226d1137453e 220 gripper.speed(-0.7);
Najib_irvani 8:3cc68df2bebf 221
Najib_irvani 8:3cc68df2bebf 222 if (limit1 ==0){
Najib_irvani 8:3cc68df2bebf 223 gripper.brake(1);
Najib_irvani 8:3cc68df2bebf 224 }
Najib_irvani 8:3cc68df2bebf 225
Najib_irvani 2:2f7bed7fb055 226 pc.printf("down \n");
rizqicahyo 9:226d1137453e 227 c--;
Najib_irvani 2:2f7bed7fb055 228 }
nibrosul_umam 7:4d6a73d924ff 229 else{
Najib_irvani 2:2f7bed7fb055 230 gripper.brake(1);
rizqicahyo 9:226d1137453e 231 if ((c <= 7) && (c>=-7)){
rizqicahyo 9:226d1137453e 232 c=0;
rizqicahyo 9:226d1137453e 233 }
nibrosul_umam 7:4d6a73d924ff 234
rizqicahyo 9:226d1137453e 235 pc.printf("diam \n");
nibrosul_umam 7:4d6a73d924ff 236 }
rizqicahyo 9:226d1137453e 237 if((c > 7) && (limit0 == 0)){
rizqicahyo 9:226d1137453e 238 c = 0;
rizqicahyo 9:226d1137453e 239 gripper.brake(1);
rizqicahyo 9:226d1137453e 240 }
rizqicahyo 9:226d1137453e 241 else if((c < -7) && (limit1 == 0)){
rizqicahyo 9:226d1137453e 242 c = 0;
rizqicahyo 9:226d1137453e 243 gripper.brake(1);
rizqicahyo 9:226d1137453e 244 }
rizqicahyo 9:226d1137453e 245 else if( (c > 7) && (limit0 != 0)){
rizqicahyo 9:226d1137453e 246 gripper.speed(1);
rizqicahyo 9:226d1137453e 247 }
rizqicahyo 9:226d1137453e 248 else if ((c<-7) && (limit1 != 0)){
rizqicahyo 9:226d1137453e 249 gripper.speed(-0.9);
rizqicahyo 9:226d1137453e 250 }
rizqicahyo 9:226d1137453e 251 }
rizqicahyo 9:226d1137453e 252
rizqicahyo 9:226d1137453e 253 void servo_edf(){
rizqicahyo 9:226d1137453e 254 if(ps2.read(PS_PAD::PAD_X)==1){
Najib_irvani 2:2f7bed7fb055 255 //PWM ++
Najib_irvani 2:2f7bed7fb055 256 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 257 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 258 }
nibrosul_umam 7:4d6a73d924ff 259 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
Najib_irvani 2:2f7bed7fb055 260 //PWM--
Najib_irvani 2:2f7bed7fb055 261 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 262 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 263 }
Najib_irvani 2:2f7bed7fb055 264
nibrosul_umam 7:4d6a73d924ff 265 if(ps2.read(PS_PAD::PAD_L2)==1){
Najib_irvani 2:2f7bed7fb055 266 //SERVO --
Najib_irvani 8:3cc68df2bebf 267 sudut += 3;
Najib_irvani 2:2f7bed7fb055 268 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 269 }
nibrosul_umam 7:4d6a73d924ff 270 else if(ps2.read(PS_PAD::PAD_R2)==1){
Najib_irvani 2:2f7bed7fb055 271 //SERVO ++
Najib_irvani 8:3cc68df2bebf 272 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 273 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 274 }
Najib_irvani 5:34be90fa6d27 275
rizqicahyo 9:226d1137453e 276 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 9:226d1137453e 277
rizqicahyo 9:226d1137453e 278 sudut = -60;
rizqicahyo 9:226d1137453e 279 pwm = 0.18;
rizqicahyo 9:226d1137453e 280 }
rizqicahyo 9:226d1137453e 281
Najib_irvani 2:2f7bed7fb055 282 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 283 init_pwm();
nibrosul_umam 7:4d6a73d924ff 284 edf.setThrottle(pwm);
nibrosul_umam 7:4d6a73d924ff 285 edf.pulse();
Najib_irvani 8:3cc68df2bebf 286 wait_ms(25);
Najib_irvani 2:2f7bed7fb055 287 myservo.position(sudut);
rizqicahyo 9:226d1137453e 288 }
rizqicahyo 9:226d1137453e 289
rizqicahyo 9:226d1137453e 290 void pnuematik_running(){
rizqicahyo 9:226d1137453e 291 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 9:226d1137453e 292 {
rizqicahyo 9:226d1137453e 293 //mekanisme ambil gripper
rizqicahyo 9:226d1137453e 294 pc.printf("mekanisme gripper");
rizqicahyo 9:226d1137453e 295 if (g==1){
rizqicahyo 9:226d1137453e 296 pc.printf("ambil 1");
rizqicahyo 9:226d1137453e 297 pnuematik_lengan = 1;
rizqicahyo 9:226d1137453e 298 pnuematik_gripper = 1;
rizqicahyo 9:226d1137453e 299 g=2;
rizqicahyo 9:226d1137453e 300 wait_ms(400);
rizqicahyo 9:226d1137453e 301 }
rizqicahyo 9:226d1137453e 302 else
rizqicahyo 9:226d1137453e 303 {
rizqicahyo 9:226d1137453e 304 pnuematik_gripper = 0;
rizqicahyo 9:226d1137453e 305 wait_ms(400);
rizqicahyo 9:226d1137453e 306 pnuematik_lengan = 0;
rizqicahyo 9:226d1137453e 307
rizqicahyo 9:226d1137453e 308 g=1;
rizqicahyo 9:226d1137453e 309 pc.printf("ambil 2");
rizqicahyo 9:226d1137453e 310 wait_ms(400);
rizqicahyo 9:226d1137453e 311 }
rizqicahyo 9:226d1137453e 312 }
rizqicahyo 9:226d1137453e 313 }
rizqicahyo 9:226d1137453e 314
rizqicahyo 9:226d1137453e 315 void line_follower(float speed){
rizqicahyo 9:226d1137453e 316 int pv;
rizqicahyo 9:226d1137453e 317 float speedR,speedL;
rizqicahyo 9:226d1137453e 318
rizqicahyo 9:226d1137453e 319 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 9:226d1137453e 320 if(datasensor[0]){
rizqicahyo 9:226d1137453e 321 pv = -5;
rizqicahyo 9:226d1137453e 322 over=1;
rizqicahyo 9:226d1137453e 323 }
rizqicahyo 9:226d1137453e 324 else if(datasensor[5]){
rizqicahyo 9:226d1137453e 325 pv = 5;
rizqicahyo 9:226d1137453e 326 over=2;
rizqicahyo 9:226d1137453e 327 }
rizqicahyo 9:226d1137453e 328 else if(datasensor[0] && datasensor[1]){
rizqicahyo 9:226d1137453e 329 pv = -4;
rizqicahyo 9:226d1137453e 330 }
rizqicahyo 9:226d1137453e 331 else if(datasensor[4] && datasensor[5]){
rizqicahyo 9:226d1137453e 332 pv = 4;
rizqicahyo 9:226d1137453e 333 }
rizqicahyo 9:226d1137453e 334 else if(datasensor[1]){
rizqicahyo 9:226d1137453e 335 pv = -3;
rizqicahyo 9:226d1137453e 336 }
rizqicahyo 9:226d1137453e 337 else if(datasensor[4]){
rizqicahyo 9:226d1137453e 338 pv = 3;
rizqicahyo 9:226d1137453e 339 }
rizqicahyo 9:226d1137453e 340 else if(datasensor[1] && datasensor[2]){
rizqicahyo 9:226d1137453e 341 pv = -2;
rizqicahyo 9:226d1137453e 342 }
rizqicahyo 9:226d1137453e 343 else if(datasensor[3] && datasensor[4]){
rizqicahyo 9:226d1137453e 344 pv = 2;
rizqicahyo 9:226d1137453e 345 }
rizqicahyo 9:226d1137453e 346 else if(datasensor[2]){
rizqicahyo 9:226d1137453e 347 pv = -1;
rizqicahyo 9:226d1137453e 348 }
rizqicahyo 9:226d1137453e 349 else if(datasensor[3]){
rizqicahyo 9:226d1137453e 350 pv = 1;
rizqicahyo 9:226d1137453e 351 }
rizqicahyo 9:226d1137453e 352 else if(datasensor[2] && datasensor[3]){
rizqicahyo 9:226d1137453e 353 pv = 0;
rizqicahyo 9:226d1137453e 354 }
rizqicahyo 9:226d1137453e 355 else
rizqicahyo 9:226d1137453e 356 {
rizqicahyo 9:226d1137453e 357 ///////////////// robot bergerak keluar dari sensor/////////////////////
rizqicahyo 9:226d1137453e 358 if(over==1){
rizqicahyo 9:226d1137453e 359 /*if(speed_ka > 0){
rizqicahyo 9:226d1137453e 360 wait_ms(10);
rizqicahyo 9:226d1137453e 361 motor2.speed(-speed_ka);
rizqicahyo 9:226d1137453e 362 wait_ms(10);
rizqicahyo 9:226d1137453e 363 }
rizqicahyo 9:226d1137453e 364 else{
rizqicahyo 9:226d1137453e 365 motor2.speed(speed_ka);
rizqicahyo 9:226d1137453e 366 }
rizqicahyo 9:226d1137453e 367 wait_ms(10);*/
rizqicahyo 9:226d1137453e 368
rizqicahyo 9:226d1137453e 369 motor1.brake(1);
rizqicahyo 9:226d1137453e 370 //wait_ms(100);
rizqicahyo 9:226d1137453e 371
rizqicahyo 9:226d1137453e 372 }
rizqicahyo 9:226d1137453e 373 else if(over==2){
rizqicahyo 9:226d1137453e 374 /*if(speed_ki > 0){
rizqicahyo 9:226d1137453e 375 wait_ms(10);
rizqicahyo 9:226d1137453e 376 motor1.speed(-speed_ki);
rizqicahyo 9:226d1137453e 377 wait_ms(10);
rizqicahyo 9:226d1137453e 378 }
rizqicahyo 9:226d1137453e 379 else{
rizqicahyo 9:226d1137453e 380 wait_ms(10);
rizqicahyo 9:226d1137453e 381 motor1.speed(speed_ki);
rizqicahyo 9:226d1137453e 382 wait_ms(10);
rizqicahyo 9:226d1137453e 383 }
rizqicahyo 9:226d1137453e 384 wait_ms(10);*/
rizqicahyo 9:226d1137453e 385 motor2.brake(1);
rizqicahyo 9:226d1137453e 386 //wait_ms(100);
rizqicahyo 9:226d1137453e 387 }
rizqicahyo 9:226d1137453e 388 }
rizqicahyo 9:226d1137453e 389 PID.setProcessValue(pv);
rizqicahyo 9:226d1137453e 390 PID.setSetPoint(0);
nibrosul_umam 7:4d6a73d924ff 391
rizqicahyo 9:226d1137453e 392 // memulai perhitungan PID
rizqicahyo 9:226d1137453e 393
rizqicahyo 9:226d1137453e 394 speedR = speed - PID.compute();
rizqicahyo 9:226d1137453e 395 if(speedR > speed){
rizqicahyo 9:226d1137453e 396 speedR = speed;
rizqicahyo 9:226d1137453e 397 }
rizqicahyo 9:226d1137453e 398 else if(speedR < gMin_speed)
rizqicahyo 9:226d1137453e 399 speedR = gMin_speed;
rizqicahyo 9:226d1137453e 400 motor2.speed(speedR);
rizqicahyo 9:226d1137453e 401
rizqicahyo 9:226d1137453e 402 speedL = speed + PID.compute();
rizqicahyo 9:226d1137453e 403 if(speedL > speed)
rizqicahyo 9:226d1137453e 404 speedL = speed;
rizqicahyo 9:226d1137453e 405 else if(speedL < gMin_speed)
rizqicahyo 9:226d1137453e 406 speedL = gMin_speed;
rizqicahyo 9:226d1137453e 407 motor1.speed(speedL);
rizqicahyo 9:226d1137453e 408 }
rizqicahyo 9:226d1137453e 409
rizqicahyo 9:226d1137453e 410 void init_sensor(){
rizqicahyo 9:226d1137453e 411 char data;
rizqicahyo 9:226d1137453e 412 if(sensor.readable()){
rizqicahyo 9:226d1137453e 413 data = sensor.getc();
rizqicahyo 9:226d1137453e 414
rizqicahyo 9:226d1137453e 415 for(int i=0; i<6; i++){
rizqicahyo 9:226d1137453e 416 datasensor[i] = (data >> i) & 1;
rizqicahyo 9:226d1137453e 417 }
rizqicahyo 0:ac7353383a8e 418 }
rizqicahyo 9:226d1137453e 419 }
rizqicahyo 9:226d1137453e 420
rizqicahyo 9:226d1137453e 421 /*********************************************************************************************/
rizqicahyo 9:226d1137453e 422 /** PROGRAM UTAMA **/
rizqicahyo 9:226d1137453e 423 /*********************************************************************************************/
rizqicahyo 9:226d1137453e 424 /*********************************************************************************************/
rizqicahyo 9:226d1137453e 425 int main(void){
rizqicahyo 9:226d1137453e 426 //inisialisasi serial
rizqicahyo 9:226d1137453e 427 pc.baud(115200);
rizqicahyo 9:226d1137453e 428 sensor.baud(115200);
rizqicahyo 9:226d1137453e 429
rizqicahyo 9:226d1137453e 430 //inisialisasi joystick
rizqicahyo 9:226d1137453e 431 ps2.init();
rizqicahyo 9:226d1137453e 432
rizqicahyo 9:226d1137453e 433 //set inisialisasi servo pada posisi 0
rizqicahyo 9:226d1137453e 434 myservo.position(0);
rizqicahyo 9:226d1137453e 435
rizqicahyo 9:226d1137453e 436 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
rizqicahyo 9:226d1137453e 437 edf.setThrottle(pwm);
rizqicahyo 9:226d1137453e 438 edf.pulse();
rizqicahyo 9:226d1137453e 439
rizqicahyo 9:226d1137453e 440 //inisialisasi gripper propeller
rizqicahyo 9:226d1137453e 441 pnuematik_lengan = 0;
rizqicahyo 9:226d1137453e 442 pnuematik_gripper = 0;
rizqicahyo 9:226d1137453e 443
rizqicahyo 9:226d1137453e 444 //inisialisasi PID sensor
rizqicahyo 9:226d1137453e 445 PID.setInputLimits(-15,15);
rizqicahyo 9:226d1137453e 446 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 9:226d1137453e 447 PID.setMode(1.0);
rizqicahyo 9:226d1137453e 448 PID.setBias(0.0);
rizqicahyo 9:226d1137453e 449 PID.reset();
rizqicahyo 9:226d1137453e 450
rizqicahyo 9:226d1137453e 451 bool manual=true;
rizqicahyo 9:226d1137453e 452
rizqicahyo 9:226d1137453e 453 while(manual)
rizqicahyo 9:226d1137453e 454 {
rizqicahyo 9:226d1137453e 455 ps2.poll();
rizqicahyo 9:226d1137453e 456
rizqicahyo 9:226d1137453e 457 motor_base();
rizqicahyo 9:226d1137453e 458
rizqicahyo 9:226d1137453e 459 motor_gripper();
rizqicahyo 9:226d1137453e 460
rizqicahyo 9:226d1137453e 461 pnuematik_running();
rizqicahyo 9:226d1137453e 462
rizqicahyo 9:226d1137453e 463 servo_edf();
rizqicahyo 9:226d1137453e 464
rizqicahyo 9:226d1137453e 465 if(ps2.read(PS_PAD::ANALOG_RIGHT)==1){
rizqicahyo 9:226d1137453e 466 manual = false;
rizqicahyo 9:226d1137453e 467 }
rizqicahyo 9:226d1137453e 468 }
rizqicahyo 9:226d1137453e 469
rizqicahyo 9:226d1137453e 470 timer.start();
rizqicahyo 9:226d1137453e 471 while(1)
rizqicahyo 9:226d1137453e 472 {
rizqicahyo 9:226d1137453e 473 init_sensor();
rizqicahyo 9:226d1137453e 474
rizqicahyo 9:226d1137453e 475 //line_follower(0.4);
rizqicahyo 9:226d1137453e 476
rizqicahyo 9:226d1137453e 477 for(int i=0; i<6; i++){
rizqicahyo 9:226d1137453e 478 pc.printf("%d ",datasensor[i]);
rizqicahyo 9:226d1137453e 479 }
rizqicahyo 9:226d1137453e 480 pc.printf("\n");
rizqicahyo 9:226d1137453e 481 }
rizqicahyo 9:226d1137453e 482
rizqicahyo 0:ac7353383a8e 483 }