Programming Used

#include <stdio.h>
#include <stdlib.h>
#include <reset.h>
// DSPIC33EP512MU814 Configuration Bit Settings
#include <xc.h>
// FGS
#pragma config GWRP = OFF
Segment may be written)
#pragma config GSS = OFF
Segment Code protect is disabled)

// General Segment Write-Protect bit (General

// General Segment Code-Protect bit (General

#pragma config GSSK = OFF

// General Segment Key bits (General Segment
Write Protection and Code Protection is Disabled)
// FOSCSEL
#pragma config FNOSC = PRI
Oscillator (XT, HS, EC))

// Oscillator Source Selection bits (Primary

#pragma config IESO = ON

// Two-speed Oscillator Start-up Enable bit (Start up
device with FRC, then switch to user-selected oscillator source)
// FOSC
#pragma config POSCMD = HS
Oscillator Mode)
#pragma config OSCIOFNC = OFF
#pragma config IOL1WAY = ON
one reconfiguration)

// Primary Oscillator Mode Select Bit (HS Crystal

// OSC2 Pin Function Bit (OSC2 is clock output)
// Peripheral pin select configuration (Allow only

#pragma config FCKSM = CSECME

// Clock Switching Mode bits (Both Clock
switching and Fail-safe Clock Monitor are enabled)
// FWDT
#pragma config WDTPOST = PS32768
#pragma config WDTPRE = PR128

// Watchdog Timer Postscaler bits (1:32,768)

// Watchdog Timer Prescaler bit (1:128)

#pragma config PLLKEN = OFF

wait for the PLL lock signal.)

// PLL Lock Wait Enable bit (Clock switch will not

#pragma config WINDIS = OFF

Timer in Non-Window mode)

// Watchdog Timer Window Enable bit (Watchdog

#pragma config FWDTEN = OFF

enabled/disabled by user software)

// Watchdog Timer Enable bit (Watchdog timer

// FPOR
#pragma config FPWRT = PWR64

// Power-on Reset Timer Value Select bits (64ms)

#pragma config BOREN = ON

(BOR is enabled)

// Brown-out Reset (BOR) Detection Enable bit

#pragma config ALTI2C1 = OFF

selected as the I/O pins for I2C1)

// Alternate I2C pins for I2C1 (SDA1/SCK1 pins are

#pragma config ALTI2C2 = OFF

SDA2/SCL2 pins)

// Alternate I2C pins for I2C2 (I2C2 mapped to

// FICD
#pragma config ICS = PGD1
// ICD Communication Channel Select bits
(Communicate on PGEC2 and PGED2)
#pragma config RSTPRI = PF
// Reset Target Vector Select bit (Device will obtain
reset instruction from Primary flash)
#pragma config JTAGEN = OFF

// JTAG Enable Bit (JTAG is disabled)

// FAS
#pragma config AWRP = OFF
may be written)
#pragma config APL = OFF
Code protect is disabled)

// Auxiliary Segment Write-protect bit (Aux Flash

// Auxiliary Segment Code-protect bit (Aux Flash

#pragma config APLK = OFF

// Auxiliary Segment Key bits (Aux Flash Write
Protection and Code Protection is Disabled)
//=====================================================
=====================================================
==========================================
//===LED===//
#define LED_RED
#define LED_GREEN
#define LED_WHITE
#define LED_BLUE

LATHbits.LATH4
LATHbits.LATH6
LATBbits.LATB14
LATFbits.LATF13

//===SWITCH===//
#define SW1

PORTHbits.RH8

#define SW2

PORTHbits.RH10

//===MOTOR===//
#define servo1

PDC2

//SERVO 1

#define servo2

PDC4

//SERVO 2

#define M1A

LATGbits.LATG6

//MOTOR 5

#define M1B
#define sp1

LATJbits.LATJ13
PDC6

#define M2A

LATGbits.LATG8

#define M2B

LATGbits.LATG7

#define sp2

PDC5

#define M3A

LATEbits.LATE8

#define M3B

LATAbits.LATA0

#define sp3

// max 2500 PDC

//MOTOR 3

PDC3

#define M4A

LATKbits.LATK0

#define M4B

LATEbits.LATE9

#define sp4

//MOTOR 2

PDC7

#define M5A

LATBbits.LATB5

#define M5B

LATKbits.LATK1

#define sp5

//MOTOR 4

PDC1

