Programming Used 6

8/17/2019 Programming Used 6

1/33

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

unsigned char s2L;

unsigned char s2;

unsignedint static ! = "#;

unsignedint static y = $$;

int main(void)

%

&'&'isable;

init_*+();

*,,-=1;

."-,=#;

.0-,=#;

delay(1###);

*,,-=1;

L-'_-'=1;

servo1=2$#; //trigger

servo2=1#; //gripper

hile(1)

%

if(skps(p_start)==#)

%


2/33

L-'_-'=#;

*,,-=1;

hile(1)

% L-'_,--3=1;

if(skps(p_up)==#)

%

if(skps(p_right)==#)

%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_right(5"65");

7

else if(skps(p_left)==#)

%

L-'_-'=1;

L-'_8L.-=1;

mecanum_left(5"65");

7

else if(skps(p_r1)==#)

%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_9&(5"65");

7

else if(skps(p_l1)==#)

%

L-'_-'=1;

L-'_8L.-=1;

mecanum_99&(5"65");

7



3/33

%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_0$(5"65"); 7


%

L-'_-'=1;

L-'_8L.-=1;

mecanum_1"$(5"65");

7

else

%

L-'_-'=1;

L-'_,--3=1;

mecanum_forard(5"65");

7

7

else if(skps(p_don)==#)

%

if(skps(p_right)==#)

%

L-'_,--3=1;

L-'_&4*-=1;


7


%

L-'_-'=1;

L-'_8L.-=1;

mecanum_right(5"65");

7


4/33


%

L-'_,--3=1;

L-'_&4*-=1;mecanum_9&(5"65");

7


%

L-'_-'=1;

L-'_8L.-=1;

mecanum_99&(5"65");

7


%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_"1$(5"65");

7


%

L-'_-'=1;

L-'_8L.-=1;

mecanum_22$(5"65");

7

else

%

L-'_8L.-=1;

L-'_&4*-=1;

mecanum_reverse(5"65");

7

7

else if(skps(p_right)==#)


5/33

%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_right(5"65"); 7


%

L-'_-'=1;

L-'_8L.-=1;


7


%

L-'_-'=1;

L-'_8L.-=1;

mecanum_9&(5"65");

7


%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_99&(5"65");

7


%

L-'_,--3=1;

L-'_&4*-=1;

mecanum_0$(5"65");

7


%

L-'_-'=1;


6/33

L-'_8L.-=1;

mecanum_1"$(5"65");

7

else if(skps(p_:oy_ld)=$#) %

servo1=servo1$;

if(servo1=0##)

%

servo1=0##;

7

7

else if(skps(p_:oyl)==#)

%

servo1=20#;

7

else if(skps(p_:oy_rl)=$#)

%

servo2=servo2>$;

if(servo1=2$#)

%

servo2=2$#;

7

7


7/33

else if(skps(p_:oy_rr)=$#)

%

servo2=servo2


8/33

return #;

7

voidinit_*+(void)%

//================- */+ ? '*,*?L======================//

?3-L? = #;

?3-L8 = #;

?3-L9 = #;

?3-L' = #;

?3-L- = #;

//=================- @*3 ? *3@.========================//

//@.4 8.+3

*4bitsA*4B = 1;

*4bitsA*41# = 1;

//=================- @*3 ? +.@.=======================//

//L-'

*4bitsA*40 = #;

*4bitsA*45 = #;

*8bitsA*810 = #;

*CbitsA*C1" = #;

//D++

*-bitsA*-B = #;

*-bitsA*- = #;

*?bitsA*?# = #;

*EbitsA*E# = #;

*,bitsA*,B = #;

*EbitsA*E1 = #;

*,bitsA*,F = #;

*8bitsA*8$ = #;


9/33

*,bitsA*,5 = #;

*8bitsA*80 = #;

*GbitsA*G1" = #;

*8bitsA*8" = #;

//12@*3 */#

*4bitsA*412 = #; //*,,-

change_pin();

init_uart2();

init_@&D();

//.18, =8,H?L; //untuk testing .? manual

//.28, =8,H?L;

."8, =8,H?L;

.08, =8,H?L;

//2=#;

"=#;

0=#;

//=================*3**?L*I- + I-+======================//

L-'_8L.- = #; //merah//sebelumtukar

L-'_,--3 = #; //hi:au

L-'_-' = #; //putih

L-'_&4*- = #; //biru

*,,-=1;

D1?=D18=D2?=D28=D"?=D"8=D0?=D08=D$?=D$8=#;

7

voidinit_@&D()

%


10/33

@@- = 20#0; /J @@- = ((1 / 0##k4K) / 1A#0ns) = 20#06 here0##k4K

is the desired sitching freuency and 1A#0ns is @&D resolutionA J/

@9+32bitsA@9LE'*H=5; //##1 = 'ivide


11/33

*+9+3"bitsA@D+' = 1; /J elect *ndependent +utput @&Dmode J/

@'9" =#; /J *nitial 'uty cycle J/

'" = 50; /J 'eadtime setting J/

?L'" = 50; /J 'eadtime setting J/

@4?-" = #; /J 3o phase shift J/

/JMMMMMMMMMMMMMMMMMMMMMMM @&D0 9onNgurationMMMMMMMMMMMMMMMMMMMMMMMMMMMMJ/

*+9+30bitsA@-34 = 1; /J @&D14 is controlled by @&D moduleJ/

*+9+30bitsA@-3L = #; /J @&D1L is controlled by ,@*+ module

J/

*+9+30bitsA@D+' = 1; /J elect *ndependent +utput @&Dmode J/

//@'90 =#; /J *nitial 'uty cycle J/

'0 = 50; /J 'eadtime setting J/

?L'0 = 50; /J 'eadtime setting J/

@4?-0 = #; /J 3o phase shift J/

/JMMMMMMMMMMMMMMMMMMMMMMM @&D$ 9onNgurationMMMMMMMMMMMMMMMMMMMMMMMMMMMMJ/

*+9+3$bitsA@-34 = 1; /J @&D14 is controlled by @&D moduleJ/

*+9+3$bitsA@-3L = #; /J @&D1L is controlled by ,@*+ moduleJ/

*+9+3$bitsA@D+' = 1; /J elect *ndependent +utput @&Dmode J/

@'9$ =#; /J *nitial 'uty cycle J/

'$ = 50; /J 'eadtime setting J/

?L'$ = 50; /J 'eadtime setting J/

@4?-$ = #; /J 3o phase shift J/

/JMMMMMMMMMMMMMMMMMMMMMMM @&D5 9onNgurationMMMMMMMMMMMMMMMMMMMMMMMMMMMMJ/

*+9+35bitsA@-34 = 1; /J @&D14 is controlled by @&D moduleJ/


12/33

*+9+35bitsA@-3L = #; /J @&D1L is controlled by ,@*+ moduleJ/

*+9+35bitsA@D+' = 1; /J elect *ndependent +utput @&Dmode J/

@'95 =#; /J *nitial 'uty cycle J/

'5 = 50; /J 'eadtime setting J/

?L'5 = 50; /J 'eadtime setting J/

@4?-5 = #; /J 3o phase shift J/

/JMMMMMMMMMMMMMMMMMMMMMMM @&DF 9onNgurationMMMMMMMMMMMMMMMMMMMMMMMMMMMMJ/

*+9+3FbitsA@-34 = 1; /J @&D14 is controlled by @&D module

J/

*+9+3FbitsA@-3L = #; /J @&D1L is controlled by ,@*+ moduleJ/

*+9+3FbitsA@D+' = 1; /J elect *ndependent +utput @&Dmode J/

@'9F =#; /J *nitial 'uty cycle J/

'F = 50; /J 'eadtime setting J/

?L'F = 50; /J 'eadtime setting J/

@4?-F = #; /J 3o phase shift J/

//==================================//

@9+3bitsA@-3 = 1; /J -nable the @&D Dodule J/

7

voidchange_pin(void) //.? E-3- .E? @*3

%

//.1D+'-bitsA.?-3 =1; //D+'-

//.1?bitsA.-3 =1; //9+3+LL -,*-

//.2D+'-bitsA.?-3 =1; //dh setting ktfuntion .?

//.2?bitsA.-3 =1;

."D+'-bitsA.?-3 =1;

."?bitsA.-3 =1;

.0D+'-bitsA.?-3 =1;


13/33

.0?bitsA.-3 =1;

//.? 2


14/33

hile(.2?bitsA.'? == #); //ait for data to received

.2?bitsA+- = #;

return .2-,; //return the received data

7// skps function (read and rite data to @2)

//=======================================================================

unsigned char skps(unsigned char data) //function to read button and :oystick

%

uart_send(data);

returnuart_rec();

7

void delay(unsigned long i)

%

for(;i#;i


15/33

s1L = 50 > mL

s1 = 12 > m

."-, = s1L;

."-, = s1;

s2L = 50 < mL >!;

s2 = 12 < m >!;

.0-, = s2L;

.0-, = s2;

7

else if(skps(p_circle)==#)

%

s1L = 50 > mL m y;

s2 = 12 < m >y;

.0-, = s2L;

.0-, = s2;

7

else

%

s1L = 50 > mL;

s1 = 12 > m;

."-, = s1L;

."-, = s1;

s2L = 50 < mL;

s2 = 12 < m;

.0-, = s2L;


16/33

.0-, = s2;

7

7

voidmecanum_reverse(unsigned int mL6 unsigned intm)

%

if(skps(p_triangle)==#)

%

s1L = 50 < mL >!;

s1 = 12 < m >!;

."-, = s1L;

."-, = s1;

s2L = 50 > mL

s2 = 12 > m

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 < mL >y;

s1 = 12 < m >y;

."-, = s1L;

."-, = s1;

s2L = 50 > mL m


17/33

s1L = 50 < mL;

s1 = 12 < m;

."-, = s1L;

."-, = s1;

s2L = 50 > mL;

s2 = 12 > m;

.0-, = s2L;

.0-, = s2;

7

7

voidmecanum_left(unsigned int mL6 unsigned intm)

%


%

s1L = 50 < mL >!;

s1 = 12 > m

."-, = s1L;

."-, = s1;

s2L = 50 > mL

s2 = 12 < m >!;

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 < mL >y;

s1 = 12 > m


18/33

s2L = 50 > mL y;

.0-, = s2L; .0-, = s2;

7

else

%

s1L = 50 < mL;

s1 = 12 > m;

."-, = s1L;

."-, = s1;

s2L = 50 > mL;

s2 = 12 < m;

.0-, = s2L;

.0-, = s2;

7

7

voidmecanum_right(unsigned int mL6 unsigned intm)

%


%

s1L = 50 > mL

s1 = 12 < m >!;

."-, = s1L;

."-, = s1;

s2L = 50 < mL >!;

s2 = 12 > m

.0-, = s2L;


19/33

.0-, = s2;

7


% s1L = 50 > mL y;

."-, = s1L;

."-, = s1;

s2L = 50 < mL >y;

s2 = 12 > m mL;

s1 = 12 < m;

."-, = s1L;

."-, = s1;

s2L = 50 < mL;

s2 = 12 > m;

.0-, = s2L;

.0-, = s2;

7

7

void mecanum_0$(unsigned int mL6 unsigned intm)

%


%


20/33

s1L = 50 > mL

s1 = 12;

."-, = s1L;

."-, = s1;

s2L = 50 < m >!;

s2 = 12;

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 > mL y;

s2 = 12;

.0-, = s2L;

.0-, = s2;

7

else

%

s1L = 50 > mL;

s1 = 12;

."-, = s1L;

."-, = s1;

s2L = 50 < m;

s2 = 12;

.0-, = s2L;


21/33

.0-, = s2;

7

7

void mecanum_1"$(unsigned int mL6 unsigned intm)

%


%

s1L = 50;

s1 = 12 > m

."-, = s1L;

."-, = s1;

s2 = 50;

s2L = 12 < mL >!;

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50;

s1 = 12 > m y;

.0-, = s2L;

.0-, = s2;

7

else

%


22/33

s1L = 50;

s1 = 12 > m;

."-, = s1L;

."-, = s1;

s2 = 50;

s2L = 12 < mL;

.0-, = s2L;

.0-, = s2;

7

7

void mecanum_22$(unsigned int mL6 unsigned intm)

%


%

s1L = 50 < mL >!;

s1 = 12;

."-, = s1L;

."-, = s1;

s2L = 50 > m

s2 = 12;

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 < mL >y;

s1 = 12;

."-, = s1L;

."-, = s1;


23/33

s2L = 50 > m m;

s2 = 12;

.0-, = s2L;

.0-, = s2;

7

7

void mecanum_"1$(unsigned int mL6 unsigned intm)

%


%

s1L = 50;

s1 = 12 < m >!;

."-, = s1L;

."-, = s1;

s2 = 50;

s2L = 12 > mL

.0-, = s2L;


24/33

.0-, = s2;

7


% s1L = 50;

s1 = 12 < m >y;

."-, = s1L;

."-, = s1;

s2 = 50;

s2L = 12 > mL mL;

.0-, = s2L;

.0-, = s2;

7

7

voidmecanum_99&(unsigned int mL6 unsigned intm)

%


%


25/33

s1L = 50 < mL >!;

s1 = 12 > m

."-, = s1L;

."-, = s1;

s2L = 50 < mL >!;

s2 = 12 > m

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 < mL >y;

s1 = 12 > m y;

s2 = 12 > m m;

."-, = s1L;

."-, = s1;

s2L = 50 < mL;

s2 = 12 > m;

.0-, = s2L;


26/33

.0-, = s2;

7

7

voidmecanum_9&(unsigned int mL6 unsigned intm)

%


%

s1L = 50 > mL

s1 = 12 < m >!;

."-, = s1L;

."-, = s1;

s2L = 50 > mL

s2 = 12 < m >!;

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 > mL y;

."-, = s1L;

."-, = s1;

s2L = 50 > mL y;

.0-, = s2L;

.0-, = s2;

7

else

%


27/33

s1L = 50 > mL;

s1 = 12 < m;

."-, = s1L;

."-, = s1;

s2L = 50 > mL;

s2 = 12 < m;

.0-, = s2L;

.0-, = s2;

7

7

void mecanum_turn1(unsigned int mL6 unsigned intm)

%


%

s1L = 50 > mL

s1 = 12 < m >!;

."-, = s1L;

."-, = s1;

s2L = 50 < mL >!;

s2 = 12 > m

.0-, = s2L;

.0-, = s2;

7


%

s1L = 50 > mL y;

."-, = s1L;


28/33


29/33

D0?=#; //forard

D08=1;

// sp0=y;

sp_relay_normal();7

voidmecanum_relay_reverse (void)

%

D1?=#; //backard

D18=1;

// sp1=!;

D2?=1; //backard

D28=#;

// sp2=y;

D"?=#; //backard

D"8=1;

// sp"=!;

D0?=1; //backard

D08=#;

// sp0=y;

sp_relay_normal();

7

voidmecanum_relay_left (void)

%

D1?=#; //backard

D18=1;

// sp1=!;

D2?=#; //backard

D28=1;

// sp2=y;

D"?=1; //forard


30/33

D"8=#;

// sp"=!;

D0?=1; //forard

D08=#;// sp0=y;

sp_relay_normal();

7

voidmecanum_relay_right (void)

%

D1?=1; //forard

D18=#;

// sp1=!;

D2?=1; //forard

D28=#;

// sp2=y;

D"?=#; //backard

D"8=1;

// sp"=!;

D0?=#; //backard

D08=1;

// sp0=y;

sp_relay_normal();

7

voidmecanum_relay_9& (void)

%

D1?=1; //forard

D18=#;

// sp1=!;

D2?=1; //forard

D28=#;


31/33

// sp2=y;

D"?=1; //forard

D"8=#;

// sp"=!; D0?=1; //forard

D08=#;

// sp0=y;

sp_relay_normal();

7

voidmecanum_relay_99& (void)

%

D1?=#; //forard

D18=1;

// sp1=!;

D2?=#; //forard

D28=1;

// sp2=y;

D"?=#; //forard

D"8=1;

// sp"=!;

D0?=#; //forard

D08=1;

// sp0=y;

sp_relay_normal();

7

voidsp_relay_normal(void)

%


%

sp_relay_ma!();


32/33

7

else if(skps(p_suare)==#)

%

sp_relay_slo(); 7

else

%

sp1=2###;

sp2=2###;

sp"=2###;

sp0=2###;

7

7

voidsp_relay_slo(void)

%

sp1=$##;

sp2=$##;

sp"=$##;

sp0=$##;

7

voidsp_relay_ma!(void)

%

sp1=2$##;

sp2=2$##;

sp"=2$##;

sp0=2$##;

7

voidtembak(void)

%

*,,-=#;

L-'_8L.-=1;


33/33

Programming Used 6

Documents

Transcript of Programming Used 6