Linefollower Robot

[gbaby]

Hi all!

I’m trying to program very simple linefollower robot in c and python but i failed to do so. I used 2 light sensors and 2 motors for moving the robot.

Heres my codes:

Codes in C
(both light sensors reading doesnt seems to be stable)

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "tick.h"
#include <wiringPi.h>
#include "BrickPi.h"
#include <linux/i2c-dev.h>
#include <fcntl.h>

#define THRESHOLD 40
// gcc -o (OUTPUTFILENAME) (CFILENAME).c -lrt -lm -L/usr/local/lib -lwiringPi
// ./program

int result;

const int Left  = 3;   // Left  motor on D
const int Right = 0;   // Right motor on A
const int LLight = 3;// Left Light Sensor on port 4
const int RLight = 2;// Right Light Sensor on port 3

#undef DEBUG

void msleep(int msec){
	usleep(1000*msec);
}

void do_update(){
	////////////////////
	// Run
	// BrickPiUpdateValues()
	// until success
	////////////////////
	  do{
		  result = BrickPiUpdateValues();
		  msleep(10);
		  //cout << "attempt" << endl;
	  }
	  while(result != 0);
	  //cout << "success!"  << endl;
} //http://robotsquare.com/2013/10/23/manty-with-raspberry-pi-and-brickpi/

int main()
{
   ClearTick();
   result = BrickPiSetup(); 

   if (result)
   return 0;

   BrickPi.Address[0] = 1;
   BrickPi.Address[1] = 2;

   BrickPi.SensorType[LLight] = TYPE_SENSOR_LIGHT_ON;
   BrickPi.SensorType[RLight] = TYPE_SENSOR_LIGHT_ON;

   BrickPi.MotorEnable[Left] = 1; 
   BrickPi.MotorEnable[Right] = 1; 

   result = BrickPiSetupSensors(); 

   if(result)
   return 0;

   do_update();

   while(1)
   {
       if (LLight > 480)
       {
           BrickPi.MotorSpeed[Right] = 100;
           do_update();
       }

       else if (RIGHTSENSOR > 440)
       {
           BrickPi.MotorSpeed[Left] = 100;
           do_update();
       }

       else
       {
           BrickPi.MotorSpeed[Left] = 50;
           BrickPi.MotorSpeed[Right] = 50;
           do_update();
       }
   }
}

Python codes
(i’m new to python, hope someone can explain to me why i cant compile and execute the codes ><)

from BrickPi import *   

BrickPiSetup()  

BrickPi.MotorEnable[PORT_A] = 1 #Enable the Motor A
BrickPi.MotorEnable[PORT_D] = 1 #Enable the Motor B

BrickPi.SensorType[PORT_1] = TYPE_SENSOR_LIGHT_ON
BrickPi.SensorType[PORT_4] = TYPE_SENSOR_LIGHT_ON

BrickPiSetupSensors()   

while True:

if BrickPi.Sensor[PORT_1] < 480 :
            BrickPi.MotorSpeed[PORT_A] = 0  
            BrickPi.MotorSpeed[PORT_D] = 100  
        elif BrickPi.Sensor[PORT_4] > 440 :
            BrickPi.MotorSpeed[PORT_A] = 100  
            BrickPi.MotorSpeed[PORT_D] = 0 
        else :
            BrickPi.MotorSpeed[PORT_A] = 50	
            BrickPi.MotorSpeed[PORT_D] = 50

        BrickPiUpdateValues() 
        time.sleep(.1)   

The picture was the result of the program.
top - python
bottom - c

Your help would be greatly appreciated!
G

[gbaby]

Hi everyone!

I found out why i cant compile and execute the program!
it was because of my if else statement! sorry for asking such idiotic question >.<’’’

G

[gbaby]

Hi Guys!

i managed to do simple linefollower robot in python! however, its not working in c, due to the light sensor values. see the pic i uploaded in first post! If you have time, please comment on it! Thank you for your time!

G

Codes in C

 
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "tick.h"
#include "BrickPi.h"
#include <linux/i2c-dev.h>
#include <wiringPi.h>
#include <fcntl.h>

int result;
int Left;
int Right;
int LLight;
int RLight;

#undef DEBUG

void fwd(void)
{
  BrickPi.MotorSpeed[Left]  = -50;
  BrickPi.MotorSpeed[Right] = -50;
}

