programming used 6
TRANSCRIPT
-
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)==#)
%
-
8/17/2019 Programming Used 6
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
else if(skps(p_r2)==#)
-
8/17/2019 Programming Used 6
3/33
%
L-'_,--3=1;
L-'_&4*-=1;
mecanum_0$(5"65"); 7
else if(skps(p_l2)==#)
%
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;
mecanum_left(5"65");
7
else if(skps(p_left)==#)
%
L-'_-'=1;
L-'_8L.-=1;
mecanum_right(5"65");
7
-
8/17/2019 Programming Used 6
4/33
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
else if(skps(p_r2)==#)
%
L-'_,--3=1;
L-'_&4*-=1;
mecanum_"1$(5"65");
7
else if(skps(p_l2)==#)
%
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)==#)
-
8/17/2019 Programming Used 6
5/33
%
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-'_-'=1;
L-'_8L.-=1;
mecanum_9&(5"65");
7
else if(skps(p_l1)==#)
%
L-'_,--3=1;
L-'_&4*-=1;
mecanum_99&(5"65");
7
else if(skps(p_r2)==#)
%
L-'_,--3=1;
L-'_&4*-=1;
mecanum_0$(5"65");
7
else if(skps(p_l2)==#)
%
L-'_-'=1;
-
8/17/2019 Programming Used 6
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
-
8/17/2019 Programming Used 6
7/33
else if(skps(p_:oy_rr)=$#)
%
servo2=servo2
-
8/17/2019 Programming Used 6
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$ = #;
-
8/17/2019 Programming Used 6
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()
%
-
8/17/2019 Programming Used 6
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
-
8/17/2019 Programming Used 6
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/
-
8/17/2019 Programming Used 6
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;
-
8/17/2019 Programming Used 6
13/33
.0?bitsA.-3 =1;
//.? 2
-
8/17/2019 Programming Used 6
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
-
8/17/2019 Programming Used 6
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;
-
8/17/2019 Programming Used 6
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
else if(skps(p_circle)==#)
%
s1L = 50 < mL >y;
s1 = 12 < m >y;
."-, = s1L;
."-, = s1;
s2L = 50 > mL m
-
8/17/2019 Programming Used 6
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)
%
if(skps(p_triangle)==#)
%
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 >y;
s1 = 12 > m
-
8/17/2019 Programming Used 6
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)
%
if(skps(p_triangle)==#)
%
s1L = 50 > mL
s1 = 12 < m >!;
."-, = s1L;
."-, = s1;
s2L = 50 < mL >!;
s2 = 12 > m
.0-, = s2L;
-
8/17/2019 Programming Used 6
19/33
.0-, = s2;
7
else if(skps(p_circle)==#)
% 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)
%
if(skps(p_triangle)==#)
%
-
8/17/2019 Programming Used 6
20/33
s1L = 50 > mL
s1 = 12;
."-, = s1L;
."-, = s1;
s2L = 50 < m >!;
s2 = 12;
.0-, = s2L;
.0-, = s2;
7
else if(skps(p_circle)==#)
%
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;
-
8/17/2019 Programming Used 6
21/33
.0-, = s2;
7
7
void mecanum_1"$(unsigned int mL6 unsigned intm)
%
if(skps(p_triangle)==#)
%
s1L = 50;
s1 = 12 > m
."-, = s1L;
."-, = s1;
s2 = 50;
s2L = 12 < mL >!;
.0-, = s2L;
.0-, = s2;
7
else if(skps(p_circle)==#)
%
s1L = 50;
s1 = 12 > m y;
.0-, = s2L;
.0-, = s2;
7
else
%
-
8/17/2019 Programming Used 6
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)
%
if(skps(p_triangle)==#)
%
s1L = 50 < mL >!;
s1 = 12;
."-, = s1L;
."-, = s1;
s2L = 50 > m
s2 = 12;
.0-, = s2L;
.0-, = s2;
7
else if(skps(p_circle)==#)
%
s1L = 50 < mL >y;
s1 = 12;
."-, = s1L;
."-, = s1;
-
8/17/2019 Programming Used 6
23/33
s2L = 50 > m m;
s2 = 12;
.0-, = s2L;
.0-, = s2;
7
7
void mecanum_"1$(unsigned int mL6 unsigned intm)
%
if(skps(p_triangle)==#)
%
s1L = 50;
s1 = 12 < m >!;
."-, = s1L;
."-, = s1;
s2 = 50;
s2L = 12 > mL
.0-, = s2L;
-
8/17/2019 Programming Used 6
24/33
.0-, = s2;
7
else if(skps(p_circle)==#)
% 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)
%
if(skps(p_triangle)==#)
%
-
8/17/2019 Programming Used 6
25/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 >y;
s1 = 12 > m y;
s2 = 12 > m m;
."-, = s1L;
."-, = s1;
s2L = 50 < mL;
s2 = 12 > m;
.0-, = s2L;
-
8/17/2019 Programming Used 6
26/33
.0-, = s2;
7
7
voidmecanum_9&(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
else if(skps(p_circle)==#)
%
s1L = 50 > mL y;
."-, = s1L;
."-, = s1;
s2L = 50 > mL y;
.0-, = s2L;
.0-, = s2;
7
else
%
-
8/17/2019 Programming Used 6
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)
%
if(skps(p_triangle)==#)
%
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 y;
."-, = s1L;
-
8/17/2019 Programming Used 6
28/33
-
8/17/2019 Programming Used 6
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
-
8/17/2019 Programming Used 6
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=#;
-
8/17/2019 Programming Used 6
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)
%
if(skps(p_triangle)==#)
%
sp_relay_ma!();
-
8/17/2019 Programming Used 6
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;
-
8/17/2019 Programming Used 6
33/33