//MOTOR 1

//PDC4 tukaruntuk servo1

#define M6A

LATBbits.LATB3

#define M6B

LATBbits.LATB4

//MOTOR KANAN

//=======12PIN I/O========//
#define TRIGGER

LATHbits.LATH12

//====================UART====================//
#define TX3

U3TXREG

#define TX4

U4TXREG

//===========BAURATE==============//
#define BRGVAL

((FP/BAUDRATE)/16)-1

#define BAUDRATE 9600

#define FP

5000000

//===========skps protocol==============//
#define

p_select

#define p_joyl

0
1

#define p_joyr

#define p_start

#define p_up

#define p_right

#define p_down

#define p_left

#define

p_l2

#define

p_r2

#define p_l1

10

#define p_r1

11

#define p_triangle

12

#define p_circle

13

#define p_cross

14

#define

15

p_square

#define p_joy_lx
#define

16

p_joy_ly

17

#define p_joy_rx

18

#define p_joy_ry

19

#define p_joy_lu

20

#define p_joy_ld

21

#define p_joy_ll

22

#define p_joy_lr

23

#define p_joy_ru

24

#define p_joy_rd

25

#define p_joy_rl

26

#define p_joy_rr

27

#define

p_con_status

28

#define p_motor1

29

#define p_motor2

30

//=======================================//
voidinit_IO(void);
voidinit_PWM(void);
voidchange_pin(void);
//====function mecanum using sabertooth====//
voidmecanum_forward(unsigned int mL, unsigned intmR);
voidmecanum_reverse(unsigned int mL, unsigned intmR);
voidmecanum_left(unsigned int mL, unsigned intmR);
voidmecanum_right(unsigned int mL, unsigned intmR);
void mecanum_45(unsigned int mL, unsigned intmR);
void mecanum_135(unsigned int mL, unsigned intmR);
void mecanum_225(unsigned int mL, unsigned intmR);
void mecanum_315(unsigned int mL, unsigned intmR);
voidmecanum_CW(unsigned int mL, unsigned intmR);
voidmecanum_CCW(unsigned int mL, unsigned intmR);
void mecanum_turn1(unsigned int mL, unsigned intmR);
//====function mecanum using relay====//
voidmecanum_relay_forward (void);
voidmecanum_relay_reverse (void);
voidmecanum_relay_right (void);
voidmecanum_relay_left (void);
voidmecanum_relay_CW (void);
voidmecanum_relay_CCW (void);
voidsp_relay_max(void); //function speed max/normal/slow
voidsp_relay_slow(void);
voidsp_relay_normal(void);
voidtembak (void);
//====skps====//
voiduart_send(unsigned char data);
unsigned char uart_rec(void);

unsigned char skps(unsigned char data);

void init_uart2(void);
//====function tambahan====//
void delay(unsigned long i);
voidled_blinking(void);
unsigned char s1L;
unsigned char s1R;
unsigned char s2L;
unsigned char s2R;
unsignedint static x = 30;
unsignedint static y = 55;

int main(void)
{
WDTSWDisable;
init_IO();
TRIGGER=1;
U3TXREG=0;
U4TXREG=0;
delay(1000);
TRIGGER=1;
LED_RED=1;
servo1=250; //trigger
servo2=190;

//gripper

while(1)
{
if(skps(p_start)==0)
{
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 */
PDC3 =0;
DTR3

= 64;

= 1;

/* Select Independent Output PWM

/* Initial Duty cycle */
/* 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 */
PDC6 =0;

= 1;

/* Select Independent Output PWM

/* Initial Duty cycle */

DTR6

= 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;

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

//U2STAbits.UTXEN =1;
U3MODEbits.UARTEN =1;
U3STAbits.UTXEN =1;

U4MODEbits.UARTEN =1;
U4STAbits.UTXEN =1;

//UART 2-SKPS
RPINR19bits.U2RXR=112;

//guna UART2
//SKPS-RX 111 0000

RPOR1bits.RP67R=3;

//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)
{
s1L = 64 + mL -x;
s1R = 192 + mR -x;
U3TXREG = s1L;
U3TXREG = s1R;

//function to read button and

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;

//forward

M3B=0;
// sp3=x;
M4A=0;
M4B=1;
// sp4=y;
sp_relay_normal();

//forward

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;
M4B=0;

//forward

// 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;

//forward

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

//forward

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);
}