Skip to content

Commit

Permalink
28/09/17
Browse files Browse the repository at this point in the history
  • Loading branch information
@hungthuanmk
hungthuanmk committed Sep 28, 2017
1 parent 5a6a0fc commit fe66587
Show file tree
Hide file tree
Showing 2 changed files with 124 additions and 83 deletions.
Binary file added Lib/NewPing_v1.8.zip
View file
Binary file not shown.
207 changes: 124 additions & 83 deletions SumoBot/SumoBot.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,29 +7,61 @@
// https://www.facebook.com/hungthuanmk
/////////////////////////////////////////////////////

#define DEBUG true
/////////////////////////////////////////////////////
//USgreen GND->14 ECHO->15 TRIG->16 VCC->17
//USblue GND->7 ECHO->6 TRIG->5 VCC->4
//L298 IN1->8 IN2->9 IN3->10 IN4->11
//COLORgreen S0->33 S1->35 S2->39 S3->37 OUT->A15
//COLORblue S0->A0 S1->A1 S2->A7 S3->A6 OUT->A5
/////////////////////////////////////////////////////

#include <NewPing.h>

#define DEBUG true

// DEFINITION //////////////////////////////////////
// MOTOR /////////////
#define IN1 3
#define IN2 5
#define IN3 6
#define IN4 9
#define MAX_SPEED 255
#define IN1 8
#define IN2 9
#define IN3 10
#define IN4 11
#define MAX_SPEED 100
#define MIN_SPEED 0

//
#define GREEN 0
#define BLUE 1

// COLOR SENSOR /////
#define S0 A0
#define S1 A1
#define S2 A2
#define S3 A3
#define sensorOut A4

// ULTRASONIC SENSOR
#define TRIG_PIN 0
#define ECHO_PIN 1
#define TIME_OUT 5000

// COLORgreen SENSOR /////
#define S0_g 33
#define S1_g 35
#define S2_g 39
#define S3_g 37
#define sensorOut_g A15

// COLORblue SENSOR /////
#define S0_b A0
#define S1_b A1
#define S2_b A7
#define S3_b A6
#define sensorOut_b A5

// USgreen SENSOR //USgreen GND->14 ECHO->15 TRIG->16 VCC->17
#define TRIG_g 16
#define ECHO_g 15
#define MAX_DISTANCE_g 50
#define VCC_g 17
#define GND_g 14
NewPing USgreen(TRIG_g, ECHO_g, MAX_DISTANCE_g);

// USblue SENSOR //USblue GND->7 ECHO->6 TRIG->5 VCC->4
#define TRIG_b 5
#define ECHO_b 6
#define MAX_DISTANCE_b 50
#define VCC_b 4
#define GND_b 7
NewPing USblue(TRIG_b, ECHO_b, MAX_DISTANCE_b);

// OTHERS ///////////
#define rThreshold 50
#define gThreshold 50
Expand All @@ -49,27 +81,53 @@ void m1Forward(int speed);
void m1Backward(int speed);
void m2Forward(int speed);
void m2Backward(int speed);
void readColorSensor();
bool crossLine();
float getDistance();
void readColorSensor(int GREENorBLUE);
bool crossedLine();
////////////////////////////////////////////////////

void setup()
{
//motor setup
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);

//color green setup
pinMode(S0_g, OUTPUT);
pinMode(S1_g, OUTPUT);
pinMode(S2_g, OUTPUT);
pinMode(S3_g, OUTPUT);
pinMode(sensorOut_g, INPUT);
digitalWrite(S0_g, HIGH);
digitalWrite(S1_g, HIGH);

//color blue setup
pinMode(S0_b, OUTPUT);
pinMode(S1_b, OUTPUT);
pinMode(S2_b, OUTPUT);
pinMode(S3_b, OUTPUT);
pinMode(sensorOut_b, INPUT);
digitalWrite(S0_b, HIGH);
digitalWrite(S1_b, HIGH);

pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
pinMode(sensorOut, INPUT);
//ultrasonic green
pinMode(TRIG_g, OUTPUT);
pinMode(ECHO_g, INPUT);
pinMode(VCC_g, OUTPUT);
pinMode(GND_g, OUTPUT);
digitalWrite(VCC_g, HIGH);
digitalWrite(GND_g, LOW);

digitalWrite(S0, HIGH);
digitalWrite(S1, HIGH);

