Author Topic: Reading sensor data  (Read 554 times)

0 Members and 1 Guest are viewing this topic.

Offline piotr3332Topic starter

  • Beginner
  • *
  • Posts: 3
  • Helpful? 0
Reading sensor data
« on: April 17, 2013, 10:48:29 AM »
Hi

I have a question regarding my line follower. To obtain data from sensors the simple RC charge/discharge circuit was used to approximate multiple analogue to digital converter (ADC). Pulse width modulation (PWM) is used to vary the intensity of the IR TX. With a little bit of adjustment each sensor can produce an output with a 2 - 3 bit resolution i.e. 4 8 levels.  In operation each sensor is connected to a digital IO port. Initially this port is set to an output, driving a logic 1 onto the line for approximately 50us, charging the associated capacitor to VCC. The port is then switched to an input, setting the line to a high impedance state. The rate at which the capacitor discharges to zero is therefore proportional to level of reflected light i.e. the current following through the IR transistor. During this discharge period the eight inputs are polled by the processor at fixed time intervals and the zero crossing times recorded i.e. time is proportional to the reflected light level.
For example, when a sensors sees a line it gives me output of 7 on the other hand when it is off the line it gives me output 15. Could you give me the best method to use this sensor data in producing PID algoruthm?
I provide a source code for reading data.

Code: [Select]
#include "mbed.h"

DigitalOut   led(LED1);                                  // status LED
DigitalOut   test(p27);                                  // test pin

DigitalInOut ir_rx0(p11);  // IR recieves
DigitalInOut ir_rx1(p12);  // IR recieves
DigitalInOut ir_rx2(p15);  // IR recieves
DigitalInOut ir_rx3(p16);  // IR recieves
DigitalInOut ir_rx4(p17);  // IR recieves
DigitalInOut ir_rx5(p18);  // IR recieves
DigitalInOut ir_rx6(p25);  // IR recieves
DigitalInOut ir_rx7(p26);  // IR recieves

PwmOut ir_tx(p24);                                       // IR transmitter

Serial pc(USBTX, USBRX);                                 // USB serial port
 
// Black  : 4.2x200us   = 840us
// White  : 2 x 100us   = 200us
// Silver : 1.2 x 100us = 120us

         
int main() {

  bool ir_set[8];
  int  ir_value[8];
 
  int  ir_buffer;
  int  i, j, k;
   
  ir_tx.period_us(100);       // a 20ms period
  ir_tx.pulsewidth_us(100);    //
 
  led  = 1;   
  test = 0;

  while(true)
  {
    ir_rx0.output();
    ir_rx1.output();
    ir_rx2.output();
    ir_rx3.output();
    ir_rx4.output();
    ir_rx5.output();
    ir_rx6.output();
    ir_rx7.output();
 
    ir_rx0 = 1;
    ir_rx1 = 1;
    ir_rx2 = 1;
    ir_rx3 = 1;
    ir_rx4 = 1;
    ir_rx5 = 1;
    ir_rx6 = 1;
    ir_rx7 = 1;
   
    for(i=0; i<8; i++)
    {
      ir_set[i]=true;
      ir_value[i]=0;
    }

    wait_us(50);
   
    ir_rx0.input();
    ir_rx1.input(); 
    ir_rx2.input();
    ir_rx3.input();
    ir_rx4.input();
    ir_rx5.input(); 
    ir_rx6.input();
    ir_rx7.input();
         
    test = 1;
   
    for(i=0; i<20; i++)
    {
      wait_us(50);
     
      if ((ir_buffer && 0x01) == 0)
        test = 0;
       
      if((ir_rx0==0) & ir_set[0])
      {
        ir_set[0]=false; ir_value[0]=i;
      } 
      if((ir_rx1==0) & ir_set[1])
      {
        ir_set[1]=false; ir_value[1]=i;
      } 
      if((ir_rx2==0) & ir_set[2])
      {
        ir_set[2]=false; ir_value[2]=i;
      } 
      if((ir_rx3==0) & ir_set[3])
      {
        ir_set[3]=false; ir_value[3]=i;
      } 
      if((ir_rx4==0) & ir_set[4])
      {
        ir_set[4]=false; ir_value[4]=i;
      } 
      if((ir_rx5==0) & ir_set[5])
      {
        ir_set[5]=false; ir_value[5]=i;
      } 
      if((ir_rx6==0) & ir_set[6])
      {
        ir_set[6]=false; ir_value[6]=i;
      } 
      if((ir_rx7==0) & ir_set[7])
      {
        ir_set[7]=false; ir_value[7]=i;
      } 
         
    }
   
    printf( "ADC conversion\n" );
    printf( "chan 0 = %d\n", ir_value[0] ); 
    printf( "chan 1 = %d\n", ir_value[1] );     
    printf( "chan 2 = %d\n", ir_value[2] ); 
    printf( "chan 3 = %d\n", ir_value[3] );         
    printf( "chan 4 = %d\n", ir_value[4] ); 
    printf( "chan 5 = %d\n", ir_value[5] );     
    printf( "chan 6 = %d\n", ir_value[6] ); 
    printf( "chan 7 = %d\n", ir_value[7] );     
     
    wait(1);

  }
}

