KRAI 2016 / Mbed 2 deprecated Base_Hybrid_Latihan_Ok_Hajar_servo_pwm

base buat latihan

Dependencies:   ESC Motor NewTextLCD PID PS_PAD Ping Servo mbed millis

Fork of Base_Hybrid_Latihan_Ok_Hajar by KRAI 2016

main.cpp@8:3cc68df2bebf, 2016-03-21 (annotated)

Committer:
Najib_irvani
Date:
Mon Mar 21 12:10:27 2016 +0000
Revision:
8:3cc68df2bebf
Parent:
7:4d6a73d924ff 
update board baru

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 0:ac7353383a8e 21
Najib_irvani 8:3cc68df2bebf 22 //sensor ping
Najib_irvani 8:3cc68df2bebf 23 Ping pinger(PA_1);
Najib_irvani 8:3cc68df2bebf 24 //read jarak
Najib_irvani 8:3cc68df2bebf 25 int read_ping(){
Najib_irvani 8:3cc68df2bebf 26 pinger.Send();
Najib_irvani 8:3cc68df2bebf 27 wait_ms(30);
Najib_irvani 8:3cc68df2bebf 28 return ((pinger.Read_cm())/2);
Najib_irvani 8:3cc68df2bebf 29 }
rizqicahyo 0:ac7353383a8e 30
Najib_irvani 8:3cc68df2bebf 31 // joystick PS2
Najib_irvani 8:3cc68df2bebf 32 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12
Najib_irvani 8:3cc68df2bebf 33 /*
rizqicahyo 0:ac7353383a8e 34 // Motor(pwm, fwd, rev)
Najib_irvani 8:3cc68df2bebf 35 Motor gripper(PC_6, PC_8, PC_9); //PB_6, PB_8, PB_9
nibrosul_umam 7:4d6a73d924ff 36 //Motor slider(PC_6, PC_9, PC_8);
nibrosul_umam 7:4d6a73d924ff 37 Motor motor2(PB_3, PB_4, PB_5); //kanan
nibrosul_umam 7:4d6a73d924ff 38 Motor motor1(PA_8, PC_7, PA_9); //kiri
Najib_irvani 8:3cc68df2bebf 39 */
Najib_irvani 8:3cc68df2bebf 40 Motor gripper(PA_10, PB_3, PB_5); //PB_6, PB_8, PB_9
Najib_irvani 8:3cc68df2bebf 41 //Motor slider(PC_6, PC_9, PC_8);
Najib_irvani 8:3cc68df2bebf 42 Motor motor2(PA_11, PB_12, PA_7); //kanan
Najib_irvani 8:3cc68df2bebf 43 Motor motor1(PA_8, PB_4, PB_1); //kiri
Najib_irvani 8:3cc68df2bebf 44
Najib_irvani 8:3cc68df2bebf 45 DigitalOut limit0(PC_0,PullUp);
Najib_irvani 8:3cc68df2bebf 46 DigitalOut limit1(PC_1,PullUp);
rizqicahyo 0:ac7353383a8e 47
rizqicahyo 0:ac7353383a8e 48
rizqicahyo 0:ac7353383a8e 49 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 50 /** DEKLARASI VARIABEL GLOBAL **/
rizqicahyo 0:ac7353383a8e 51 /*********************************************************************************************/
Najib_irvani 8:3cc68df2bebf 52 float gMax_speed=0.83; //nilai maksimum kecepatan motor
rizqicahyo 0:ac7353383a8e 53 float gMin_speed=-0.05; //nilai minimum kecepatan motor
Najib_irvani 8:3cc68df2bebf 54 float gTuning = 0.14;
rizqicahyo 0:ac7353383a8e 55
nibrosul_umam 7:4d6a73d924ff 56 // tambahan power
nibrosul_umam 7:4d6a73d924ff 57 // inisialisasi pwm awal servo
nibrosul_umam 7:4d6a73d924ff 58 float pwm = 0.00;
nibrosul_umam 7:4d6a73d924ff 59
nibrosul_umam 7:4d6a73d924ff 60 // inisialisasi sudut awal
nibrosul_umam 7:4d6a73d924ff 61 int sudut = 0;
nibrosul_umam 7:4d6a73d924ff 62 //membatasi nilai brushless pada edf
nibrosul_umam 7:4d6a73d924ff 63 float min=0;
nibrosul_umam 7:4d6a73d924ff 64 float max=0.70;
rizqicahyo 0:ac7353383a8e 65
rizqicahyo 0:ac7353383a8e 66 unsigned char i; // variabel iterasi
rizqicahyo 1:c9f11055fb12 67 int over=0;
Najib_irvani 8:3cc68df2bebf 68 int lapangan = 0;
rizqicahyo 0:ac7353383a8e 69
nibrosul_umam 7:4d6a73d924ff 70 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
nibrosul_umam 7:4d6a73d924ff 71 unsigned char gCase=0; //variabel keadaan proses
nibrosul_umam 7:4d6a73d924ff 72
nibrosul_umam 7:4d6a73d924ff 73
rizqicahyo 0:ac7353383a8e 74
rizqicahyo 0:ac7353383a8e 75 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 76 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 0:ac7353383a8e 77 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 78
rizqicahyo 0:ac7353383a8e 79 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 80 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 81 /** PROGRAM UTAMA **/
rizqicahyo 0:ac7353383a8e 82 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 83 /*********************************************************************************************/
Najib_irvani 2:2f7bed7fb055 84 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 85 if (i){
Najib_irvani 8:3cc68df2bebf 86 if (sudut>60){
Najib_irvani 8:3cc68df2bebf 87 sudut=60;
Najib_irvani 2:2f7bed7fb055 88 }
Najib_irvani 8:3cc68df2bebf 89 if (sudut<-60){
Najib_irvani 8:3cc68df2bebf 90 sudut=-60;
Najib_irvani 2:2f7bed7fb055 91 }
Najib_irvani 2:2f7bed7fb055 92 } else {
Najib_irvani 2:2f7bed7fb055 93
Najib_irvani 8:3cc68df2bebf 94 if (sudut>60){
Najib_irvani 8:3cc68df2bebf 95 sudut=60;
Najib_irvani 2:2f7bed7fb055 96 }
Najib_irvani 8:3cc68df2bebf 97 if (sudut<-60){
Najib_irvani 8:3cc68df2bebf 98 sudut=-60;
nibrosul_umam 7:4d6a73d924ff 99 }
Najib_irvani 2:2f7bed7fb055 100 }
Najib_irvani 2:2f7bed7fb055 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 }
nibrosul_umam 7:4d6a73d924ff 111
Najib_irvani 3:d3708c3ed288 112 int count=1;
Najib_irvani 3:d3708c3ed288 113
rizqicahyo 0:ac7353383a8e 114 int main(void){
rizqicahyo 0:ac7353383a8e 115 //inisialisasi joystick
nibrosul_umam 7:4d6a73d924ff 116 ps2.init();
Najib_irvani 2:2f7bed7fb055 117 //tambahan power
Najib_irvani 8:3cc68df2bebf 118 ESC edf(PC_6,20); //pwm esc PB_8
Najib_irvani 8:3cc68df2bebf 119 Servo myservo(PC_8); //pwm servo PB_9
Najib_irvani 2:2f7bed7fb055 120 //set inisialisasi servo pada posisi 0
Najib_irvani 8:3cc68df2bebf 121 myservo.position(0);
Najib_irvani 8:3cc68df2bebf 122
Najib_irvani 2:2f7bed7fb055 123 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
nibrosul_umam 7:4d6a73d924ff 124 edf.setThrottle(pwm);
nibrosul_umam 7:4d6a73d924ff 125 edf.pulse();
nibrosul_umam 7:4d6a73d924ff 126
nibrosul_umam 7:4d6a73d924ff 127 float k;
rizqicahyo 0:ac7353383a8e 128
rizqicahyo 0:ac7353383a8e 129 pc.baud(115200);
Najib_irvani 2:2f7bed7fb055 130 float speed;
rizqicahyo 0:ac7353383a8e 131
rizqicahyo 0:ac7353383a8e 132 while(1)
rizqicahyo 0:ac7353383a8e 133 {
nibrosul_umam 7:4d6a73d924ff 134 ps2.poll();
nibrosul_umam 7:4d6a73d924ff 135 //init_sensor();
nibrosul_umam 7:4d6a73d924ff 136 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
nibrosul_umam 7:4d6a73d924ff 137 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 138 motor1.brake(0.2);
nibrosul_umam 7:4d6a73d924ff 139 motor2.speed(speed-0.05);
nibrosul_umam 7:4d6a73d924ff 140 pc.printf("maju serong kiri\n");
nibrosul_umam 7:4d6a73d924ff 141
nibrosul_umam 7:4d6a73d924ff 142 }
nibrosul_umam 7:4d6a73d924ff 143 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
nibrosul_umam 7:4d6a73d924ff 144 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 145 motor1.speed(speed-gTuning-0.05);
Najib_irvani 8:3cc68df2bebf 146 motor2.brake(0.2);
nibrosul_umam 7:4d6a73d924ff 147 pc.printf("maju serong kanan\n");
nibrosul_umam 7:4d6a73d924ff 148
nibrosul_umam 7:4d6a73d924ff 149 }
nibrosul_umam 7:4d6a73d924ff 150 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
nibrosul_umam 7:4d6a73d924ff 151 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 152 motor2.brake(1);
nibrosul_umam 7:4d6a73d924ff 153 motor1.speed(-(speed-gTuning-0.2));
nibrosul_umam 7:4d6a73d924ff 154 pc.printf("mundur serong kanan\n");
nibrosul_umam 7:4d6a73d924ff 155
Najib_irvani 2:2f7bed7fb055 156 }
nibrosul_umam 7:4d6a73d924ff 157 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
nibrosul_umam 7:4d6a73d924ff 158 speed = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 159 motor2.speed(-(speed-0.2));
nibrosul_umam 7:4d6a73d924ff 160 motor1.brake(1);
nibrosul_umam 7:4d6a73d924ff 161 pc.printf("mundur serong kiri\n");
nibrosul_umam 7:4d6a73d924ff 162
nibrosul_umam 7:4d6a73d924ff 163 }
nibrosul_umam 7:4d6a73d924ff 164 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
nibrosul_umam 7:4d6a73d924ff 165 //MOTOR DEPAN
nibrosul_umam 7:4d6a73d924ff 166
nibrosul_umam 7:4d6a73d924ff 167 speed = k;
nibrosul_umam 7:4d6a73d924ff 168
nibrosul_umam 7:4d6a73d924ff 169 if (k >= gMax_speed){
nibrosul_umam 7:4d6a73d924ff 170 k = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 171 }
nibrosul_umam 7:4d6a73d924ff 172
nibrosul_umam 7:4d6a73d924ff 173 motor1.speed(speed-gTuning);
nibrosul_umam 7:4d6a73d924ff 174 motor2.speed(speed);
nibrosul_umam 7:4d6a73d924ff 175 pc.printf("maju \n");
nibrosul_umam 7:4d6a73d924ff 176
nibrosul_umam 7:4d6a73d924ff 177 k += 0.1;
nibrosul_umam 7:4d6a73d924ff 178 }
Najib_irvani 2:2f7bed7fb055 179 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 180 //MOTOR BELAKANG
nibrosul_umam 7:4d6a73d924ff 181 speed = k;
nibrosul_umam 7:4d6a73d924ff 182
nibrosul_umam 7:4d6a73d924ff 183 if (k >= gMax_speed){
nibrosul_umam 7:4d6a73d924ff 184 k = gMax_speed;
nibrosul_umam 7:4d6a73d924ff 185 }
nibrosul_umam 7:4d6a73d924ff 186
Najib_irvani 5:34be90fa6d27 187 motor1.speed(-speed);
Najib_irvani 2:2f7bed7fb055 188 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 189 pc.printf("mundur \n");
nibrosul_umam 7:4d6a73d924ff 190
nibrosul_umam 7:4d6a73d924ff 191 k += 0.1;
Najib_irvani 2:2f7bed7fb055 192 }
Najib_irvani 2:2f7bed7fb055 193 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 194 //pivot kiri
nibrosul_umam 7:4d6a73d924ff 195 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 196 motor1.speed(-(speed*0.95-gTuning));
Najib_irvani 8:3cc68df2bebf 197 motor2.speed(speed*0.95);
nibrosul_umam 7:4d6a73d924ff 198 pc.printf("pivot kiri \n");
Najib_irvani 2:2f7bed7fb055 199 }
Najib_irvani 2:2f7bed7fb055 200 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 201 //pivot kanan
nibrosul_umam 7:4d6a73d924ff 202 speed = gMax_speed;
Najib_irvani 8:3cc68df2bebf 203 motor1.speed(speed*0.95-gTuning);
Najib_irvani 8:3cc68df2bebf 204 motor2.speed(-speed*0.95 );
nibrosul_umam 7:4d6a73d924ff 205 pc.printf("pivot kanan \n");
nibrosul_umam 7:4d6a73d924ff 206
Najib_irvani 2:2f7bed7fb055 207 }
nibrosul_umam 7:4d6a73d924ff 208 else{
Najib_irvani 2:2f7bed7fb055 209 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 210 motor2.brake(1);
nibrosul_umam 7:4d6a73d924ff 211
nibrosul_umam 7:4d6a73d924ff 212 k = 0.6;
Najib_irvani 2:2f7bed7fb055 213 }
nibrosul_umam 7:4d6a73d924ff 214
Najib_irvani 2:2f7bed7fb055 215
nibrosul_umam 7:4d6a73d924ff 216 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
Najib_irvani 2:2f7bed7fb055 217 //POWER WINDOW ATAS
Najib_irvani 8:3cc68df2bebf 218 gripper.speed(1);
Najib_irvani 5:34be90fa6d27 219
Najib_irvani 8:3cc68df2bebf 220 if (limit0 == 0){
Najib_irvani 8:3cc68df2bebf 221 gripper.brake(1);
Najib_irvani 8:3cc68df2bebf 222 }
Najib_irvani 2:2f7bed7fb055 223 pc.printf("up \n");
Najib_irvani 2:2f7bed7fb055 224 }
nibrosul_umam 7:4d6a73d924ff 225 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
Najib_irvani 5:34be90fa6d27 226 //POWER WINDOW BAWAH
Najib_irvani 5:34be90fa6d27 227
nibrosul_umam 7:4d6a73d924ff 228 gripper.speed(-1);
Najib_irvani 8:3cc68df2bebf 229
Najib_irvani 8:3cc68df2bebf 230 if (limit1 ==0){
Najib_irvani 8:3cc68df2bebf 231 gripper.brake(1);
Najib_irvani 8:3cc68df2bebf 232 }
Najib_irvani 8:3cc68df2bebf 233
Najib_irvani 2:2f7bed7fb055 234 pc.printf("down \n");
Najib_irvani 2:2f7bed7fb055 235 }
nibrosul_umam 7:4d6a73d924ff 236 else{
Najib_irvani 2:2f7bed7fb055 237 gripper.brake(1);
nibrosul_umam 7:4d6a73d924ff 238
nibrosul_umam 7:4d6a73d924ff 239 pc.printf("diam \n");
nibrosul_umam 7:4d6a73d924ff 240
nibrosul_umam 7:4d6a73d924ff 241 }
Najib_irvani 2:2f7bed7fb055 242
nibrosul_umam 7:4d6a73d924ff 243 if(ps2.read(PS_PAD::PAD_X)==1){
Najib_irvani 2:2f7bed7fb055 244 //PWM ++
Najib_irvani 2:2f7bed7fb055 245 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 246 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 247 }
nibrosul_umam 7:4d6a73d924ff 248 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
Najib_irvani 2:2f7bed7fb055 249 //PWM--
Najib_irvani 2:2f7bed7fb055 250 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 251 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 252 }
Najib_irvani 2:2f7bed7fb055 253
nibrosul_umam 7:4d6a73d924ff 254 if(ps2.read(PS_PAD::PAD_L2)==1){
Najib_irvani 2:2f7bed7fb055 255 //SERVO --
Najib_irvani 8:3cc68df2bebf 256 sudut += 3;
Najib_irvani 2:2f7bed7fb055 257 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 258 }
nibrosul_umam 7:4d6a73d924ff 259 else if(ps2.read(PS_PAD::PAD_R2)==1){
Najib_irvani 2:2f7bed7fb055 260 //SERVO ++
Najib_irvani 8:3cc68df2bebf 261 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 262 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 263 }
Najib_irvani 5:34be90fa6d27 264
Najib_irvani 2:2f7bed7fb055 265 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 266 init_pwm();
nibrosul_umam 7:4d6a73d924ff 267 edf.setThrottle(pwm);
nibrosul_umam 7:4d6a73d924ff 268 edf.pulse();
Najib_irvani 8:3cc68df2bebf 269 wait_ms(25);
Najib_irvani 2:2f7bed7fb055 270 myservo.position(sudut);
nibrosul_umam 7:4d6a73d924ff 271
rizqicahyo 0:ac7353383a8e 272 }
rizqicahyo 0:ac7353383a8e 273 }