object_avoiding_code_p1.zip (471Bytes)
object_avoiding_code_p2.zip (920Bytes)
Ok everyone I had some free time and wanted to continue making my RMOAR (you can find it with a search).
So I decided to write the code from scratch cause I had an idea about its object avoiding function.
I started writing some of the code and also debugging and testing it in every step. Allthough I figured out that when
I added a line of code ,when the servo used for scanning was going left all were going smoothly when was going right
suddenly the speed was increased .So Im asking here "What Im doing wrong?" .
The code is attached to the post or can be seen below .(By the way Im using arduino for this one)
Code:
#include <Servo.h>
Servo s;
int x1,x2;
int x[179];
int i = 0;
void setup(){
pinMode(0,INPUT);
pinMode(1,INPUT);
pinMode(6,OUTPUT);
Serial.begin(9600);
s.attach(6);
}
void loop(){
scan();
}
int readSensors() {
x1 = analogRead(0);
x2 = analogRead(1);
if(x1 >= 370 || x2 >= 370) {
return 1;
}
else {
return 0;
}
}
int scan(){
for(i = 0 ; i < 180; i+=1){
s.write(i);
delay(20);
Serial.println(readSensors());
x[i] = readSensors();
}
for( i = 180 ; i >= 1; i-=1){
s.write(i);
delay(20);
Serial.println(readSensors());
}
}
(UPDATE: 03/01/2012):
Ok I just updated the code and fixed the array issue though another problem came up .
With the changes made what I expect it to do is to scan the area and print the position in which ,the biggest angle without any object in front of the bot ,starts .The problem Im facing is that it scans the area once and then stucks on the left side and keeps
printing the first value of z which is 0 .The code follows .
Code :
#include <Servo.h>
Servo s;
int x1,x2,k,z,v,a;// z is the position where the biggest angle with no obstacle in front of the bot begins. k,v,a are counters to accomplish the process of finding z .
int x[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot)
int y[180];//coutners for countring the different angle with no obstacle in front of the bot
int maxx;//max value that y[k] gets.
int i = 0;//general counter
void setup(){
//pin assignment
pinMode(0,INPUT);
pinMode(1,INPUT);
pinMode(6,OUTPUT);
Serial.begin(9600);
s.attach(6); // attaching the scanning servo to pin 6
}
void loop(){
scan();
a = 0;
k = 0 ;
z = 0 ;
maxx = 0;
for(i = 0 ; i < 180; i+=1) {
y[i] = 0;
}
for(i = 0; i < 180; i+=1) {
if(x[1] = 0) {
k = 1;
y[k] = 1;
z = 1;
}
else{
if(x[i] == 0 && x[i - 1] != 0){
k+=1;
y[k] = 1;
}
else if(x[i] == 0 && x[i - 1] == 0){
y[k] += 1;
}
}
for(i = 0 ; i < k; i+=1){
if(y[k] > maxx && y[k] != 0){
v = 0;
maxx = y[k];
for(a = 0 ; a < (k - 1); a+=1){
v = v + y[k];// sum of the previous y before y[k]
}
z = v + 1;
}
}
Serial.println(z);
delay(500);
}
}
int readSensors() {
x1 = analogRead(0);
x2 = analogRead(1);
if(x1 >= 370 || x2 >= 370) {
return 1;
}
else {
return 0;
}
}
int scan(){
for(i = 0 ; i < 180; i+=1){
s.write(i);
delay(20);
x[i] = readSensors();
}
for( i = 180 ; i >= 1; i-=1){
s.write(i);
delay(20);
}
}
(UPDATE: 04/01/2012): I fixed the for's but still getting the same result ,though now it prints 0 once and then prints 1 instead of 0 .
The code is above with the changes made .(maxx variable added for's fixed).
(UPDATE: 07/01/2012):Ok everyone ,I have fixed it and now it does what I want it to do .It finds the angle in which the biggest angle without an obstacle in front of the bot starts .The code follows and I have attached the file too.
Code :
#include <Servo.h>
Servo scan_servo;
int reading1,reading2,k,turn_to;// turn_to is the position where the biggest angle with no obstacle in front of the bot begins. k is a counter to accomplish the process of finding turn_to .
int maxx;//the maximum value of angles
int state[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot)
int angles[180];//coutners for countring the different angle with no obstacle in front of the bot
int pos[180];//An array in which is saved the angle that the
int i = 0;//general counter
void setup(){
//pin assignment
pinMode(0,INPUT);
pinMode(1,INPUT);
pinMode(6,OUTPUT);
Serial.begin(9600);
scan_servo.attach(6); // attaching the scanning servo to pin 6
}
void loop(){
scan();//scan the area.
k = 0 ;
turn_to = 0 ;
maxx = 0;
for(i = 0 ; i < 180; i+=1) {
angles[i] = 0;
}
for(i = 0; i < 180; i+=1) {//fill the angles array
if(i == 1 && state[i] == 0){
k+=1;
angles[k] = 1;
pos[k] = i;
}
else if(state[i] == 0 && state[i - 1] == 0 ){
angles[k]+=1;
}
else if(state[i] == 0 && state[i - 1] != 0){
k+=1;
angles[k] = 1;
pos[k] = i;
}
}
for(i = 0 ; i < k; i+=1){//find the biggest angles and set turn_to
if(angles[i] > maxx){
turn_to = pos[i] ;
}
}
Serial.println(turn_to);
delay(500);
}
int readSensors() {//read the sensors
reading1 = analogRead(0);
reading2 = analogRead(1);
if(reading1 >= 370 || reading2 >= 370) {
return 1;
}
else {
return 0;
}
}
int scan(){//scan the area using the scan_servo and fill the state array.
for(i = 0 ; i < 180; i+=1){
scan_servo.write(i);
delay(20);
state[i] = readSensors();
}
for( i = 180 ; i >= 1; i-=1){
scan_servo.write(i);
delay(20);
state[i] = readSensors();
}
}
(UPDATE : 08/01/2012):I think I have the whole object avoidance code ready .Testing is only what remains for it .(Propably sometime today)
Code:
#include <Servo.h>
Servo scan_servo,left_servo,right_servo;
int reading1,reading2,k,turn_to,start_pos,state_sum;// turn_to is the position where the biggest angle with no obstacle in front of the bot begins. k is a counter to accomplish the process of finding turn_to .
int maxx;//the maximum value of angles
long pi = 3.14 ;
int R;//radius of the wheels
int d;//distance between the wheels
int max_servo_speed = 60;//maximum value of servo's speed in rpm
int state[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot)
int angles[180];//coutners for countring the different angle with no obstacle in front of the bot
int pos[180];//An array in which is saved the angle that the
int i = 0;//general counter
void setup(){
//pin assignment
pinMode(0,INPUT);//set pin 0 of analog pins as analog input.
pinMode(1,INPUT);//set pin 1 of analog pins as analog input .
pinMode(6,OUTPUT);//set pin 6 of digital pins as digital output .
scan_servo.attach(6); // attaching the scanning servo to pin 6 .
left_servo.attach(7);// attaching the left continue rotation servo to pin 7.
right_servo.attach(8);//attaching the left continue rotation servo to pin 8.
}
void loop(){
if(scan() == 0){//if there is no object in front of the bot move forward till you find an object in front of the bot .
center_scan_servo();
do{
move(150);
}
while(readSensors() == 0);
}
else if(scan() == 2){//if there all the area in front of bot is full of objects then turn around .
center_scan_servo();
turn(150,180);
}
else if(scan() == 1){// if you find an object in front of the bot turn a little bit to a place with no object and move forward till you find an object in front of the bot.
center_scan_servo();
turn(150,turn2());
do{
move(150);
}
while(readSensors() == 0);
}
}
int readSensors() {//read the sensors
reading1 = analogRead(0);
reading2 = analogRead(1);
if(reading1 >= 370 || reading2 >= 370) {
return 1;
}
else {
return 0;
}
}
int scan(){//scan the area using the scan_servo and fill the state array.
state_sum = 0;
for(i = 0 ; i < 180; i+=1){
scan_servo.write(i);
delay(20);
state[i] = readSensors();
state_sum = state_sum + state[i];
}
for( i = 180 ; i >= 1; i-=1){
scan_servo.write(i);
delay(20);
state[i] = readSensors();
state_sum = state_sum + state[i];
}
if(state_sum == 0){
return 0;
}
if(state_sum == 180){
return 2;
}
else{
return 1;
}
}
int turn2(){//find the angle that the robot should turn to in order not to bump into any object
k = 0 ;
turn_to = 0 ;
maxx = 0;
for(i = 0 ; i < 180; i+=1) {
angles[i] = 0;
}
for(i = 0; i < 180; i+=1) {//fill the angles array
if(i == 1 && state[i] == 0){
k+=1;
angles[k] = 1;
pos[k] = i;
}
else if(state[i] == 0 && state[i - 1] == 0 ){
angles[k]+=1;
}
else if(state[i] == 0 && state[i - 1] != 0){
k+=1;
angles[k] = 1;
pos[k] = i;
}
}
for(i = 0 ; i < k; i+=1){//find the biggest angles and set turn_to
if(angles[i] > maxx){
start_pos = pos[i] ;
maxx = angles[i];
}
}
turn_to = start_pos + (maxx/2);
if(maxx%2 != 0) {
turn_to = turn_to + (1/2);
}
return turn_to;
}
void turn(int servo_speed,int turning_angle){//the robot in the turning_angle with servo_speed
int s_speed = map(servo_speed/2,0,90,0,max_servo_speed);
int N = (2*pi*R)/((d/2)*turning_angle);
int t_servo = N/s_speed;
int target_time = N * t_servo;
left_servo.write(servo_speed);
delay(target_time);
right_servo.write(abs(servo_speed - 180));
delay(target_time);
}
void move(int servo_speed) {//move the robot forward or backward depending the servo_speed
left_servo.write(servo_speed);
right_servo.write(servo_speed);
}
void center_scan_servo(){//center the scan servo
scan_servo.write(90);
delay(20);
}
R and d variables aren't set yet cause I have to measure the radius of the wheels Im using and the distance between the wheels .
I will be uploading a video too if all go smoothly .