Thanks

Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,930
  • Helpful? 98
Re: Reading sensor data
« Reply #1 on: April 17, 2013, 06:12:17 PM »
Why PID?
Have you coded for the Bot to stay on the line?
What didn't work well?

Offline piotr3332Topic starter

  • Beginner
  • *
  • Posts: 3
  • Helpful? 0
Re: Reading sensor data
« Reply #2 on: April 18, 2013, 05:43:44 AM »
PID Because it is one of the requirenments of my project. I would also want to code my solution based on fuzzy logic but at the moment I dont have idea how I can do this.
Yes I have coded my robot to follow the line.
Here is my code
Code: [Select]
#include "mbed.h"

DigitalOut   led(LED1);                                  // status LED
DigitalOut   test(p27);                                  // test pin

DigitalInOut ir_rx0(p11);  // IR recieves
DigitalInOut ir_rx1(p12);  // IR recieves
DigitalInOut ir_rx2(p15);  // IR recieves
DigitalInOut ir_rx3(p16);  // IR recieves
DigitalInOut ir_rx4(p17);  // IR recieves
DigitalInOut ir_rx5(p18);  // IR recieves
DigitalInOut ir_rx6(p25);  // IR recieves
DigitalInOut ir_rx7(p26);  // IR recieves

PwmOut ir_tx(p24);                                       // IR transmitter

Serial pc(USBTX, USBRX);                                 // USB serial port
 
// Black  : 4.2x200us   = 840us
// White  : 2 x 100us   = 200us
// Silver : 1.2 x 100us = 120us


//------MOTOR----------
I2C i2c(p9, p10);        // sda, scl
 
PwmOut pwm0(p22);
PwmOut pwm1(p23);

const int addr = 0x40; // define the I2C Address

int kp = 200;
int kd = 1;
int ki = 1;



unsigned int FORWARD = 0x66;
unsigned int RIGHT = 0x55;
unsigned int LEFT = 0xAA;

int superslow = 6000; //pwm width value
float readSensors();   

int main() {


  //************MOTOR INIT******************
  char cmd[2];
  pwm0.period_us(20);         
  pwm1.period_ms(20);   


  //**********PID INIT************************

  float i_error[10]= { 0,0,0,0,0,0,0,0,0,0};  //stores error values of last 10 iterations for I control

  int i_counter=0;
 
  int sum_i_error=0;

  int turn;                                                                                   

  int error,error_old=0,zero=0,error_diff,temp;
 
 
   
  while(true)
  {   

//command for robot to go straight
    cmd[0] = FORWARD;           
    cmd[1] = 0x00;           
    i2c.write(addr, cmd, 1);

  //returned position is the error
    error = readSensors();
 

    //****************I control*****************************//

    sum_i_error =sum_i_error - i_error[i_counter];

    sum_i_error =sum_i_error + error;
    //printf("I ERROR: %d\n",  sum_i_error);
    i_error[i_counter%10]=error;

    i_counter++;

 

    if (i_counter==10)

        i_counter=0;

   //****************D control*****************************//   

    error_diff = error-error_old;

    turn = error*kp +error_diff*kd + sum_i_error*ki;

    error_old=error;

   
   
//adjusting the speed of motors based on turn
    pwm0.pulsewidth_us(turn + superslow);
    printf("left after loop: %d\n",  turn + superslow);
    pwm1.pulsewidth_us(turn - superslow);
    printf("right after loop: %d\n",  turn - superslow);
   
   
   

   
  } 
 
}
 

