Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define M1IN1 2
- #define M1IN2 4
- #define M2IN3 7
- #define M2IN4 6
- #define EnA 3
- #define EnB 5
- int Sen[]={A0,A1,A2,A3,A4};
- int ReadSen[5]={};
- int tempSen[]={1,1,0,1,1};
- int ch[]={1,1,0,1,1};
- int Speed=0,x=0;
- char lastDir='F';
- boolean array_cmp(int *a, int *b, int len_a, int len_b){
- int n;
- if (len_a != len_b) return false;
- for (n=0;n<len_a;n++) if (a[n]!=b[n]) return false;
- return true;
- }
- void setup() {
- // put your setup code here, to run once:
- for(int Motor=2;Motor<8;Motor++)// For Motor OutPut From Pin 2 to Pin 7
- {
- pinMode(Motor,OUTPUT);
- }
- pinMode(13,OUTPUT);digitalWrite(13,HIGH);//Vcc for sensor
- pinMode(12,OUTPUT);digitalWrite(12,HIGH);//Vcc for sensor
- for(int Sensor=0;Sensor<5;Sensor++)// For Sensor Input From A0 To A4
- {
- pinMode(Sen[Sensor],INPUT);
- }
- Serial.begin(9600); //Enable Serial Communications
- //////////////////////////////////////////////////////////
- for(int temp=0;temp<5;temp++)
- {
- Serial.print(digitalRead(Sen[temp]));
- ReadSen[temp]=digitalRead(Sen[temp]);
- tempSen[temp]=ReadSen[temp];
- }
- Serial.println();
- //Timer On & OverFollow interrupt
- TIMSK2 = (TIMSK2 & B11111110) | 0x01;
- TCCR2B = (TCCR2B & B11111000) | 0x07;
- }
- int MoveF(int Speed)
- {
- digitalWrite(M1IN1,HIGH);
- digitalWrite(M1IN2,LOW);
- digitalWrite(M2IN3,HIGH);
- digitalWrite(M2IN4,LOW);
- analogWrite(EnA,Speed);
- analogWrite(EnB,Speed+20);
- }
- int MoveR(int Speed)
- {
- digitalWrite(M1IN1,HIGH);
- digitalWrite(M1IN2,LOW);
- digitalWrite(M2IN3,HIGH);
- digitalWrite(M2IN4,LOW);
- analogWrite(EnA,Speed-8);
- analogWrite(EnB,0);
- }
- int MoveL(int Speed)
- {
- digitalWrite(M1IN1,HIGH);
- digitalWrite(M1IN2,LOW);
- digitalWrite(M2IN3,HIGH);
- digitalWrite(M2IN4,LOW);
- analogWrite(EnA,0);
- analogWrite(EnB,Speed-5);
- }
- int MoveS(int Speed)
- {
- digitalWrite(M1IN1,HIGH);
- digitalWrite(M1IN2,LOW);
- digitalWrite(M2IN3,HIGH);
- digitalWrite(M2IN4,LOW);
- analogWrite(EnA,0);
- analogWrite(EnB,0);
- }
- void loop() {
- // put your main code here, to run repeatedly:
- //1-11011
- if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- Serial.print("MoveF");
- MoveF(Speed);
- lastDir='F';
- }
- //////////////////////////////////////
- //Move Left
- //2-01111
- else if(ReadSen[0]==0&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- delay(250);
- //Serial.print("MoveRR");
- lastDir='L';
- MoveL(Speed+30);
- }
- //3-00111
- else if(ReadSen[0]==0&&ReadSen[1]==0&&ReadSen[2]==1&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- lastDir='L';
- MoveL(Speed);
- }
- //4-00011
- else if(ReadSen[0]==0&&ReadSen[1]==0&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- MoveL(Speed);
- lastDir='L';
- }
- //5-01011
- else if(ReadSen[0]==0&&ReadSen[1]==1&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- MoveL(Speed);
- lastDir='L';
- }
- //6-10011
- else if(ReadSen[0]==1&&ReadSen[1]==0&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- MoveL(Speed);
- lastDir='L';
- }
- //7-10111
- else if(ReadSen[0]==1&&ReadSen[1]==0&&ReadSen[2]==1&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- MoveL(Speed);
- lastDir='L';
- }
- ///////////////////////////////////////////////////////
- //Move Right
- //8-11110
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==1&&ReadSen[4]==0)
- {
- delay(250);
- MoveR(Speed+30);
- lastDir='R';
- }
- //9-11100
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==0&&ReadSen[4]==0)
- {
- MoveR(Speed);
- lastDir='R';
- }
- //10-11000
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==0&&ReadSen[3]==0&&ReadSen[4]==0)
- {
- MoveR(Speed);
- lastDir='R';
- }
- //11-11010
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==0)
- {
- MoveR(Speed);
- lastDir='R';
- }
- //12-11001
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==0&&ReadSen[3]==0&&ReadSen[4]==1)
- {
- MoveR(Speed);
- lastDir='R';
- }
- //13-11101
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==0&&ReadSen[4]==1)
- {
- MoveR(Speed);
- lastDir='R';
- }
- /////////////////////////////////////////////////////////////
- //14-11111 With Move
- else if(ReadSen[0]==1&&ReadSen[1]==0&&ReadSen[2]==0&&ReadSen[3]==0&&ReadSen[4]==0)
- {
- MoveR(Speed);
- lastDir='R';
- }
- else if(ReadSen[0]==1&&ReadSen[1]==0&&ReadSen[2]==0&&ReadSen[3]==0&&ReadSen[4]==0)
- {
- MoveL(Speed);lastDir='L';
- }
- //16-00001
- else if(ReadSen[0]==0&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==0&&ReadSen[4]==1)
- {
- MoveL(Speed);lastDir='L';
- }
- else if(ReadSen[0]==0&&ReadSen[1]==0&&ReadSen[2]==0&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- MoveL(Speed);
- lastDir='L';
- }
- //17-01110
- else if(ReadSen[0]==1&&ReadSen[1]==1&&ReadSen[2]==1&&ReadSen[3]==1&&ReadSen[4]==1)
- {
- TIMSK2 = (TIMSK2 & B1
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement