Code lập trình RoBo tự động

/************************&&&&&&&&&&&*************** **************

Project : ROBOT BANG TAY BAN TU DONG

Version : 19902505__1804__1

Date : 4/25/2011

Author : TRAN TIEN PHAT___MSSV 08101088__DIEN.TU

Company : SPKT

Comments: ROBOT BANG TAY BAN TU DONG

Chip type : ATmega32

Program type : Application

Clock frequency : 16.000000 MHZ NGOAI

Memory model : Small_Ant

External RAM size : 0

Data Stack size : 512

************************************************** ****

UUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUU UUUU

************************************************** ****

!!!!!!!!!!!!!!!KHOI TAO & GAN CHAN CHO VDK !!!!!!!!!!!

************************************************** ****

UUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUU UU*/

#include <mega32.h>

#include <delay.h>

/// GAN CHAN DK DONG CO

#define DIR_A PORTA.0

#define DIR_B PORTB.5

#define DIR_Nangtay PORTB.7

#define DIR_Xoaydia PORTB.6 //a.2 //a.3

#define DIR_Truottay PORTA.2//b.6

#define Truottay PORTA.1

/*************************************************

=========== GAN CHAN DIEU KHIEN KHI NEN ==========

**************************************************/

#define Khi6 PORTA.4 // kep qua

#define Khi7 PORTC.0

#define Khi8 PORTC.6

#define Khi0 PORTA.3

//van khi nen xoay tru r

#define Khi1 PORTC.1 // ha tay lay nhan r

#define Khi3 PORTA.6 //kep nhan

#define Khi4 PORTC.3 // bng tay trai r

#define Khi5 PORTC.5 // Bung tay phai r

#define CongTac PINC.2 // chon san

#define CongTacChonSan PINC.4 // chon san

#define CamBienKhe PINC.7 // chon san

/************************************************** **********

=========== CAC CHAN NOI VOI TAY GAME PLAYSTASION===========

============== DUNG CHUAN PS ___1.1++ =======================

************************************************** **********/

#define data PINB.0

#define command PORTB.1

#define att PORTB.2

#define clock PORTB.4

unsigned char mode,bytedata4,bytedata5,bytemotorl,bytemotorr;

unsigned char byte1,byte2,byte3,byte4;

unsigned char byte5,byte6,byte8,byte9,byte10,byte11,bytenangtay;

unsigned char rjoyx,rjoyy,ljoyx,ljoyy;

unsigned char chuongtrinh=0;

unsigned char bittudong=0,bitbangtay=0,bittangtoc=0;

int i=200,encoderl=0,encoderr=0,j=0,h=0;

void ChuongTrinh2xanh();

/************************************************** ***********

=========== chuong trinh truyen nhan du lieu voi ============

=============== TAY GAME SONY PLAYSTASION 2 ================

************************************************** ***********/

unsigned char access(unsigned char tbyte)

{

unsigned char rbyte;

unsigned int i;

clock=1;

for(i=0;i<8;i++)

{

command=tbyte&0x01;

delay_us(2);

clock=0;

delay_us(2);

rbyte=(rbyte>>1)|(data<<7);

clock=1;

tbyte=tbyte>>1;

}

delay_us(100);

return rbyte;

}

/************************************************** ***********

=============== chuong trinh doc du lieu =================

=============== TAY GAME SONY PLAYSTASION 2 ================

************************************************** ***********/

void read_data(void)

{

Chi Tiết liên Hệ 0908090635

}

/************************************************** ***********

=================== CHUONG TRINH NGAT 1 ====================

=================== ENCODER R ====================

************************************************** ***********/

interrupt [EXT_INT1] void ext_int1_isr(void)