float readSensors()
{


  bool ir_set[8];
  int  ir_value[8];
 
  int  ir_buffer;
  int  i, j, k;
   
  led  = 1;   
  test = 0;
 
     ir_tx.period_ms(20);       // a 20ms period
  ir_tx.pulsewidth_ms(20);    //
 
 
  int position; 
 

    ir_rx0.output();
    ir_rx1.output();
    ir_rx2.output();
    ir_rx3.output();
    ir_rx4.output();
    ir_rx5.output();
    ir_rx6.output();
    ir_rx7.output();
 
    ir_rx0 = 1;
    ir_rx1 = 1;
    ir_rx2 = 1;
    ir_rx3 = 1;
    ir_rx4 = 1;
    ir_rx5 = 1;
    ir_rx6 = 1;
    ir_rx7 = 1;
   
    for(i=0; i<8; i++)
    {
      ir_set[i]=true;
      ir_value[i]=0;
    }

    wait_us(50);
   
    ir_rx0.input();
    ir_rx1.input(); 
    ir_rx2.input();
    ir_rx3.input();
    ir_rx4.input();
    ir_rx5.input(); 
    ir_rx6.input();
    ir_rx7.input();
         
    test = 1;
   
    for(i=0; i<20; i++)
    {
      wait_us(50);
     
      if ((ir_buffer && 0x01) == 0)
        test = 0;
       
      if((ir_rx0==0) & ir_set[0])
      {
        ir_set[0]=false; ir_value[0]=i;
      } 
      if((ir_rx1==0) & ir_set[1])
      {
        ir_set[1]=false; ir_value[1]=i;
      } 
      if((ir_rx2==0) & ir_set[2])
      {
        ir_set[2]=false; ir_value[2]=i;
      } 
      if((ir_rx3==0) & ir_set[3])
      {
        ir_set[3]=false; ir_value[3]=i;
      } 
      if((ir_rx4==0) & ir_set[4])
      {
        ir_set[4]=false; ir_value[4]=i;
      } 
      if((ir_rx5==0) & ir_set[5])
      {
        ir_set[5]=false; ir_value[5]=i;
      } 
      if((ir_rx6==0) & ir_set[6])
      {
        ir_set[6]=false; ir_value[6]=i;
      } 
      if((ir_rx7==0) & ir_set[7])
      {
        ir_set[7]=false; ir_value[7]=i;
      } 
         
    }

    //   
   
    if(ir_value[3] == 8 && ir_value[4] == 8 )
    {   
        position = 0;
        //perfectly straight
    }
   
    if(ir_value[3] == 10 && ir_value[5] == 10)
    {   
        //slighltly left
        position = 1;
    }
   
    if(ir_value[5] < 10)
    {
        //left
        position = 2;
    }   
    if(ir_value[5] == 7 && ir_value[7] == 11)
    {
        //more left
        //position = 3;
       
    }
    if(ir_value[7] < 13)
    {
        //hard left
        position = 4;

       
    }
    if(ir_value[7] > 6 && ir_value[5] > 14)
    {
       
        //going left off line
        //position = 5;

       
       
    }
   
    if(ir_value[3] == 10 && ir_value[2] == 10)
    {   
        //slighltly right
        position = -1;
    }
   
    if(ir_value[2] < 10 )
    {
        //right
        position = -2;
   
    }
   
    if(ir_value[2] >= 6 && ir_value[0] == 11)
    {
        //more right
        //position = -3;
       
    }
   
    if(ir_value[1] < 13)
    {
        //hard right
        position = -4;
       
       
    }
    if(ir_value[0] == 8 && ir_value[1] == 8)
    {
       
        //going right off line
        //position = -5;
       

       
       
    }
   
   
   
    return position;
     printf("position: %d\n",  position);
     
   
 
}


The problem with my solution is that it is not accurate, meaning the position value for each sensor is returned only when the if expression is satisfied. Could there be another method of programming this solution? I would reall appreciate any help.

Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,930
  • Helpful? 98
Re: Reading sensor data
« Reply #3 on: April 19, 2013, 06:10:46 AM »
Have you googled "multi sensor line follower". There are a  few good articles on how to do this.

The starting basics is to combine all the sensors into one variable that is used as the input the PID (or just P) control loop.

Here is a link to a good article showing how to add the sensors together:
http://www.ikalogic.com/line-tracking-sensors-and-algorithms/

 


Get Your Ad Here

data_list