KRAI 2016
base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of
Ultimate_Hybrid
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 0:edddd373a163
- Child:
- 1:fc1535231c0d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 19 02:47:31 2016 +0000
@@ -0,0 +1,342 @@
+/*********************************************************************************************/
+/** FILE HEADER **/
+/*********************************************************************************************/
+#include "mbed.h"
+#include "Motor.h"
+#include "PS_PAD.h"
+#include "PID.h"
+#include "esc.h"
+#include "Servo.h"
+
+/*********************************************************************************************/
+/** DEKLARASI INPUT OUTPUT **/
+/*********************************************************************************************/
+//serial
+Serial pc(USBTX,USBRX); //debug
+Serial com(PA_0,PA_1); //sensor
+
+//joystick PS2
+PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
+
+//motor (PWM, forward, reverse)
+Motor motor1(PA_8, PB_0, PC_15);
+Motor motor2(PA_11, PA_6, PA_5);
+//Motor motor3(PB_6, PA_7, PB_12);
+Motor motor3(PA_9, PC_2, PC_3);
+Motor motor4(PB_7, PA_14, PA_15);
+Motor motorC1(PB_9, PA_12, PC_5);
+Motor motorC2(PB_8, PB_1, PA_13);
+Motor motorS(PA_10, PB_5, PB_4);
+
+//pnuematik
+DigitalOut pnuematik1(PC_11);
+DigitalOut pnuematik2(PC_10);
+DigitalOut pnuematik3(PD_2);
+DigitalOut pnuematik4(PC_12);
+
+//Limit Switch
+DigitalIn limit1(PC_13 ,PullUp);
+DigitalIn limit2(PC_14 ,PullUp);
+DigitalIn limit3(PC_1 ,PullUp);
+DigitalIn limit4(PC_0 ,PullUp);
+
+//PID line follower(P, I, D, Time Sampling)
+PID PID(0.992,0.000,0.81,0.001);
+
+//servo(PWM)
+Servo servoEDF(PC_8);
+
+//EDF(PWM, timer)
+ESC edf(PC_6,20);
+
+//Timer Pnuematik
+Timer timer;
+
+/*********************************************************************************************/
+/** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
+/*********************************************************************************************/
+float gMax_speed = 1;
+float v0 = 0.6;
+float ax = 0.0007;
+
+// inisialisasi pwm awal servo
+double pwm = 0.00;
+
+// inisialisasi sudut awal
+double sudut = -85;
+
+// variabel kondisi pnuematik
+int g = 1;
+
+///////
+void aktuator();
+void edf_servo();
+/*********************************************************************************************/
+/*********************************************************************************************/
+/** PROGRAM UTAMA **/
+/*********************************************************************************************/
+/*********************************************************************************************/
+int main() {
+ //inisiasi serial
+ pc.baud(115200);
+ com.baud(115200);
+
+ //inisiasi joystick
+ ps2.init();
+
+ //set inisiasi servo pada posisi 0
+ servoEDF.position(sudut);
+
+ //set edf pada posisi bukan kalibrasi, yaitu set edf 0
+ edf.setThrottle(0);
+ edf.pulse();
+
+ //inisiasi pnuematik
+ pnuematik1 = 1;
+ pnuematik2 = 1;
+ pnuematik3 = 1;
+ pnuematik4 = 1;
+
+ //inisiasi PID sensor
+ PID.setInputLimits(-15,15);
+ PID.setOutputLimits(-1.0,1.0);
+ PID.setMode(1.0);
+ PID.setBias(0.0);
+ PID.reset();
+
+ //inisisasi TIMER
+ timer.start();
+
+ //kondisi robot
+ bool manual=true;
+
+
+ while(manual){
+
+ ps2.poll();
+
+ aktuator();
+ edf_servo();
+
+ if(limit3==0) manual = false;
+
+ }
+
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+
+ pnuematik1=0;
+ wait_ms(1500);
+
+ while(limit4!=0){
+ motorC1.speed(1);
+ motorC2.speed(-1);
+ }
+
+ motorC1.brake(1);
+ motorC2.brake(1);
+
+ pnuematik3 = 0;
+ wait_ms(1500);
+ pnuematik2 = 1;
+ wait_ms(500);
+ pnuematik3 = 1;
+
+
+
+ return 0;
+}
+
+/*********************************************************************************************/
+/** ALGORITMA PROSEDUR DAN FUNGSI **/
+/*********************************************************************************************/
+void aktuator(){
+ float speed = v0;
+ float tuning = 0.01;
+ if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
+ //pivot kiri
+ motor2.speed(speed*0.7);
+ motor4.speed(-speed*0.7);
+ motor1.speed(speed*0.7-tuning);
+ motor3.speed(-(speed*0.7-tuning));
+ pc.printf("pivot kiri \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
+ //pivot kanan
+ motor2.speed(-speed*0.7);
+ motor4.speed(speed*0.7);
+ motor1.speed(-(speed*0.7-tuning));
+ motor3.speed(speed*0.7-tuning);
+ pc.printf("pivot kanan \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
+ //serong atas kanan
+ motor2.speed(speed);
+ motor4.brake(1);
+ motor1.brake(1);
+ motor3.speed(speed-tuning);
+ pc.printf("serong atas kanan \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ //serong atas kiri
+ motor2.brake(1);
+ motor4.speed(-speed);
+ motor1.speed(-(speed-tuning));
+ motor3.brake(1);
+ pc.printf("serong atas kiri \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
+ //serong bawah kiri
+ motor2.speed(-speed);
+ motor4.brake(1);
+ motor1.brake(1);
+ motor3.speed(-(speed-tuning));
+ pc.printf("serong bawah kiri \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
+ //serong bawah kanan
+ motor2.brake(1);
+ motor4.speed(speed);
+ motor1.speed(speed-tuning);
+ motor3.brake(1);
+ pc.printf("serong bawah kanan \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+ //maju
+ motor1.speed(-(speed-tuning));
+ motor3.speed(speed-tuning);
+ motor2.speed(speed);
+ motor4.speed(-speed);
+ pc.printf("maju \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
+ //mundur
+ motor1.speed(speed-tuning);
+ motor3.speed(-(speed-tuning));
+ motor2.speed(-speed);
+ motor4.speed(speed);
+ pc.printf("mundur \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
+ //kanan
+ motor2.speed(speed);
+ motor4.speed(speed);
+ motor1.speed(speed);
+ motor3.speed(speed);
+ pc.printf("kanan \n");
+
+ v0 += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ //kiri
+ motor2.speed(-speed);
+ motor4.speed(-speed);
+ motor1.speed(-speed);
+ motor3.speed(-speed);
+ pc.printf("kiri \n");
+
+ v0 += ax;
+ }
+ else{
+ motor1.brake(1);
+ motor3.brake(1);
+ motor2.brake(1);
+ motor4.brake(1);
+ pc.printf("diam \n");
+
+ v0 = 0.6;
+ }
+
+ if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
+ //POWER WINDOW ATAS
+ motorS.speed(1);
+ pc.printf("up \n");
+ }
+ else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
+ //POWER WINDOW BAWAH
+ motorS.speed(-1);
+ pc.printf("down \n");
+ }
+ else{
+ motorS.brake(1);
+ pc.printf("diam \n");
+
+ }
+
+ if ((ps2.read(PS_PAD::PAD_SELECT)==1))
+ {
+ //mekanisme ambil gripper
+ pc.printf("mekanisme gripper");
+ if (g==1){
+ pc.printf("ambil 1");
+ pnuematik2 = 0;
+ g=2;
+ wait_ms(400);
+ }
+ else
+ {
+ pnuematik2 = 1;
+ wait_ms(400);
+ g=1;
+ }
+ }
+}
+
+void edf_servo(){
+ if(ps2.read(PS_PAD::PAD_X)==1){
+ //PWM ++
+ pwm += 0.002;
+ if( pwm > 0.7) pwm = 0.7;
+ pc.printf("gaspol \n");
+ }
+ else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
+ //PWM--
+ pwm -= 0.002;
+
+ if(pwm < 0) pwm = 0.0;
+ pc.printf("rem ndeng \n");
+ }
+
+ if(ps2.read(PS_PAD::PAD_R2)==1){
+ //SERVO --
+ sudut += 0.5;
+
+ if(sudut > 90) sudut = 90;
+ pc.printf("servo max \n");
+ }
+ else if(ps2.read(PS_PAD::PAD_L2)==1){
+ //SERVO ++
+ sudut -= 0.5;
+
+ if(sudut < -90) sudut = -90;
+ pc.printf("servo min \n");
+ }
+
+ if(ps2.read(PS_PAD::PAD_START)==1){
+
+ sudut = 0;
+ pwm = 0.25;
+ }
+
+ servoEDF.position((float)sudut);
+ edf.setThrottle((float)pwm);
+ edf.pulse();
+}
\ No newline at end of file