int main()
{
   ClearTick();
   result = BrickPiSetup(); 

   if (result)
   return 0;

   BrickPi.Address[0] = 1;
   BrickPi.Address[1] = 2;

   Left = PORT_D;
   Right = PORT_A;
   LLight = PORT_1;
   RLight = PORT_4;

   BrickPi.SensorType[LLight] = TYPE_SENSOR_LIGHT_ON;
   BrickPi.SensorType[RLight] = TYPE_SENSOR_LIGHT_ON;

   BrickPi.MotorEnable[Left] = 1;
   BrickPi.MotorEnable[Right] = 1;

   result = BrickPiSetupSensors();

   if(result)
   return 0;

   while(1)
   {
     if(!result)
       {
           printf("LS VALUE: %d %dn",BrickPi.Sensor[LLight],BrickPi.Sensor[RLight]);
           fwd();

           if (BrickPi.Sensor[LLight] < 650)
           {
               BrickPi.MotorSpeed[Right] = -75;
               BrickPi.MotorSpeed[Left]  = 0;
           }

           if (BrickPi.Sensor[RLight] < 650)
           {
               BrickPi.MotorSpeed[Right] = 0;
               BrickPi.MotorSpeed[Left]  = -75;
           }
	   BrickPiUpdateValues();
           usleep(10000); // delay for 10ms
       }
   }
}

Codes in Python

from BrickPi import * 

BrickPiSetup() 
BrickPi.SensorType[PORT_1] = TYPE_SENSOR_LIGHT_ON 
BrickPi.SensorType[PORT_4] = TYPE_SENSOR_LIGHT_ON 

BrickPi.MotorEnable[PORT_A] = 1
BrickPi.MotorEnable[PORT_D] = 1

BrickPiSetupSensors() 

left = PORT_D
right = PORT_A

LLight = PORT_1
RLight = PORT_4

while True:
    result = BrickPiUpdateValues() 
    if not result :
        print BrickPi.Sensor[LLight],BrickPi.Sensor[RLight] #left right 
        
        BrickPi.MotorSpeed[right] = -50
        BrickPi.MotorSpeed[left]  = -50

        if BrickPi.Sensor[LLight] < 650: #goes near to white tape
                     BrickPi.MotorSpeed[right] = -75
                     BrickPi.MotorSpeed[left]  = 0

        if BrickPi.Sensor[RLight] < 650: #goes near to white tape
                     BrickPi.MotorSpeed[right]  = 0
                     BrickPi.MotorSpeed[left] = -75

    time.sleep(.01)     # sleep for 10 ms

[JohnC]

Hey GBABY, from the picture, are you getting odd values or something? I take it that each time you get a ~1020 value, that’s an error. Is that right?

If that’s the error you’re getting, I would add a filter in around where you read the analog values. IE, I would filter out readings that are great than 1000, just reject them and take the reading again.

Keep us apprised about how it’s going!

[gbaby]

Hi john,

i followed my python linefollower and change it to C. The sensor value was fine now, however, the robot was not following the line.
I didnt add the filter in as this time round theres no more 1020 value. The picture shows the sensor values when robot is at stationary.
There shouldnt be such a big range, am i right? is there somewhere i did wrongly in my c code?

Thank you John for always helping me with my problem!
GBABY

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

#include "tick.h"
#include "BrickPi.h"

#include <linux/i2c-dev.h>
#include <fcntl.h>

#undef DEBUG

int result;
int leftm, rightm, llight, rlight;

void fwd(void)
{   
        BrickPi.MotorSpeed[rightm] = 70;
	BrickPi.MotorSpeed[leftm]  = 70;
}

void left(void)
{
        BrickPi.MotorSpeed[rightm] = -200;
	BrickPi.MotorSpeed[leftm]  =  200;
}

void right(void)
{
        BrickPi.MotorSpeed[rightm] =  200;
	BrickPi.MotorSpeed[leftm]  = -200;
}

int main()
{
   ClearTick(); // in tick.h
   result = BrickPiSetup(); // setup the serial port for communication
   // printf("BrickPiSetup: %d\n", result); // Put this if you want to check setup.

   if (result)
     return 0; // check error

   BrickPi.Address[0] = 1; // Communication addresses
   BrickPi.Address[1] = 2;

   llight = PORT_3;
   rlight = PORT_1;

   BrickPi.SensorType[llight] = TYPE_SENSOR_LIGHT_ON; // right light sensor
   BrickPi.SensorType[rlight] = TYPE_SENSOR_LIGHT_ON; // left light sensor

   leftm = PORT_D; // left
   rightm = PORT_A; // right

   BrickPi.MotorEnable[leftm] = 1; // Note that 1 is Enable, 0 is Disable.
   BrickPi.MotorEnable[rightm] = 1; // Note that 1 is Enable, 0 is Disable.

   result = BrickPiSetupSensors(); // Send the properties of sensors to BrickPi
   // printf("BrickPiSetupSensors: %d\n",result); // Check if BrickPiSetupSensors work.

   if (!result)
   {
     while(1)
       {
	 printf("%d %d\n",BrickPi.Sensor[llight],BrickPi.Sensor[rlight]);
      	 fwd();
	 if (BrickPi.Sensor[llight] < 580)
	   right();
	 if (BrickPi.Sensor[rlight] < 580)
	   left();

         BrickPiUpdateValues(); // Ask BrickPi to update values result
         usleep(10000); // Delay for 10ms
	} 
   } 
   return 0;
}