{

LED_RED=0;
TRIGGER=1;
while(1)
{
LED_GREEN=1;
if(skps(p_up)==0)
{
if(skps(p_right)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_right(63,63);
}
else if(skps(p_left)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_left(63,63);
}
else if(skps(p_r1)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_CW(63,63);
}
else if(skps(p_l1)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_CCW(63,63);
}

else if(skps(p_r2)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_45(63,63);
}
else if(skps(p_l2)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_135(63,63);
}
else
{
LED_RED=1;
LED_GREEN=1;
mecanum_forward(63,63);
}
}
else if(skps(p_down)==0)
{
if(skps(p_right)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_left(63,63);
}
else if(skps(p_left)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_right(63,63);

}
else if(skps(p_r1)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_CW(63,63);
}
else if(skps(p_l1)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_CCW(63,63);
}
else if(skps(p_r2)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_315(63,63);
}
else if(skps(p_l2)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_225(63,63);
}
else
{
LED_BLUE=1;
LED_WHITE=1;
mecanum_reverse(63,63);
}
}

else if(skps(p_right)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_right(63,63);
}
else if(skps(p_left)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_left(63,63);
}
else if(skps(p_r1)==0)
{
LED_RED=1;
LED_BLUE=1;
mecanum_CW(63,63);
}
else if(skps(p_l1)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_CCW(63,63);
}
else if(skps(p_r2)==0)
{
LED_GREEN=1;
LED_WHITE=1;
mecanum_45(63,63);
}
else if(skps(p_l2)==0)
{

LED_RED=1;
LED_BLUE=1;
mecanum_135(63,63);
}
else if(skps(p_joy_ld)>=50)
{
servo1=servo1-5;
if(servo1<=200)
{
servo1=200;
}
}
else if(skps(p_joy_lu)>=50)
{
servo1=servo1+5;
if(servo1>=400)
{
servo1=400;
}
}
else if(skps(p_joyl)==0)
{
servo1=240;
}
else if(skps(p_joy_rl)>=50)
{
servo2=servo2+5;
if(servo1>=250)
{
servo2=250;
}
}

else if(skps(p_joy_rr)>=50)
{
servo2=servo2-5;
if(servo1<=200)
{
servo2=200;
}
}
else if(skps(p_cross)==0)
{
LED_BLUE=1;
TRIGGER=0;
//

tembak();
}

else if(skps(p_select)==0)
{
init_IO();
LED_BLUE=LED_GREEN=LED_WHITE=LED_RED=1;
delay(100000);
LED_BLUE=LED_GREEN=LED_WHITE=LED_RED=0;
}
else
{
init_IO();
LED_BLUE=LED_GREEN=LED_WHITE=LED_RED=0;
TRIGGER=1;
U3TXREG=0;
U4TXREG=0;
}
}
}

}
return 0;
}

voidinit_IO(void)
{
//================SET I/O AS DIGITAL======================//
ANSELA = 0;
ANSELB = 0;
ANSELC = 0;
ANSELD = 0;
ANSELE = 0;
//=================SET PIN AS INPUT========================//
//PUSH BUTTON
TRISHbits.TRISH8 = 1;
TRISHbits.TRISH10 = 1;

//=================SET PIN AS OUTPUT=======================//

//LED
TRISHbits.TRISH4 = 0;
TRISHbits.TRISH6 = 0;
TRISBbits.TRISB14 = 0;
TRISFbits.TRISF13 = 0;

//MOTOR
TRISEbits.TRISE8 = 0;
TRISEbits.TRISE9 = 0;
TRISAbits.TRISA0 = 0;
TRISKbits.TRISK0 = 0;
TRISGbits.TRISG8 = 0;
TRISKbits.TRISK1 = 0;
TRISGbits.TRISG7 = 0;

TRISBbits.TRISB5 = 0;
TRISGbits.TRISG6 = 0;
TRISBbits.TRISB4 = 0;
TRISJbits.TRISJ13 = 0;
TRISBbits.TRISB3 = 0;

//12PIN I/0
TRISHbits.TRISH12 = 0;

//TRIGGER

change_pin();
init_uart2();
init_PWM();

//U1BRG =BRGVAL;

//untuk testing UART manual

//U2BRG =BRGVAL;
U3BRG =BRGVAL;
U4BRG =BRGVAL;

//TX2=0;
TX3=0;
TX4=0;

//=================INITIALIZE TO ZERO======================//
LED_BLUE = 0;
LED_GREEN = 0;
LED_RED

= 0;

LED_WHITE = 0;

//merah//sebelumtukar
//hijau
//putih
//biru

TRIGGER=1;
M1A=M1B=M2A=M2B=M3A=M3B=M4A=M4B=M5A=M5B=0;
}