{

// Place your code here

if (PIND.1)

{

encoderr++;

}

else

{

encoderr--;

}

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== MOTOR R ====================

************************************************** ***********/

void MotorR(int nhan)

{ if(nhan>0)

{

OCR1A=nhan;

DIR_A=0;

}

else if(nhan<0)

{

nhan=-nhan;

OCR1A=nhan;

DIR_A=1;

}

else

{

OCR1A=0;

DIR_A=1;

}

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== MOTOR L ====================

************************************************** ***********/

void MotorL(int nhan)

{ if(nhan>0)

{

OCR1B=nhan;

DIR_B=0;

}

else if(nhan<0)

{

nhan=-nhan;

OCR1B=nhan;

DIR_B=1;

}

else

{

OCR1B=0;

DIR_B=1;

}

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== XOAYTHAN NGANG ====================

************************************************** ***********/

void XoayNgang()

{

Khi0=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== XOAY THAN THANG ====================

************************************************** ***********/

void XoayThang()

{

Khi0=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== HA TAY LAY NHANG ====================

************************************************** ***********/

void HaTayLayNhang()

{

Khi1=0;

Khi7=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== NANG TAY LAY NHANG ====================

************************************************** ***********/

void NangTayLayNhang()

{

Khi1=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== KEP NHANG ====================

************************************************** ***********/

void KepNhang()

{

Khi3=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== THA NHANG ====================

************************************************** ***********/

void ThaNhang()

{

Khi3=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== BUNG TAY TRAI ====================

************************************************** ***********/

void BungTayTrai()

{

Khi4=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== KHEP TAY TRAI ====================

************************************************** ***********/

void KhepTayTrai()

{

Khi4=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== BUNG TAY PHAI ====================

************************************************** ***********/

void BungTayFai()

{

Khi5=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== KHEP TAY PHAI ====================

************************************************** ***********/

void KhepTayFai()

{

Khi5=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== KEP 3 QUA ====================

************************************************** ***********/

void KepQua()

{

Khi6=1;

Khi7=0;

Khi1=0;

Khi8=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== THA 2 QUA TRAI ====================

************************************************** ***********/

void ThaQuaTrai()

{

Khi6=0;

Khi1=1;

Khi7=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK KHI ====================

=================== THA 2 QUA PHAI ====================

************************************************** ***********/

void ThaQuaFai()

{

Khi8=0;

Khi1=1;

Khi7=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== TRUOC TAY RA ====================

************************************************** ***********/

void TruotTay()

{

DIR_Truottay=0;

Truottay=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== THU TAY LAI ====================

************************************************** ***********/

void ThuTay()

{

DIR_Truottay=1;

Truottay=0;

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== NGUNG TAY LAI ====================

************************************************** ***********/

NgungTay()

{

DIR_Truottay=1;

Truottay=1;

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== XOAY DIA ====================

************************************************** ***********/

XoayDia(int in)

{

if (in > 2 )

{

DIR_Xoaydia=0;

OCR2=90;

}

else if (in < -2 )

{

DIR_Xoaydia=1;

OCR2=90;

}

else

{

DIR_Xoaydia=1;

OCR2=0;

}

}

/************************************************** ***********

=================== CHUONG TRINH DK DC ====================

=================== NANG HA TAY ====================

************************************************** ***********/

void MotorNangtay(unsigned char dir,int nhan)

{

if (dir==0)

{

DIR_Nangtay=0;

if ((bittangtoc==0)&&(nhan>30))

{

i++;

OCR0=i/2;

delay_ms(0);

if (nhan *2)

bittangtoc=1;

}

else

{

i=200;

OCR0=nhan;

}

}

else

{

DIR_Nangtay=1;

if ((bittangtoc==0)&&(nhan>30))

{

i++;

OCR0=i/6;

if (i<nhan*8)

bittangtoc=1;

}

else

{

i=200;

OCR0=nhan;

}

}

}

/************************************************** ***********

================= CHUONG TRINH TU DONG ====================

================= CT1 SAN XANH ====================

========== AP 3 TRU DAU TIEN CHUAN BI LAY QUA PID ============

************************************************** ***********/

void ChuongTrinh1xanh()

{

int toc_do;

unsigned long timer;

encoderl=0;

encoderr=0;

i=0;

while((bittudong==1)&&(encoderl<1230))

{

read_data();// DOC TAY GAME

if(encoderl <100) // GIA TOC

{

MotorL(150);

MotorR(150);

}

if((encoderl > 100 )&&(encoderl < 730 ))

{

MotorL(243); // CHAY FULL

MotorR(230);

}

if((encoderl > 730 )&&(encoderl < 1150 ))

{

MotorL(90); // CHAY FULL

MotorR(30);

}

if((encoderl > 1150 )&&(encoderl < 1200 ))

{

MotorL(-140); // CHAY FULL

MotorR(-180);

}

if((encoderl > 1200 )&&(encoderl < 1230 ))

{

MotorL(90); // CHAY FULL

MotorR(0);

}

/////////////////////////////////

switch(byte2)

{

case 0x02:

//motor truoc tay;

TruotTay();

break;

case 0x01:

//motor TH TAY

ThuTay();

break;

default: NgungTay();

}

////////////////////////////////

if(CamBienKhe)

{

MotorNangtay(1,180);

}

else

{

MotorNangtay(1,30);

}

if(encoderl > 700 )

{

Khi4=0;

Khi5=0;

}

}

encoderl=0;

MotorL(0); MotorR(0);

timer=10000;

/************************************************** ***********

================= THANG PID ====================

************************************************** ***********/

while(timer-- || bittudong==1)

{

read_data(); if(byte2||bytemotorl||bytemotorr) { break; }

if(encoderl<-3 || encoderl>3)

{

toc_do = -1*encoderl*3; // #define Kp 10

if(toc_do> 240) toc_do= 100;

if(toc_do<-240) toc_do=-100;

MotorL(toc_do); MotorR(toc_do);

}

else {

MotorL(40);MotorR(0);

delay_ms(200);

MotorL(0);MotorR(0);

}

}

MotorL(0); MotorR(0);

bittudong=0;

for(j=0;j<10;j++)

{

Gamepad_setup();

delay_ms(10);

}

}

/************************************************** ***********

================= CHUONG TRINH TU DONG ====================

================= CT1 SAN DO ====================

========== AP 3 TRU DAU TIEN CHUAN BI LAY QUA PID ============

************************************************** ***********/

void ChuongTrinh1do()

{

int toc_do;

unsigned long timer;

encoderl=0;

encoderr=0;

i=0;

while((bittudong==1)&&(encoderl<1200))

{

read_data();// DOC TAY GAME

if(encoderl <100) // GIA TOC

{

MotorL(150);

MotorR(180);

}

if((encoderl > 100 )&&(encoderl < 800 ))

{

MotorL(200); // CHAY FULL

MotorR(243);

}

if((encoderl > 800 )&&(encoderl < 1200 ))

{

MotorL(50); // CHAY FULL

MotorR(90);

}

if(CamBienKhe)

{

MotorNangtay(1,180);

}

else

{

MotorNangtay(1,30);

}

if(encoderl > 700 )

{

Khi4=0;

Khi5=0;

}

}

encoderl=0;

MotorL(0); MotorR(0);

timer=100000;

/************************************************** ***********

================= THANG PID ====================

************************************************** ***********/

while(timer-- || bittudong==1)

{

read_data(); if(byte2||bytemotorr||bytemotorl) { break; }

if(encoderl<-3 || encoderl>3)

{

toc_do = -1*encoderl*3; // #define Kp 10

if(toc_do> 240) toc_do= 100;

if(toc_do<-240) toc_do=-100;

MotorL(toc_do); MotorR(toc_do);

}

else {MotorL(0);MotorR(0);}

}

MotorL(0); MotorR(0);

bittudong=0;

for(j=0;j<10;j++)

{

Gamepad_setup();

delay_ms(10);

}

}

void thalua()

{

int tam1=0,tam2=0,tam3=0,tam4=0,tam5=0;

int dem1,dem2,dem3,dem4,dem5;

int CB1,CB2,CB3,CB4,CB5;

while(1)

dem1=0;dem2=0;dem3=0;dem4=0;dem5=0;

{

while(1)

{

if(CB1==0)

{

tam1=1;

}

if(CB2==0)

{

tam2=1;

}

if(CB3==0)

{

tam3=1;

}

if(CB4==0)

{

tam4=1;

}

if(CB5==0)

{

tam5=1;

}

///////////////////////////////

if ((CB1==1)&&(tam1==1))

{

tam1=0;

dem1++;

}

if ((CB2==1)&&(tam2==1))

{

tam2=0;

dem2++;

}

if ((CB3==1)&&(tam3==1))

{

tam3=0;

dem3++;

}

if ((CB4==1)&&(tam4==1))

{

tam4=0;

dem4++;

}

if ((CB5==1)&&(tam5==1))

{

tam5=0;

dem5++;

}

//////////////////////

if(dem1==3|| dem2==3 || dem3==3 || dem4==3 || dem5==3)

{

break;

}

}

if(

}

}

/************************************************** ***********

================= CHUONG TRINH TU DONG ====================

================= CT2 SAN XANH ====================

================= LAY QUA ====================

========== AP 3 TRU THU 2 CHUAN BI THA QUA PID ============

************************************************** ***********/

void ChuongTrinh2xanh()

{

NgungTay();

KepQua();

/////////////////////////////////

DIR_Nangtay=1;

OCR0=230;

/////////////////////////////////

delay_ms(450);

// VUA THU VUA NANG

TruotTay(); // vi nguoc

while(CamBienKhe)

{

Khi4=1; // khep tay lai

//Khi5=1;

MotorL(180);

MotorR(40);

MotorNangtay(1,230);

}

MotorNangtay(1,0);

XoayThang();

encoderl=0;

encoderr=0;

while((bittudong)&&(encoderl<100))

{

read_data();

MotorL(180);

MotorR(150);

}

NgungTay();

MotorNangtay(1,30);

while((bittudong)&&(encoderl<200))

{

MotorNangtay(0,90);

read_data();

MotorL(200);

MotorR(150);

}

MotorNangtay(1,30);

while((bittudong)&&(encoderl<200))

{

read_data();

MotorL(200);

MotorR(200);

}

while((bittudong)&&(encoderl<1000))

{

read_data();

MotorL(200);

MotorR(200);

}

if (bittudong)

{ read_data();

MotorL(-90);

MotorR(-90);

delay_ms(300);

}

MotorL(0);

MotorR(0);

chuongtrinh=3;

bittudong=0;

for(j=0;j<10;j++)

{

Gamepad_setup();

delay_ms(10);

}

}

/************************************************** ***********

================= CHUONG TRINH TU DONG ====================

================= CT2 SAN DO ====================

================= LAY QUA ====================

========== AP 3 TRU THU 2 CHUAN BI THA QUA PID ============

************************************************** ***********/

void ChuongTrinh2do()

{

NgungTay();

KepQua();

/////////////////////////////////

DIR_Nangtay=1;

OCR0=230;

/////////////////////////////////

delay_ms(450);

// VUA THU VUA NANG

DIR_Nangtay=1;

OCR0=230;

TruotTay(); // vi nguoc

while(CamBienKhe)

{

// Khi4=1; // khep tay lai

Khi5=1;

MotorNangtay(1,230);

MotorR(190);

MotorL(90);

}

MotorNangtay(1,30);

XoayThang();

encoderl=0;

encoderr=0;

while((bittudong)&&(encoderl<100))

{

read_data();

MotorR(230);

MotorL(0);

}

NgungTay();

while((bittudong)&&(encoderl<200))

{

MotorNangtay(0,90);

read_data();

MotorR(230);

MotorL(100);

}

MotorNangtay(1,30);

while((bittudong)&&(encoderl<200))

{

read_data();

MotorR(200);

MotorL(200);

}

while((bittudong)&&(encoderl<1000))

{

read_data();

MotorR(200);

MotorL(200);

}

if (bittudong)

{ read_data();

MotorR(-90);

MotorL(-90);

delay_ms(300);

}

MotorL(0);

MotorR(0);

chuongtrinh=3;

bittudong=0;

for(j=0;j<10;j++)

{

Gamepad_setup();

delay_ms(10);

}

}

/************************************************** ***********

================= CHUONG TRINH TU DONG ====================

================= XET DIEU KIEN ====================

================= CAC BIT CT ====================

************************************************** ***********/

void ChuongTrinhTuDong()

{

if (bittudong==1)

{

switch(chuongtrinh)

{

case 1:

if(CongTacChonSan==0)

{

ChuongTrinh1xanh();

}

else

{

ChuongTrinh1do();

}

break;

case 2:

if(CongTacChonSan==0)

{

ChuongTrinh2xanh();

}

else

{

ChuongTrinh2do();

}

break;

default:

bittudong=0;

}

}

}

/************************************************** ***********

================= CHUONG TRINH BANG TAY ====================

************************************************** ***********/

void ChuongTrinhBangTay()

{

int i=1;

int tam=0;

int hehe=1;

read_data();

if (bittudong==0)

{

switch(bytemotorl)

{

case 0x02:

//motor trai chay toi

MotorL(249);

break;

case 0x01:

//motor trai chay lui

MotorL(-190);

break;

default: MotorL(0);

}

switch(bytemotorr)

{

case 0x02:

//motor fai chay toi

MotorR(249);

break;

case 0x01:

//motor fai chay lui

MotorR(-190);

break;

default: MotorR(0);

}

switch(bytenangtay) //bytenangtay=x |_| tac dong muc cao

{ // dieu khien nangtay

case 0x02:

//nhan x -xoay trai

MotorNangtay(1,200);

break;

case 0x01:

//nhan select -xoay fai

MotorNangtay(0,130);

break;

default: MotorNangtay(1,30);

}

if (bitbangtay==0) //chuong trinh bang tay trong giai doan lay 3 chau nhang

{

switch(byte2)

{

case 0x02:

//motor truoc tay;

TruotTay();

break;

case 0x01:

//motor TH TAY

ThuTay();

break;

default: NgungTay();

}

switch(byte5)

{

case 0x01:

//tha 2 qua phai

ThaQuaFai();

break;

case 0x02:

//NHAN VUONG THA 2 QUA

ThaQuaTrai();

break;

}

switch(byte4)

{

case 0x02:

//tha 2 qua phai

if(hehe=1)

{

KepQua();

hehe=2;

}

else if(hehe=2)

{

ThaQuaTrai();

ThaQuaFai();

hehe=1;

}

break;

case 0x01:

if(CongTacChonSan==0)

{

ChuongTrinh2xanh();

}

else

{

ChuongTrinh2do();

}

break;

}

//////////////////////////////////////////////

// khep tay

/////////////////////////////

if(rjoyy > 200)

{

BungTayFai();

}

else if(rjoyy < 60)

{

KhepTayFai();

}

if(ljoyy > 200)

{

BungTayTrai();

}

else if(ljoyy < 60)

{

KhepTayTrai();

}

///////////////////////////////

if(ljoyx > 200)

{

XoayNgang();

}

else if(ljoyx < 60)

{

XoayThang();

}

}

else //chuong trinh bang tay lay nhang va lay de

{

if(tam =0)

{

for(j=0;j<10;j++)

{

tam++;

Gamepad_setup();

delay_ms(10);

}

}

BungTayFai(); // VI NGUOC

BungTayTrai();

// ThaQuaTrai();

//ThaQuaFai();

KepQua();

HaTayLayNhang(); // nguoc

/////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////

switch(bytemotorl)

{

case 0x02:

//motor trai chay toi

MotorL(249);

break;

case 0x01:

//motor trai chay lui

MotorL(-240);

break;

default: MotorL(0);

}

switch(bytemotorr)

{

case 0x02:

//motor fai chay toi

MotorR(249);

break;

case 0x01:

//motor fai chay lui

MotorR(-240);

break;

default: MotorR(0);

}

switch(bytenangtay) //bytenangtay=x |_| tac dong muc cao

{ // dieu khien nangtay

case 0x02:

//nhan x -xoay trai

MotorNangtay(1,200);

break;

case 0x01:

//nhan select -xoay fai

MotorNangtay(0,90);

break;

default: MotorNangtay(1,30);

}

switch(byte2)

{

case 0x02:

//motor truoc tay;

TruotTay();

break;

case 0x01:

//motor TH TAY

ThuTay();

break;

default: NgungTay();

}

switch(byte5)

{

case 0x02:

KepNhang();

break;

case 0x01:

ThaNhang();

break;

}

switch(byte4)

{

case 0x02:

XoayDia(10);

break;

case 0x01:

XoayDia(-10);

break;

default: XoayDia(0);

}

//++++++++++++++++++++++++++++++++++++++++++++++++++

if(rjoyy > 200)

{

BungTayFai();

}

else if(rjoyy < 60)

{

KhepTayFai();

}

//++++++++++++++++++++++++++++++++++++++++++++++++++

if(ljoyy > 200)

{

BungTayTrai();

}

else if(ljoyy < 60)

{

KhepTayTrai();

}

//++++++++++++++++++++++++++++++++++++++++++++++++

if(ljoyx > 200)

{

XoayNgang();

}

else if(ljoyx < 60)

{

XoayThang();

}

}

}

}

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

void ReStart()

{

XoayNgang();

//delay_ms(500);

KhepTayFai();

KhepTayTrai();

ThaQuaTrai();

ThaQuaFai();

}

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

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

void main(void)

{

PORTA=0x01;

DDRA=0xFF;

PORTB=0x00;

DDRB=0xFE;

PORTC=0x94;

DDRC=0x6B;

PORTD=0x00;

DDRD=0xF0;

/************************************************** **************

=====================KHOI TAO PWM 0 AM===========================

************************************************** **************/

TCCR0=0x7A;

TCNT0=0x00;

OCR0=0x00;

/************************************************** **************

=====================KHOI TAO PWM 1 AB AM =======================

************************************************** **************/

TCCR1A=0xF1;

TCCR1B=0x0A;

TCNT1H=0x00;

TCNT1L=0x00;

ICR1H=0x00;

ICR1L=0x00;

OCR1AH=0x00;

OCR1AL=0x00;

OCR1BH=0x00;

OCR1BL=0x00;

/************************************************** **************

=====================KHOI TAO PWM 2 AM ==========================

************************************************** **************/

ASSR=0x00;

TCCR2=0x79;

TCNT2=0x00;

OCR2=0x00;

/************************************************** ***************

====================KHOI TAO NGAT NGOAI ==========================

************************************************** ***************/

GICR|=0xC0;

MCUCR=0x0A;

MCUCSR=0x00;

GIFR=0xC0;

// Timer(s)/Counter(s) Interrupt(s) initialization

TIMSK=0x40;

// Analog Comparator initialization

// Analog Comparator: Off

// Analog Comparator Input Capture by Timer/Counter 1: Off

ACSR=0x80;

SFIOR=0x00;

// CHO PHEP NGAT TOAN BO

#asm("sei")

PORTA=255; // TAT CAC PORT

PORTC=255; //

delay_ms(500);

ReStart(); //RESET BOBOT KHI KHOI DONG

/************************************************** ******

///////////BAT DAU CHUONG TRINH CHINH////////////

************************************************** *****/

while (1)

{

read_data();

ChuongTrinhBangTay();

ChuongTrinhTuDong();

};

/************************************************** *****

========== KET THUC CHUONG TRINH CHINH =================

************************************************** *****/

}

pro quá phải học hỏi thôi