//ultrasonic blue
pinMode(TRIG_b, OUTPUT);
pinMode(ECHO_b, INPUT);
pinMode(VCC_b, OUTPUT);
pinMode(GND_b, OUTPUT);
digitalWrite(VCC_b, HIGH);
digitalWrite(GND_b, LOW);

// analogWrite(IN4, 50);
if (DEBUG)
Serial.begin(9600);

Expand All @@ -80,26 +138,13 @@ void setup()

void loop()
{
readColorSensor();
if (crossLine == true)
{ // SMART BACKWARD
m1Backward(MAX_SPEED);
m2Backward(MAX_SPEED);
delay(2000);
}
else // CrossLine == false == in battlefield
{
if (getDistance() <= distanceThreshold)
{ // ATTACK
m1Forward(MAX_SPEED);
m1Forward(MAX_SPEED);
}
else
{ // FIND OPPONENT // ROTATE
m1Forward(MAX_SPEED-55);
m2Backward(MAX_SPEED-55);
}
}
m2Forward(100);
// m1Backward(50);
// readColorSensor(BLUE);
//int a = USblue.ping_cm();
// if (a!=0)
// Serial.println(a);
// delay(10);
}

void m1Stop(){
Expand All @@ -126,8 +171,8 @@ void m1Backward(int speed){

void m2Forward(int speed){
speed = constrain(speed, MIN_SPEED, MAX_SPEED);
analogWrite(IN3, speed);
digitalWrite(IN4, LOW);// dont need to use PWM
analogWrite(IN4, speed);
digitalWrite(IN3, LOW);// dont need to use PWM
}

void m2Backward(int speed){
Expand All @@ -136,11 +181,24 @@ void m2Backward(int speed){
digitalWrite(IN3, HIGH);// dont need to use PWM
}

void readColorSensor(){
void readColorSensor(int GREENorBLUE){
int s2,s3, out;
if (GREENorBLUE == GREEN)
{ //green
s2 = S2_g;
s3 = S3_g;
out = sensorOut_g;
}
else //blue
{
s2 = S2_b;
s3 = S3_b;
out = sensorOut_b;
}
// Setting CLEAR filtered photodiodes to be read
digitalWrite(S2, HIGH);
digitalWrite(S3, LOW);
C = pulseIn(sensorOut, LOW);
digitalWrite(s2, HIGH);
digitalWrite(s3, LOW);
C = pulseIn(out, LOW);
// C = map(C, 2,25,255,0);
if (DEBUG)
{
Expand All @@ -151,9 +209,9 @@ void readColorSensor(){
delay(5);

// Setting RED filtered photodiodes to be read
digitalWrite(S2, LOW);
digitalWrite(S3, LOW);
R = pulseIn(sensorOut, LOW);
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
R = pulseIn(out, LOW);
//R = map(R, 25,72,255,0);
if (DEBUG)
{
Expand All @@ -164,9 +222,9 @@ void readColorSensor(){
delay(5);

// Setting GREEN filtered photodiodes to be read
digitalWrite(S2, HIGH);
digitalWrite(S3, HIGH);
G = pulseIn(sensorOut, LOW);
digitalWrite(s2, HIGH);
digitalWrite(s3, HIGH);
G = pulseIn(out, LOW);
//G = map(G, 30,90,255,0);
if (DEBUG)
{
Expand All @@ -177,9 +235,9 @@ void readColorSensor(){
delay(5);

// Setting BLUE filtered photodiodes to be read
digitalWrite(S2, LOW);
digitalWrite(S3, HIGH);
B = pulseIn(sensorOut, LOW);
digitalWrite(s2, LOW);
digitalWrite(s3, HIGH);
B = pulseIn(out, LOW);
//B = map(B, 25,70,255,0);
if (DEBUG)
{
Expand All @@ -191,28 +249,11 @@ void readColorSensor(){

}

bool crossLine(){
readColorSensor();
bool crossedLine(){
readColorSensor(BLUE);
// if ((R <= rThreshold) && (G <= gThreshold) && (B <= bThreshold))
if (C <= cThreshold)
return true;
else
return false;
}

float getDistance(){
long duration, distanceCm;

digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

duration = pulseIn(ECHO_PIN, HIGH, TIME_OUT);

// convert to distance
distanceCm = duration / 29.1 / 2;

return distanceCm;
}

0 comments on commit fe66587

Please sign in to comment.