I have set up each sensor as a seperate function so I can only call the part of the code that I need. It runs pretty fast but is a little jumpy. I want to still figure out a way to get a more variable turn on the servo. I think that will be by smoothing those sensors and then getting an avg. difference. Somehow using that number to add or subtract the degree of the turn from center position
The speed of the motor is based on the distance from an object. Still need to work out the situation where it will go very slow and just how slow the actual motor can spin while attached to a lego car. Not sure about that either at the moment.
 |
| The sharp IR sensor and Arduino Board |
The code is below. Such fun pondering this stuff.
---------------------------------------
// for the IR Sensor
int IRpin = 0; // analog pin for reading the IR sensor
float volts = 0; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float forward_distance = 0; // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
// for both ping sensors
#include <NewPing.h>
int p_d = 0; //the value for the difference between ping sensors
#define trig_right 2 // Arduino pin tied to trigger pin on ping sensor.
#define echo_right 3 // Arduino pin tied to echo pin on ping sensor.
#define max_distance_right 200 // Maximum distanceance we want to ping for (in centimeters). Maximum sensor distanceance is rated at 400-500cm.
NewPing sonar_right(trig_right, echo_right, max_distance_right); // NewPing setup of pins and maximum distanceance.
unsigned int pingSpeed_right = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer_right = 75; // Holds the next ping time, start at 75ms to give time for the Arduino pins to stabilize.
int cm_right = 50;
#define trig_left 12 // Arduino pin tied to trigger pin on ping sensor.
#define echo_left 13 // Arduino pin tied to echo pin on ping sensor.
#define max_distance_left 200 // Maximum distanceance we want to ping for (in centimeters). Maximum sensor distanceance is rated at 400-500cm.
NewPing sonar_left(trig_left, echo_left, max_distance_left); // NewPing setup of pins and maximum distanceance.
unsigned int pingSpeed_left = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer_left = 75; // Holds the next ping time, start at 75ms to give time for the Arduino pins to stabilize.
int cm_left = 50;
// for the stearing servo
#include <Servo.h>
Servo myservo;
int center = 90; //the center position for the servo is 90 degrees
// for the dirve
int motor = 11; //When doing analog, you must use int and not define.
int motor_speed = 0;
void IR_1(){
volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
forward_distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
if(forward_distance <= 30) //this is the domain of the sensor, 30 to 140 cm.
forward_distance = 30;
if(forward_distance >= 140)
forward_distance = 140;
motor_speed = (205/110)*(forward_distance-30)+50; //this is the equation for variable motor speed.
// motor_speed = (255/110)*(forward_distance-30); //this is used for when there is an LED Test Light in Place
Serial.print(forward_distance); // print the distance
Serial.println(" cm Forward Distance");
}
void Ping_Right(){
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distanceance pings.
if (millis() >= pingTimer_right) { // pingSpeed_1 milliseconds since last ping, do another ping.
pingTimer_right += pingSpeed_right; // Set the next ping time.
cm_right = sonar_right.ping_cm(); // Send out the ping, get the results in centimeters.
if(cm_right <= 15) //this is the domain of the sensor. 15 to 60 cm.
cm_right = 15;
if(cm_right >= 60)
cm_right = 60;
Serial.print(cm_right); // Print the result (0 = outside the set distanceance range, no ping echo)
Serial.println(" cm Ping Sensor RIGHT");
delay(10);
}
}
void Ping_Left(){
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distanceance pings.
if (millis() >= pingTimer_left) { // pingSpeed_1 milliseconds since last ping, do another ping.
pingTimer_left += pingSpeed_left; // Set the next ping time.
cm_left = sonar_left.ping_cm(); // Send out the ping, get the results in centimeters.
if(cm_left <= 15) //this is the domain of the sensor. 15 to 60 cm.
cm_left = 15;
if(cm_left >= 60)
cm_left = 60;
Serial.print(cm_left); // Print the result (0 = outside the set distanceance range, no ping echo)
Serial.println(" cm Ping Sensor LEFT");
}
}
void ping_difference(){
p_d = cm_left - cm_right;
Serial.print(p_d);
Serial.println(" Ping Difference");
}
void setup() {
Serial.begin(9600); // start the serial port
myservo.attach(7);
myservo.write(50);
}
void loop() {
IR_1();
while (forward_distance == 140){ //Full Speed, Lots of Room
analogWrite(motor, 255);
myservo.write(center);
IR_1();
Serial.println("---#1 Full Speed, No Turns");
}
while (forward_distance >= 50 && forward_distance < 140){ //Variable speed, No Turns
analogWrite(motor, motor_speed);
myservo.write(center);
IR_1();
Serial.println("---#2 Variable speed, No Turns");
}
while (forward_distance >= 40 && forward_distance < 50){ //Variable speed, Little Turns
analogWrite(motor, motor_speed);
Ping_Right();
Ping_Left();
ping_difference();
if (p_d > 0){
myservo.write(center - 35);
Serial.println("---#3 Turn a Little Left");
}
else {
myservo.write(center + 35);
Serial.println("---#4 Turn a Little Right");
}
delay (700);
IR_1();
}
while (forward_distance >= 30 && forward_distance < 40){ //Variable speed, Little Turns
analogWrite(motor, motor_speed);
Ping_Right();
Ping_Left();
ping_difference();
if (p_d > 0){
myservo.write(center - 65);
Serial.println("---#6 Turn a lot Left");
}
else {
myservo.write(center + 65);
Serial.println("---#7 Turn a lot Right");
}
delay(700);
IR_1();
}
}
No comments:
Post a Comment