KRAI 2016 / Mbed 2 deprecated base_rekam_darurat_v1

darurat

Dependencies:   ESC Motor NewTextLCD PID PS_PAD base_rekam_darurat mbed millis

Fork of Base_Hybrid_V2 by KRAI 2016

main.cpp@2:2f7bed7fb055, 2016-02-16 (annotated)

Committer:
Najib_irvani
Date:
Tue Feb 16 18:33:25 2016 +0000
Revision:
2:2f7bed7fb055
Parent:
1:c9f11055fb12
Child:
3:d3708c3ed288
program sementara hybrid sambil menunggu board baru

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:ac7353383a8e 1 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 2 /** GARUDAGO-ITB (KRAI2016) **/
rizqicahyo 0:ac7353383a8e 3 /** #ROADTOBANGKOK! **/
rizqicahyo 0:ac7353383a8e 4 /** **/
rizqicahyo 0:ac7353383a8e 5 /** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
rizqicahyo 0:ac7353383a8e 6 /** **/
rizqicahyo 0:ac7353383a8e 7 /** **/
rizqicahyo 0:ac7353383a8e 8 /** Created by : **/
rizqicahyo 0:ac7353383a8e 9 /** Rizqi Cahyo Yuwono **/
rizqicahyo 0:ac7353383a8e 10 /** EL'14 - 13214090 **/
rizqicahyo 0:ac7353383a8e 11 /** **/
rizqicahyo 0:ac7353383a8e 12 /** Last Update : 19 Desember 2015, 06.10 WIB **/
rizqicahyo 0:ac7353383a8e 13 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 14
rizqicahyo 0:ac7353383a8e 15 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 16 /** FILE HEADER **/
rizqicahyo 0:ac7353383a8e 17 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 18 #include "mbed.h"
rizqicahyo 0:ac7353383a8e 19 #include "Motor.h"
rizqicahyo 0:ac7353383a8e 20 #include "NewTextLCD.h"
rizqicahyo 0:ac7353383a8e 21 #include "PS_PAD.h"
rizqicahyo 0:ac7353383a8e 22 #include "PID.h"
rizqicahyo 1:c9f11055fb12 23 #include "millis.h"
Najib_irvani 2:2f7bed7fb055 24 #include "esc.h"
Najib_irvani 2:2f7bed7fb055 25 #include "Servo.h"
rizqicahyo 0:ac7353383a8e 26
rizqicahyo 0:ac7353383a8e 27 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 28 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:ac7353383a8e 29 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 30 // serial pc
rizqicahyo 0:ac7353383a8e 31 Serial pc(USBTX,USBRX);
rizqicahyo 0:ac7353383a8e 32
rizqicahyo 0:ac7353383a8e 33 // LCD 20x4
Najib_irvani 2:2f7bed7fb055 34 //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 35
rizqicahyo 0:ac7353383a8e 36 // joystick PS2
rizqicahyo 0:ac7353383a8e 37 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
rizqicahyo 0:ac7353383a8e 38
Najib_irvani 2:2f7bed7fb055 39
Najib_irvani 2:2f7bed7fb055 40 // tambahan power
Najib_irvani 2:2f7bed7fb055 41 // inisialisasi pwm awal servo
Najib_irvani 2:2f7bed7fb055 42 float pwm = 0.00;
Najib_irvani 2:2f7bed7fb055 43
Najib_irvani 2:2f7bed7fb055 44 // inisialisasi sudut awal
Najib_irvani 2:2f7bed7fb055 45 int sudut = 0;
Najib_irvani 2:2f7bed7fb055 46
Najib_irvani 2:2f7bed7fb055 47 //membatasi nilai brushless pada edf
Najib_irvani 2:2f7bed7fb055 48 float min=0;
Najib_irvani 2:2f7bed7fb055 49 float max=0.50;
Najib_irvani 2:2f7bed7fb055 50
rizqicahyo 0:ac7353383a8e 51 // PID sensor garis
Najib_irvani 2:2f7bed7fb055 52 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rizqicahyo 0:ac7353383a8e 53
rizqicahyo 0:ac7353383a8e 54 // Motor(pwm, fwd, rev)
Najib_irvani 2:2f7bed7fb055 55 Motor motor2(PC_6, PC_8, PC_9); //gripper
Najib_irvani 2:2f7bed7fb055 56 Motor motor1(PA_15, PA_14, PA_13); //motor3
Najib_irvani 2:2f7bed7fb055 57 //Motor gripper(PB_5, PB_4, PB_3);
Najib_irvani 2:2f7bed7fb055 58 //Motor motor2(PA_3, PC_15, PC_14); //kanan
Najib_irvani 2:2f7bed7fb055 59 //Motor motor1(PA_1, PH_0, PH_1); //kiri
rizqicahyo 0:ac7353383a8e 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
rizqicahyo 0:ac7353383a8e 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 2:2f7bed7fb055 95 float gMax_speed=0.5; //nilai maksimum kecepatan motor
rizqicahyo 0:ac7353383a8e 96 float gMin_speed=-0.05; //nilai minimum kecepatan motor
Najib_irvani 2:2f7bed7fb055 97 float gTuning = 0.34;
rizqicahyo 0:ac7353383a8e 98
rizqicahyo 0:ac7353383a8e 99 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
rizqicahyo 0:ac7353383a8e 100 unsigned char gCase=0; //variabel keadaan proses
rizqicahyo 0:ac7353383a8e 101
rizqicahyo 0:ac7353383a8e 102 unsigned char i; // variabel iterasi
rizqicahyo 1:c9f11055fb12 103 int over=0;
Najib_irvani 2:2f7bed7fb055 104 int lapangan = 1;
rizqicahyo 0:ac7353383a8e 105
rizqicahyo 0:ac7353383a8e 106
rizqicahyo 0:ac7353383a8e 107
rizqicahyo 0:ac7353383a8e 108 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 109 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 0:ac7353383a8e 110 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 111
rizqicahyo 0:ac7353383a8e 112
rizqicahyo 0:ac7353383a8e 113 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 114 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 115 /** PROGRAM UTAMA **/
rizqicahyo 0:ac7353383a8e 116 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 117 /*********************************************************************************************/
Najib_irvani 2:2f7bed7fb055 118
Najib_irvani 2:2f7bed7fb055 119 void init_servo(int i){
Najib_irvani 2:2f7bed7fb055 120 if (i){
Najib_irvani 2:2f7bed7fb055 121 if (sudut>90){
Najib_irvani 2:2f7bed7fb055 122 sudut=90;
Najib_irvani 2:2f7bed7fb055 123 }
Najib_irvani 2:2f7bed7fb055 124 if (sudut<0){
Najib_irvani 2:2f7bed7fb055 125 sudut=0;
Najib_irvani 2:2f7bed7fb055 126 }
Najib_irvani 2:2f7bed7fb055 127 } else {
Najib_irvani 2:2f7bed7fb055 128
Najib_irvani 2:2f7bed7fb055 129 if (sudut>0){
Najib_irvani 2:2f7bed7fb055 130 sudut=0;
Najib_irvani 2:2f7bed7fb055 131 }
Najib_irvani 2:2f7bed7fb055 132 if (sudut<-90){
Najib_irvani 2:2f7bed7fb055 133 sudut=-90;
Najib_irvani 2:2f7bed7fb055 134 }
Najib_irvani 2:2f7bed7fb055 135 }
Najib_irvani 2:2f7bed7fb055 136 }
Najib_irvani 2:2f7bed7fb055 137
Najib_irvani 2:2f7bed7fb055 138 void init_pwm (){
Najib_irvani 2:2f7bed7fb055 139 if (pwm>max){
Najib_irvani 2:2f7bed7fb055 140 pwm = max;
Najib_irvani 2:2f7bed7fb055 141 }
Najib_irvani 2:2f7bed7fb055 142
Najib_irvani 2:2f7bed7fb055 143 if (pwm<min){
Najib_irvani 2:2f7bed7fb055 144 pwm = min;
Najib_irvani 2:2f7bed7fb055 145 }
Najib_irvani 2:2f7bed7fb055 146 }
Najib_irvani 2:2f7bed7fb055 147
rizqicahyo 0:ac7353383a8e 148 int main(void){
rizqicahyo 0:ac7353383a8e 149 //inisialisasi joystick
rizqicahyo 0:ac7353383a8e 150 ps2.init();
rizqicahyo 0:ac7353383a8e 151
Najib_irvani 2:2f7bed7fb055 152 //tambahan power
Najib_irvani 2:2f7bed7fb055 153 ESC edf(PB_9,20); //p wm esc pin PC_7
Najib_irvani 2:2f7bed7fb055 154 Servo myservo(PB_8); //pwm servo pin PA_8
Najib_irvani 2:2f7bed7fb055 155 //set inisialisasi servo pada posisi 0
Najib_irvani 2:2f7bed7fb055 156 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 157 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
Najib_irvani 2:2f7bed7fb055 158 edf.setThrottle(pwm);
Najib_irvani 2:2f7bed7fb055 159 edf.pulse();
rizqicahyo 0:ac7353383a8e 160
rizqicahyo 0:ac7353383a8e 161 pc.baud(115200);
Najib_irvani 2:2f7bed7fb055 162 float speed;
rizqicahyo 0:ac7353383a8e 163
rizqicahyo 0:ac7353383a8e 164 while(1)
rizqicahyo 0:ac7353383a8e 165 {
Najib_irvani 2:2f7bed7fb055 166 ps2.poll();
Najib_irvani 2:2f7bed7fb055 167
Najib_irvani 2:2f7bed7fb055 168 if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
Najib_irvani 2:2f7bed7fb055 169 //MOTOR DEPAN
Najib_irvani 2:2f7bed7fb055 170 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 171
Najib_irvani 2:2f7bed7fb055 172 motor1.speed(speed);
Najib_irvani 2:2f7bed7fb055 173 motor2.speed(speed);
Najib_irvani 2:2f7bed7fb055 174 pc.printf("maju \n");
Najib_irvani 2:2f7bed7fb055 175 }
Najib_irvani 2:2f7bed7fb055 176 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
Najib_irvani 2:2f7bed7fb055 177 //MOTOR BELAKANG
Najib_irvani 2:2f7bed7fb055 178 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 179
Najib_irvani 2:2f7bed7fb055 180 motor1.speed(-speed);
Najib_irvani 2:2f7bed7fb055 181 motor2.speed(-speed);
Najib_irvani 2:2f7bed7fb055 182 pc.printf("mundur \n");
Najib_irvani 2:2f7bed7fb055 183 }
Najib_irvani 2:2f7bed7fb055 184 else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
Najib_irvani 2:2f7bed7fb055 185 //pivot kiri
Najib_irvani 2:2f7bed7fb055 186 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 187
Najib_irvani 2:2f7bed7fb055 188 motor1.speed(-speed*0.8);
Najib_irvani 2:2f7bed7fb055 189 motor2.speed(speed*0.8);
Najib_irvani 2:2f7bed7fb055 190 pc.printf("kiri \n");
Najib_irvani 2:2f7bed7fb055 191 }
Najib_irvani 2:2f7bed7fb055 192 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
Najib_irvani 2:2f7bed7fb055 193 //pivot kanan
Najib_irvani 2:2f7bed7fb055 194 speed = gMax_speed;
Najib_irvani 2:2f7bed7fb055 195
Najib_irvani 2:2f7bed7fb055 196 motor1.speed(speed*0.8);
Najib_irvani 2:2f7bed7fb055 197 motor2.speed(-speed*0.8);
Najib_irvani 2:2f7bed7fb055 198 pc.printf("kanan \n");
Najib_irvani 2:2f7bed7fb055 199 }
Najib_irvani 2:2f7bed7fb055 200 else
rizqicahyo 1:c9f11055fb12 201 {
Najib_irvani 2:2f7bed7fb055 202 motor1.brake(1);
Najib_irvani 2:2f7bed7fb055 203 motor2.brake(1);
Najib_irvani 2:2f7bed7fb055 204 }
Najib_irvani 2:2f7bed7fb055 205
Najib_irvani 2:2f7bed7fb055 206 if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
Najib_irvani 2:2f7bed7fb055 207 //SLIDER KIRI
Najib_irvani 2:2f7bed7fb055 208 pc.printf("slide kiri \n");
Najib_irvani 2:2f7bed7fb055 209 }
Najib_irvani 2:2f7bed7fb055 210 else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
Najib_irvani 2:2f7bed7fb055 211 //SLIDER KANAN
Najib_irvani 2:2f7bed7fb055 212 pc.printf("slide kanan \n");
rizqicahyo 1:c9f11055fb12 213 }
Najib_irvani 2:2f7bed7fb055 214 else
Najib_irvani 2:2f7bed7fb055 215 {
Najib_irvani 2:2f7bed7fb055 216 pc.printf("slide diam \n");
Najib_irvani 2:2f7bed7fb055 217 }
Najib_irvani 2:2f7bed7fb055 218
Najib_irvani 2:2f7bed7fb055 219 /*if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
Najib_irvani 2:2f7bed7fb055 220 //POWER WINDOW ATAS
Najib_irvani 2:2f7bed7fb055 221 gripper.speed(0.75);
Najib_irvani 2:2f7bed7fb055 222 pc.printf("up \n");
Najib_irvani 2:2f7bed7fb055 223 }
Najib_irvani 2:2f7bed7fb055 224 else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
Najib_irvani 2:2f7bed7fb055 225 //POWER WINDOW BAWAH
Najib_irvani 2:2f7bed7fb055 226 gripper.speed(-0.2);
Najib_irvani 2:2f7bed7fb055 227 pc.printf("down \n");
Najib_irvani 2:2f7bed7fb055 228 }
Najib_irvani 2:2f7bed7fb055 229 else
Najib_irvani 2:2f7bed7fb055 230 {
Najib_irvani 2:2f7bed7fb055 231 gripper.brake(1);
Najib_irvani 2:2f7bed7fb055 232 pc.printf("power diam \n");
Najib_irvani 2:2f7bed7fb055 233 }*/
Najib_irvani 2:2f7bed7fb055 234
Najib_irvani 2:2f7bed7fb055 235 if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
Najib_irvani 2:2f7bed7fb055 236 //PWM ++
Najib_irvani 2:2f7bed7fb055 237 pwm += 0.01;
Najib_irvani 2:2f7bed7fb055 238 pc.printf("gaspol \n");
Najib_irvani 2:2f7bed7fb055 239 }
Najib_irvani 2:2f7bed7fb055 240
Najib_irvani 2:2f7bed7fb055 241 if(ps2.read(PS_PAD::ANALOG_RY)>=100){
Najib_irvani 2:2f7bed7fb055 242 //PWM--
Najib_irvani 2:2f7bed7fb055 243 pwm -= 0.01;
Najib_irvani 2:2f7bed7fb055 244 pc.printf("rem ndeng \n");
Najib_irvani 2:2f7bed7fb055 245 }
Najib_irvani 2:2f7bed7fb055 246
Najib_irvani 2:2f7bed7fb055 247 if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
Najib_irvani 2:2f7bed7fb055 248 //SERVO --
Najib_irvani 2:2f7bed7fb055 249 sudut -= 3;
Najib_irvani 2:2f7bed7fb055 250 pc.printf("servo min \n");
Najib_irvani 2:2f7bed7fb055 251 }
Najib_irvani 2:2f7bed7fb055 252
Najib_irvani 2:2f7bed7fb055 253 if(ps2.read(PS_PAD::ANALOG_RX)>=100){
Najib_irvani 2:2f7bed7fb055 254 //SERVO ++
Najib_irvani 2:2f7bed7fb055 255 sudut += 3;
Najib_irvani 2:2f7bed7fb055 256 pc.printf("servo max \n");
Najib_irvani 2:2f7bed7fb055 257 }
Najib_irvani 2:2f7bed7fb055 258
Najib_irvani 2:2f7bed7fb055 259 init_servo(lapangan);
Najib_irvani 2:2f7bed7fb055 260 init_pwm();
Najib_irvani 2:2f7bed7fb055 261 myservo.position(sudut);
Najib_irvani 2:2f7bed7fb055 262 wait_ms(25);
Najib_irvani 2:2f7bed7fb055 263 edf.setThrottle(pwm);
Najib_irvani 2:2f7bed7fb055 264 edf.pulse();
Najib_irvani 2:2f7bed7fb055 265 wait_ms(25);
rizqicahyo 0:ac7353383a8e 266 }
rizqicahyo 0:ac7353383a8e 267 }