Programming Used 5

8/17/2019 Programming Used 5

1/34

#defne p_con_status 28

#defne p_motor1 29

#defne p_motor2 30

//=======================================//voidinit_IO(void);

voidinit_!"(void);

voidcan$e_pin(void);

//====%unction mecanum usin$ sa&ertoot====//

voidmecanum_%or'ard(unsi$ned int m unsi$ned intm*);

voidmecanum_reverse(unsi$ned int m unsi$ned intm*);

voidmecanum_+e%t(unsi$ned int m unsi$ned intm*);

voidmecanum_ri$t(unsi$ned int m unsi$ned intm*);

void mecanum_,-(unsi$ned int m unsi$ned intm*);

void mecanum_13-(unsi$ned int m unsi$ned intm*);



voidmecanum_.!(unsi$ned int m unsi$ned intm*);

voidmecanum_..!(unsi$ned int m unsi$ned intm*);

void mecanum_turn1(unsi$ned int m unsi$ned intm*);

//====%unction mecanum usin$ re+a====//

voidmecanum_re+a_%or'ard (void);

voidmecanum_re+a_reverse (void);

voidmecanum_re+a_ri$t (void);

voidmecanum_re+a_+e%t (void);

voidmecanum_re+a_.! (void);

voidmecanum_re+a_..! (void);

voidsp_re+a_ma(void); //%unction speed ma/norma+/s+o'

voidsp_re+a_s+o'(void);

voidsp_re+a_norma+(void);

voidtem&a (void);

//====sps====//

voiduart_send(unsi$ned car data);


2/34

unsi$ned car uart_rec(void);

unsi$ned car sps(unsi$ned car data);

void init_uart2(void);

//====%unction tam&aan====//void de+a(unsi$ned +on$ i);

void+ed_&+inin$(void);

unsi$ned car s1;

unsi$ned car s1*;

unsi$ned car s2;

unsi$ned car s2*;

unsi$nedint static = 30;

unsi$nedint static = --;

int main(void)

!45!isa&+e;

init_IO();

4*I667*=1;

34*76=0;

,4*76=0;

de+a(1000);

4*I667*=1;

7_*7=1;

servo1=2-0; //tri$$er

servo2=190; //$ripper

'i+e(1)

i%(sps(p_start)==0)

7_*7=0;


3/34

4*I667*=1;

'i+e(1)

7_6*77:=1;i%(sps(p_up)==0)

i%(sps(p_ri$t)==0)

7_6*77:=1;

7_!I47=1;