voidinit_PWM()

{
PTPER = 2404;
400kHz

/* PTPER = ((1 / 400kHz) / 1.04ns) = 2404, where

is the desired switching frequency and 1.04ns is PWM resolution. */

PTCON2bits.PCLKDIV=6;

//001 = Divide-by-64

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM1 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
IOCON1bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON1bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

*/
*/
IOCON1bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

//PDC1 =0;
DTR1

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR1 = 64;

/* Deadtime setting */

PHASE1 = 0;

/* No phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM2 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
*/
*/

IOCON2bits.PENH

= 1;

/* PWM2H is controlled by PWM module

IOCON2bits.PENL

= 0;

/* PWM2L is controlled by GPIO module

IOCON2bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

//PDC2 = 0;
DTR2

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR2 = 64;

/* Deadtime setting */

PHASE2 = 0;

/* approximately 250ns phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM3 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
IOCON3bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON3bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

*/
*/

IOCON3bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

PDC3 =0;
DTR3

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR3 = 64;

/* Deadtime setting */

PHASE3 = 0;

/* No phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM4 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
*/
*/

IOCON4bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON4bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

IOCON4bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

//PDC4 =0;
DTR4

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR4 = 64;

/* Deadtime setting */

PHASE4 = 0;

/* No phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM5 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
IOCON5bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON5bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

*/
*/
IOCON5bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

PDC5 =0;
DTR5

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR5 = 64;

/* Deadtime setting */

PHASE5 = 0;

/* No phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM6 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
*/

IOCON6bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON6bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

*/
IOCON6bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

PDC6 =0;
DTR6

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR6 = 64;

/* Deadtime setting */

PHASE6 = 0;

/* No phase shift */

/*~~~~~~~~~~~~~~~~~~~~~~~ PWM7 Configuration

~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
*/
*/

IOCON7bits.PENH

= 1;

/* PWM1H is controlled by PWM module

IOCON7bits.PENL

= 0;

/* PWM1L is controlled by GPIO module

IOCON7bits.PMOD
mode */

= 1;

/* Select Independent Output PWM

PDC7 =0;
DTR7

/* Initial Duty cycle */

= 64;

/* Deadtime setting */

ALTDTR7 = 64;

/* Deadtime setting */

PHASE7 = 0;

/* No phase shift */

//==================================//
PTCONbits.PTEN

= 1;

/* Enable the PWM Module */

voidchange_pin(void) //UART KENE TUKAR PIN

{
//U1MODEbits.UARTEN =1;
//U1STAbits.UTXEN =1;
//U2MODEbits.UARTEN =1;
//U2STAbits.UTXEN =1;
U3MODEbits.UARTEN =1;
U3STAbits.UTXEN =1;

U4MODEbits.UARTEN =1;

//MODE TX
//CONTROLL REGISTER
//dh setting ktfuntion UART

U4STAbits.UTXEN =1;

//UART 2-SKPS

//guna UART2

RPINR19bits.U2RXR=112;
RPOR1bits.RP67R=3;

//SKPS-RX 111 0000

//UART2-TX 00 0011

RPOR1bits.RP66R=27;

//UART3-TX 01 1011

RPOR0bits.RP65R=29;

//UART4-TX 01 1101

void init_uart2(void)
{
U2STAbits.URXISEL = 0;
IPC7bits.U2RXIP=5;
U2STAbits.OERR = 0;
IFS1bits.U2RXIF = 0;
IEC1bits.U2RXIE = 0;
U2MODEbits.UARTEN = 1;
U2STAbits.UTXEN = 1;
U2BRG = BRGVAL;
}

// uart function (serial communication)

//=====================================================
==================
voiduart_send(unsigned char data) //function to send out a byte via uart
{
while(U2STAbits.UTXBF == 1);

//wait for previous data to finish send out

U2TXREG = data;

//send new data

unsigned char uart_rec(void)

//function to wait for a byte receive from uart

while(U2STAbits.URXDA == 0);

//wait for data to received

U2STAbits.OERR = 0;
return U2RXREG;

//return the received data

}
// skps function (read and write data to PS2)
//=====================================================
==================
unsigned char skps(unsigned char data)
joystick
{
uart_send(data);
returnuart_rec();
}

void delay(unsigned long i)

{
for(;i>0;i--)
{}
}

voidled_blinking(void)
{
LED_BLUE = 1;
delay(100000);
LED_BLUE = 0;
delay(100000);
}

voidmecanum_forward(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{

//function to read button and

s1L = 64 + mL -x;
s1R = 192 + mR -x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +x;
s2R = 192 - mR +x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 + mL -y;
s1R = 192 + mR -y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +y;
s2R = 192 - mR +y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 + mL;
s1R = 192 + mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL;
s2R = 192 - mR;
U4TXREG = s2L;

U4TXREG = s2R;
}
}

voidmecanum_reverse(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 - mL +x;
s1R = 192 - mR +x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -x;
s2R = 192 + mR -x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 - mL +y;
s1R = 192 - mR +y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -y;
s2R = 192 + mR -y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{

s1L = 64 - mL;
s1R = 192 - mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL;
s2R = 192 + mR;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

voidmecanum_left(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 - mL +x;
s1R = 192 + mR -x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -x;
s2R = 192 - mR +x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 - mL +y;
s1R = 192 + mR -y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -y;
s2R = 192 - mR +y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 - mL;
s1R = 192 + mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL;
s2R = 192 - mR;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

voidmecanum_right(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 + mL -x;
s1R = 192 - mR +x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +x;
s2R = 192 + mR -x;
U4TXREG = s2L;

U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 + mL -y;
s1R = 192 - mR +y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +y;
s2R = 192 + mR -y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 + mL;
s1R = 192 - mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL;
s2R = 192 + mR;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

void mecanum_45(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{

s1L = 64 + mL -x;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mR +x;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 + mL -y;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mR +y;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 + mL;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mR;
s2R = 192;
U4TXREG = s2L;

U4TXREG = s2R;
}
}

void mecanum_135(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64;
s1R = 192 + mR -x;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 - mL +x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64;
s1R = 192 + mR -y;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 - mL +y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{

s1L = 64;
s1R = 192 + mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 - mL;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

void mecanum_225(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 - mL +x;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mR -x;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 - mL +y;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mR -y;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 - mL;
s1R = 192;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mR;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

void mecanum_315(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64;
s1R = 192 - mR +x;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 + mL -x;
U4TXREG = s2L;

U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64;
s1R = 192 - mR +y;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 + mL -y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64;
s1R = 192 - mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2R = 64;
s2L = 192 + mL;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

voidmecanum_CCW(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{

s1L = 64 - mL +x;
s1R = 192 + mR -x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +x;
s2R = 192 + mR -x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 - mL +y;
s1R = 192 + mR -y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +y;
s2R = 192 + mR -y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 - mL;
s1R = 192 + mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL;
s2R = 192 + mR;
U4TXREG = s2L;

U4TXREG = s2R;
}
}

voidmecanum_CW(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 + mL -x;
s1R = 192 - mR +x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -x;
s2R = 192 - mR +x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 + mL -y;
s1R = 192 - mR +y;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL -y;
s2R = 192 - mR +y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{

s1L = 64 + mL;
s1R = 192 - mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 + mL;
s2R = 192 - mR;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

void mecanum_turn1(unsigned int mL, unsigned intmR)

{
if(skps(p_triangle)==0)
{
s1L = 64 + mL -x;
s1R = 192 - mR +x;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64 - mL +x;
s2R = 192 + mR -x;
U4TXREG = s2L;
U4TXREG = s2R;
}
else if(skps(p_circle)==0)
{
s1L = 64 + mL -y;
s1R = 192 - mR +y;
U3TXREG = s1L;

U3TXREG = s1R;

s2L = 64 - mL +y;
s2R = 192 + mR -y;
U4TXREG = s2L;
U4TXREG = s2R;
}
else
{
s1L = 64 - mL;
s1R = 192 + mR;
U3TXREG = s1L;
U3TXREG = s1R;

s2L = 64;
s2R = 192;
U4TXREG = s2L;
U4TXREG = s2R;
}
}

voidmecanum_relay_forward (void)
{
M1A=1;

//forward

M1B=0;
// sp1=x;
M2A=0;

//forward

M2B=1;
// sp2=y;
M3A=1;
M3B=0;
// sp3=x;

//forward

M4A=0;

//forward

M4B=1;
// sp4=y;
sp_relay_normal();
}

voidmecanum_relay_reverse (void)
{
M1A=0;

//backward

M1B=1;
// sp1=x;
M2A=1;

//backward

M2B=0;
// sp2=y;
M3A=0;

//backward

M3B=1;
// sp3=x;
M4A=1;

//backward

M4B=0;
// sp4=y;
sp_relay_normal();
}

voidmecanum_relay_left (void)
{
M1A=0;

//backward

M1B=1;
// sp1=x;
M2A=0;

//backward

M2B=1;
// sp2=y;
M3A=1;

//forward

M3B=0;
// sp3=x;
M4A=1;

//forward

M4B=0;
// sp4=y;
sp_relay_normal();
}

voidmecanum_relay_right (void)
{
M1A=1;

//forward

M1B=0;
// sp1=x;
M2A=1;

//forward

M2B=0;
// sp2=y;
M3A=0;

//backward

M3B=1;
// sp3=x;
M4A=0;

//backward

M4B=1;
// sp4=y;
sp_relay_normal();
}

voidmecanum_relay_CW (void)
{
M1A=1;

//forward

M1B=0;
// sp1=x;
M2A=1;
M2B=0;

//forward

// sp2=y;
M3A=1;

//forward

M3B=0;
// sp3=x;
M4A=1;

//forward

M4B=0;
// sp4=y;
sp_relay_normal();
}

voidmecanum_relay_CCW (void)
{
M1A=0;

//forward

M1B=1;
// sp1=x;
M2A=0;

//forward

M2B=1;
// sp2=y;
M3A=0;

//forward

M3B=1;
// sp3=x;
M4A=0;

//forward

M4B=1;
// sp4=y;
sp_relay_normal();
}

voidsp_relay_normal(void)
{
if(skps(p_triangle)==0)
{
sp_relay_max();

}
else if(skps(p_square)==0)
{
sp_relay_slow();
}
else
{
sp1=2000;
sp2=2000;
sp3=2000;
sp4=2000;
}
}
voidsp_relay_slow(void)
{
sp1=500;
sp2=500;
sp3=500;
sp4=500;
}
voidsp_relay_max(void)
{
sp1=2500;
sp2=2500;
sp3=2500;
sp4=2500;
}

voidtembak(void)
{
TRIGGER=0;
LED_BLUE=1;

delay(10000);
TRIGGER=1;
LED_BLUE=0;
delay(10000);
TRIGGER=0;
LED_BLUE=1;
delay(10000);
TRIGGER=1;
LED_BLUE=1;
delay(10000);
}