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@7:4d6a73d924ff, 2016-03-18 (annotated)

Committer:
nibrosul_umam
Date:
Fri Mar 18 22:45:08 2016 +0000
Revision:
7:4d6a73d924ff
Parent:
6:0e159860e5c6
Child:
8:3cc68df2bebf
hajar bleh....

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