mecanum_ri$t(


4/34

7_6*77:=1;

7_!I47=1;

mecanum_,-(


5/34

7_6*77:=1;

7_!I47=1;

mecanum_.!(


6/34

7_6*77:=1;

7_!I47=1;

mecanum_ri$t(


7/34

mecanum_13-(


8/34

servo2=servo2A-;

i%(servo1B=200)

servo2=200;

e+se i%(sps(p_cross)==0)

7_>7=1;

4*I667*=0;

// tem&a();

e+se i%(sps(p_se+ect)==0)

init_IO();

7_>7=7_6*77:=7_!I47=7_*7=1;

de+a(100000);

7_>7=7_6*77:=7_!I47=7_*7=0;

e+se

init_IO();

7_>7=7_6*77:=7_!I47=7_*7=0;

4*I667*=1;

34*76=0;

,4*76=0;

return 0;


9/34

voidinit_IO(void)

//================574 I/O D5 I6I4D======================//

D:57D = 0;

D:57> = 0;

D:57. = 0;

D:57 = 0;

D:577 = 0;

//=================574 I: D5 I:4========================//

//5 >44O:

4*I5&itsE4*I58 = 1;

4*I5&itsE4*I510 = 1;

//=================574 I: D5 O44=======================//

//7

4*I5&itsE4*I5, = 0;

4*I5&itsE4*I5< = 0;

4*I5>&itsE4*I5>1, = 0;

4*I5F&itsE4*I5F13 = 0;

//"O4O*

4*I57&itsE4*I578 = 0;

4*I57&itsE4*I579 = 0;

4*I5D&itsE4*I5D0 = 0;

4*I5G&itsE4*I5G0 = 0;

4*I56&itsE4*I568 = 0;

4*I5G&itsE4*I5G1 = 0;

4*I56&itsE4*I56H = 0;

4*I5>&itsE4*I5>- = 0;

4*I56&itsE4*I56< = 0;


10/34

4*I5>&itsE4*I5>, = 0;

4*I5&itsE4*I513 = 0;

4*I5>&itsE4*I5>3 = 0;

//12I: I/0

4*I5&itsE4*I512 = 0; //4*I667*

can$e_pin();

init_uart2();

init_!"();

//1>*6 =>*6JD; //untu testin$ D*4 manua+

//2>*6 =>*6JD;

3>*6 =>*6JD;

,>*6 =>*6JD;

//42=0;

43=0;

4,=0;

//=================I:I4IDIK7 4O K7*O======================//

7_>7 = 0; //mera//se&e+umtuar

7_6*77: = 0; //i?au

7_*7 = 0; //puti

7_!I47 = 0; //&iru

4*I667*=1;

"1D="1>="2D="2>="3D="3>=",D=",>="-D="->=0;

voidinit_!"()

47* = 2,0,; /L 47* = ((1 / ,00M) / 1E0,ns) = 2,0, 'ere,00M


11/34

is te desired s'itcin$ %reNuenc and 1E0,ns is !" reso+utionE L/

4.O:2&itsE.GIJ=


12/34

4*3 =


13/34

.< =0; /L Initia+ ut cc+e L/

4*< =


14/34

*I:*19&itsE2**=112; //5G5A* 111 0000

*O*1&itsE*


15/34

// sps %unction (read and 'rite data to 52)

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

unsi$ned car sps(unsi$ned car data) //%unction to read &utton and ?ostic

uart_send(data);

returnuart_rec();

void de+a(unsi$ned +on$ i)

%or(;i@0;iAA)

void+ed_&+inin$(void)

7_>7 = 1;

de+a(100000);

7_>7 = 0;

de+a(100000);

voidmecanum_%or'ard(unsi$ned int m unsi$ned intm*)

i%(sps(p_trian$+e)==0)

s1 =


16/34

34*76 = s1*;

s2 =


17/34

voidmecanum_reverse(unsi$ned int m unsi$ned intm*)


s1 =


18/34

34*76 = s1*;

s2 =


19/34

,4*76 = s2;

,4*76 = s2*;

e+se

s1 =


20/34

s1 =


21/34

34*76 = s1*;

s2 =


22/34

void mecanum_13-(unsi$ned int m unsi$ned intm*)


s1 =


23/34

34*76 = s1*;

s2* =


24/34

,4*76 = s2;

,4*76 = s2*;

e+se

s1 =


25/34

s1 =


26/34

34*76 = s1*;

s2 =


27/34

voidmecanum_.!(unsi$ned int m unsi$ned intm*)


s1 =


28/34

34*76 = s1*;

s2 =


29/34

s2* = 192 C m* A;

,4*76 = s2;

,4*76 = s2*;

e+se

s1 =


30/34

sp_re+a_norma+();

voidmecanum_re+a_reverse (void)

"1D=0; //&ac'ard

"1>=1;

// sp1=;

"2D=1; //&ac'ard

"2>=0;

// sp2=;

"3D=0; //&ac'ard

"3>=1;

// sp3=;

",D=1; //&ac'ard

",>=0;

// sp,=;

sp_re+a_norma+();

voidmecanum_re+a_+e%t (void)

"1D=0; //&ac'ard

"1>=1;

// sp1=;

"2D=0; //&ac'ard

"2>=1;

// sp2=;

"3D=1; //%or'ard

"3>=0;

// sp3=;

",D=1; //%or'ard


31/34

",>=0;

// sp,=;

sp_re+a_norma+();

voidmecanum_re+a_ri$t (void)

"1D=1; //%or'ard

"1>=0;

// sp1=;

"2D=1; //%or'ard

"2>=0;

// sp2=;

"3D=0; //&ac'ard

"3>=1;

// sp3=;

",D=0; //&ac'ard

",>=1;

// sp,=;

sp_re+a_norma+();

voidmecanum_re+a_.! (void)

"1D=1; //%or'ard

"1>=0;

// sp1=;

"2D=1; //%or'ard

"2>=0;

// sp2=;

"3D=1; //%or'ard

"3>=0;


32/34

// sp3=;

",D=1; //%or'ard

",>=0;

// sp,=;sp_re+a_norma+();

voidmecanum_re+a_..! (void)

"1D=0; //%or'ard

"1>=1;

// sp1=;

"2D=0; //%or'ard

"2>=1;

// sp2=;

"3D=0; //%or'ard

"3>=1;

// sp3=;

",D=0; //%or'ard

",>=1;

// sp,=;

sp_re+a_norma+();

voidsp_re+a_norma+(void)


sp_re+a_ma();

e+se i%(sps(p_sNuare)==0)


33/34

sp_re+a_s+o'();

e+se

sp1=2000;

sp2=2000;

sp3=2000;

sp,=2000;

voidsp_re+a_s+o'(void)

sp1=-00;

sp2=-00;

sp3=-00;

sp,=-00;

voidsp_re+a_ma(void)

sp1=2-00;

sp2=2-00;

sp3=2-00;

sp,=2-00;

voidtem&a(void)

4*I667*=0;

7_>7=1;

de+a(10000);

4*I667*=1;

7_>7=0;


34/34

de+a(10000);

4*I667*=0;

7_>7=1;

de+a(10000); 4*I667*=1;

7_>7=1;

de+a(10000);

Programming Used 5

Documents

Transcript of Programming Used 5