Last active
October 4, 2017 07:52
-
-
Save e-Gizmo/fc8dbbea9265ae1614a2a22acdd65d34 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
e-Gizmo e-BOT 4x4 Mazebot | |
(with US-100 sensor) | |
Using State Machine library of Arduino. | |
For e-BOT sample code | |
I/O Usage | |
- collision sensor inputs (not used in this demo) | |
2- colision1 as input | |
3- colision2 as input | |
4- colision3 as input | |
- line sensor inputs (not used in this demo) | |
5- linesense1 as input low on black | |
6- linesense2 as input | |
7- linesense3 as input | |
- motor control output (default PBOT pin) | |
8 - mot2 dir as output high=fwd (right motor) | |
9 - mot2 run as output | |
11 - mot1 dir as output high= fwd (left motor) | |
10 - mot1 run as output | |
- US-100 pin assignment | |
12 - Trigger | |
3 - Echo | |
- Servo pin assignment | |
6 - Signal Output | |
Modified by Amoree | |
@e-Gizmo Mechatronix Central | |
Feb 2, 2016 | |
*/ | |
#include "NewPing.h" // Runs the Ultrasonic Distance Sensor #include <SM.h>; | |
#include "SM.h" | |
// State Machine library | |
// Ultrasonic Sensor Pins | |
#define TRIGGER_PIN 12 // trigger pin on the ultrasonic sensor. | |
#define ECHO_PIN 3 // echo pin on the ultrasonic sensor. | |
#define MAX_DISTANCE 100 // Maximum distance (in centimeters). Maximum distance is rated at 400-500cm. | |
// Servo pin | |
#define SERVO_PIN 3 // to look around or neck of US -100 sensor | |
#define PIN13_LED 13 // on-board LED indicator | |
// Target Array (if target found distance) | |
#define TARGET_FOUND_ANY 0 // Values will be "1" or "0" | |
#define TARGET_LEFT 1 | |
#define TARGET_LEFT_CENTER 2 | |
#define TARGET_CENTER 3 | |
#define TARGET_RIGHT_CENTER 4 | |
#define TARGET_RIGHT 5 | |
#define TARGET_ARRAY_SIZE 6 | |
#define TARGET_TOO_CLOSE 35 | |
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Set pins and maximum distance. | |
SM eBotGo(eBotStart);//create simple statemachine | |
/*-----( Declare Variables )-----*/ | |
unsigned int uS; // Result of a ping: MicroSeconds | |
unsigned int cm; // Distance calculated for ping (0 = outside set distance range) | |
unsigned int cm_now; // For test | |
int TargetArray[TARGET_ARRAY_SIZE]; // Holds the directions a Target was found in | |
int DirectionsToLook = 4; // For LookAround() | |
int ServoDirectionData[4] = { 1600, 2000, 900, 1600}; | |
void setup() /******************* SETUP: RUNS ONCE *****************/ | |
{ | |
Serial.begin(115200); | |
delay(1000); | |
Serial.println("Gizduino e-BOT (maze) Test"); | |
pinMode(8, OUTPUT); | |
pinMode(9, OUTPUT); | |
pinMode(10, OUTPUT); | |
pinMode(11, OUTPUT); | |
pinMode(SERVO_PIN,OUTPUT); | |
pinMode(PIN13_LED,OUTPUT); | |
}//--(end setup )--- | |
/************** LOOP: RUNS CONSTANTLY **************************/ | |
void loop() | |
{ | |
EXEC(eBotGo);//run statemachine | |
delay(100); | |
}//--(end main loop )--- | |
/*----------------( Declare User-written Functions )---------------*/ | |
//------( MOTOR CONTROL FUNCTIONS )---------------- | |
void ForwardSlow() | |
{ | |
digitalWrite(8, HIGH); | |
digitalWrite(9, 128); | |
digitalWrite(10, 128); | |
digitalWrite(11, HIGH); | |
} | |
/*---------------------------*/ | |
void ForwardMedium() | |
{ | |
digitalWrite(8, HIGH); | |
digitalWrite(9, 200); | |
digitalWrite(10, 200); | |
digitalWrite(11, HIGH); | |
; | |
} | |
/*---------------------------*/ | |
void ForwardFast() | |
{ | |
digitalWrite(8, HIGH); | |
digitalWrite(9, HIGH); | |
digitalWrite(10, HIGH); | |
digitalWrite(11, HIGH); | |
} | |
/*---------------------------*/ | |
void BackwardSlow(int runspeed) | |
{ | |
digitalWrite(8, LOW); | |
digitalWrite(9, runspeed);//run2 | |
digitalWrite(10, runspeed);//run1 | |
digitalWrite(11, LOW); | |
delay(1000); | |
} | |
/*---------------------------*/ | |
void BackwardMedium(int runspeed) | |
{ | |
digitalWrite(8, LOW); | |
digitalWrite(9, runspeed); | |
digitalWrite(10, runspeed); | |
digitalWrite(11, LOW); | |
} | |
/*---------------------------*/ | |
void BackwardFast(int runspeed) | |
{ | |
digitalWrite(8, LOW); | |
digitalWrite(9, runspeed); | |
digitalWrite(10, runspeed); | |
digitalWrite(11, LOW); | |
} | |
/*---------------------------*/ | |
void Stop() | |
{ | |
digitalWrite(8, HIGH); | |
digitalWrite(9, LOW); | |
digitalWrite(10, LOW); | |
digitalWrite(11, HIGH); | |
} | |
/*---------------------------*/ | |
void SpinLeft(int runspeed) | |
{ | |
digitalWrite(8, HIGH); | |
digitalWrite(9, HIGH); | |
digitalWrite(10, HIGH); | |
digitalWrite(11, LOW); | |
delay(runspeed); | |
} | |
/*---------------------------*/ | |
void SpinRight(int runspeed) | |
{ | |
digitalWrite(8, LOW); | |
digitalWrite(9, HIGH); | |
digitalWrite(10, HIGH); | |
digitalWrite(11, HIGH); | |
delay(runspeed); | |
} | |
/*---------------------------*/ | |
unsigned int PingBlink() | |
{ | |
uS = sonar.ping(); // Send ping, get ping time in microseconds (uS). | |
cm = uS / US_ROUNDTRIP_CM; // Convert ping time to distance in cm | |
Serial.print(" cm = "); | |
Serial.print(cm,DEC); | |
if ((cm < 40) && (cm != 0)) | |
{ | |
return(cm); | |
} | |
else | |
{ | |
return(100); // No Valid Distance | |
} | |
}// end PingBlink | |
/*---------------------------*/ | |
void PointServo(int ServoAngle) | |
{ | |
for (int i=0;i<20;i++) // Send the pulse 10 times | |
{ | |
digitalWrite(SERVO_PIN,HIGH); | |
delayMicroseconds(ServoAngle); | |
digitalWrite(SERVO_PIN,LOW); | |
delay(20); | |
} | |
}// PointServo end | |
/*---------------------------*/ | |
void LookAround() // Sets next state if Target Found | |
{ | |
for(int Direction = 0; Direction < DirectionsToLook ; Direction ++) | |
{ | |
Serial.print("DIRECTION = "); | |
Serial.print(Direction,DEC); | |
PointServo(ServoDirectionData[Direction]); // Get servo pulse width from array | |
delay(100); | |
cm_now = PingBlink(); // Read the Ultrasonic distance | |
Serial.print(" cm_now = "); | |
Serial.println(cm_now,DEC); | |
if (cm_now < TARGET_TOO_CLOSE) digitalWrite(PIN13_LED,HIGH); | |
Stop(); | |
if (cm_now < 40) | |
{ | |
TargetArray[TARGET_FOUND_ANY ] = true; | |
} | |
else TargetArray[TARGET_FOUND_ANY ] = false; | |
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 0)) //LEFT | |
{ | |
TargetArray[TARGET_LEFT ] = true; | |
Serial.println("WallonLeft"); | |
} | |
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 1)) //Center | |
{ | |
TargetArray[TARGET_CENTER ] = true; | |
Serial.println("WallonCenter"); | |
} | |
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 2)) //RIGHT | |
{ | |
TargetArray[TARGET_RIGHT ] = true; | |
Serial.println("WallonRight"); | |
} | |
}// END Directions | |
}// END LookAround | |
/**************************( STATE MACHINE FUNCTIONS )******************************/ | |
State eBotStart() | |
{ | |
Serial.println("+++ eBotStart"); | |
digitalWrite(PIN13_LED,LOW); // LED Means Target Too Close | |
ForwardFast(); // Start moving forward | |
delay(25); | |
ForwardSlow(); // Start moving forward | |
delay(600); | |
eBotGo.Set(eBotStoptoLook); | |
}// END State eBotStart | |
State eBotStoptoLook() | |
{ | |
Serial.println("+++ eBotStoptoLook"); | |
for (int i = 0; i < TARGET_ARRAY_SIZE; i++) TargetArray[i] = false; | |
Stop(); // Start moving forward | |
LookAround(); // Ping Ultrasonic in different directions, Set TargetArray | |
if (TargetArray[TARGET_CENTER ] == true) eBotGo.Set(eboTargetCenter); | |
else if (TargetArray[TARGET_LEFT ] == true) eBotGo.Set(eboTargetLeft); | |
else if (TargetArray[TARGET_RIGHT ] == true) eBotGo.Set(eboTargetRight); | |
else eBotGo.Set(eBotStart); | |
}// END State eBotStart | |
State eboTargetLeft() | |
{ | |
Serial.println("***** eboTavoidleft"); | |
BackwardSlow(50); | |
SpinRight(500); | |
eBotGo.Set(eBotStart); | |
}// END State eboTavoidleft ---------------------- | |
State eboTargetCenter() | |
{ | |
Serial.println("***** eboTavoidCenter"); | |
Stop(); | |
BackwardSlow(50); | |
SpinRight(500); | |
eBotGo.Set(eBotStart); | |
} // END State eboTavoidCenter ----------------- | |
State eboTargetRight() | |
{ | |
Serial.println("***** eboTavoidRight"); | |
BackwardSlow(50); | |
SpinLeft(500); | |
eBotGo.Set(eBotStart); | |
}// END State eboTavoidRight ----------------- | |
//*********( THE END )*********** | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment