Sumo_Robot_Kit_(SKU_unknow) - jimaobian/DFRobotWiki GitHub Wiki

// # Editor    :Holiday from DFRobot
// # Data      :14.08.2013

// # Product name:Sumo Robot Kit
// # Product SKU:unknow
// # Version : unknow

// # Description:
// # This sample gives you a example of how to make your car toy into a sumo robot
// # The function "Forward(int x,int y)"may not fit your robot because of the difference of our circuits,
// # if so,you can change your code depend on the real action your motor dose
// # Connection:
// #       LEFT IR DISTANCE SENSOR -> A0 (Arduino)
// #       LEFT IR DISTANCE SENSOR -> A3 (Arduino)
// #       LEFT LINE TRACKING SENSOR -> 8 (Arduino)
// #       RIGHT LINE TRACKING SENSOR -> 9 (Arduino)
// #       10CM SWITCH SENSOR -> 10(Arduino)

#define LEFT_A  A0    //analog Left -->IR distance sensor in the left
#define RIGHT_A A3    //analog Right-->IR distance sensor in the right
#define MIDDLE 10    //10cm switch sensor in the middle ---> less than 10cm return 0
#define LEFT_D  8    //Line Tracking Sensor in the left ----> find white return 1
#define RIGHT_D 9    //Line Tracking Sensor in the right----> find white return 1
#define E1 5
#define E2 6
#define M1 4
#define M2 7
int Distance_L=0,Distance_R=0;
int Switch_L,Switch_M,Switch_R;


void setup()
{
  pinMode(LEFT_A,INPUT);
  pinMode(RIGHT_A,INPUT);
  pinMode(MIDDLE,INPUT);
  pinMode(LEFT_D,INPUT);
  pinMode(RIGHT_D,INPUT);
  for (int i=4;i<8;i++)
  pinMode(i,OUTPUT);
 Serial.begin(9600);
}

void loop()
{
 switch (Get_state())
 {
   case 0: {Turn_Left(200,200);Serial.println("L");}        break;
   case 1: {Turn_Right(200,200);Serial.println("R");}       break;
   case 2: {Turn_Forward(200,200);Serial.println("F");delay(20);}     break;
   case 3: {Turn_Forward(200,200);Serial.println("F");}     break;
   case 4: {Turn_Back(200,200);Serial.println("B");}        break;
   default :break;

 }
 //delay(200);
}

int Get_state()
{
  int St=0;
  Distance_L=analogRead(LEFT_A);
  Distance_R=analogRead(RIGHT_A);
  Switch_L  =digitalRead(LEFT_D);
  Switch_R  =digitalRead(RIGHT_D);
  Switch_M  =digitalRead(MIDDLE);
  if (Distance_L>300)  St=St+1;   //left found
  if (Distance_R>300)  St=St+2;   //
  if (Switch_L==1)  St=St+4;
  if (Switch_R==1)  St=St+8;
  if (Switch_M==0)  St=St+16;
//  Serial.println(St);
//  Serial.print( Distance_L);Serial.print("  ");
//  Serial.print( Distance_R);Serial.print("  ");
//  Serial.print(  Switch_L);Serial.print("  ");
//  Serial.print( Switch_R);Serial.print("  ");
//  Serial.println(  Switch_M);
 if (St==1 || St==8 || St==9 || St==10 || St==11)
 return 0;
 else if ( St==2 || St==4 || St==5 || St==6 || St==7 || St==20 || St==21|| St==22 || St==23)
 return 1;
 else if ( St==16 || St==17 || St==18 || St==19 || St==3 )
 return 2;
 else if(St==0)
 return 3;
 else return 4;


}

void Turn_Right(int x,int y)
{
  analogWrite (E1,x);      //PWM Speed Control
  digitalWrite(M1,HIGH);
  analogWrite (E2,y);
  digitalWrite(M2,LOW);
}

void Turn_Left(int x,int y)
{
  analogWrite (E1,x);      //PWM Speed Control
  digitalWrite(M1,LOW);
  analogWrite (E2,y);
  digitalWrite(M2,HIGH);
}

void Turn_Forward(int x,int y)
{
  analogWrite (E1,x);      //PWM Speed Control
  digitalWrite(M1,HIGH);
  analogWrite (E2,y);
  digitalWrite(M2,HIGH);
}

void Turn_Back(int x,int y)
{
  analogWrite(E1,x);analogWrite(E2,y);
  digitalWrite(M1,LOW);digitalWrite(M2,LOW);
}