General

Mini-Q 4WD line sensor problem

userHead adamcrumpton 2011-12-03 11:12:43 7658 Views2 Replies
I have got the DF Mini-Q 4WD and it works great. I am working on programming all of the sensors into a single sketch. So I got the obstacle avoidance working well and I want to add in the line tracking sensors to stop it from driving off a cliff. I got that part working... or so I thought. The sensors print out great values until the motors start and then their values drop close to zero and don't come back up.  I hope their is a simple way to read these sensors when the motors running.

And though I am embarassed by it, I will post this cut, pasted, and modified code for all to see:


#define EN1 6//Right Motor Enable Pin
#define IN1 7//Right Motor Direction Pin
#define EN2 5//Left Motor Enable Pin
#define IN2 4//Left Motor Direction Pin
#define Vr 5 // reference voltage
#define FORW 1//Forward
#define BACK 0//Backward

#define IR_IN  8//Infrared Receiver (Digital Pin 8)
#define L_IR 9//Left Infared Transmitter (Digital Pin 9)
#define R_IR 10//Right Infrared Transmitter (Digital Pin 10)
#define BUZZER 11 // control the number of IO pins buzzer
#define LED_RED 12 // red LED lights control digital IO pins
#define LED_GREEN 13 // green LED lights control digital IO pins
const int analogInPin = A2;  // Analog input pin that the potentiometer is attached to
//const int analogOutPin = 12; // Analog output pin that the LED is attached to
int count;                  //Pulse counter
int sensorValue = 0;        // value read from the pot
int outputValue = 0;        // value output to the PWM (analog out)
float data [8] = {0X00, 0X00, 0X00, 0X00, 0x00, 0xff, 0x00, 0x00};  // store the 8-channel ad conversion value
unsigned char value [5] = {0x00, 0x00, 0x00, 0x00, 0x00};          // five hunt voltage sensor
unsigned char key_1 = 0x00;                                        // number of keys pressed to count 1

void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN)//Motor control function
{
  //////////M1////////////////////////
  if(M1_DIR==FORW)//Set M1 Motor Direction 
    digitalWrite(IN1,HIGH);//HIGH, Rotate clockwise
  else
    digitalWrite(IN1,LOW);//LOW, Anti Rotate clockwise 
  if(M1_EN==0)//M1 Motor Speed
    {
    analogWrite(EN1,LOW);//Low to stop miniQ
    delay(10);
    }
  else  {
    analogWrite(EN1,M1_EN);
    delay(10);
  }
  ///////////M2//////////////////////
  if(M2_DIR==FORW)//Set M2 Motor Direction   
    digitalWrite(IN2,HIGH);//HIGH, Rotate clockwise
  else
    digitalWrite(IN2,LOW);//LOW, Anti Rotate clockwise 
  if(M2_EN==0) //M2 Motor Speed
    {
    analogWrite(EN2,LOW);//Low to stop miniQ
    delay(10);
  }
  else  {
    analogWrite(EN2,M2_EN);
    delay(10);
  }
}

void L_Send38KHZ(void)//Left Infrared transmitter generate 38kHZ pulse
{
  digitalWrite(L_IR,HIGH);//Set the infrared to HIGH level
  delayMicroseconds(10);//delay 10 microsecond
  digitalWrite(L_IR,LOW);//Set the infrared to LOW level
  delayMicroseconds(10);//delay 10 microsecond
}
void R_Send38KHZ(void)//Right Infrared transmitter generate 38kHZ pulse
{
  digitalWrite(R_IR,HIGH);//Set the infrared to HIGH level
  delayMicroseconds(10);//delay 10 microsecond
  digitalWrite(R_IR,LOW);//Set the infrared to LOW level
  delayMicroseconds(10);//delay 10 microsecond
}
void pcint0_init(void)//Init the interrupter
{
  PCICR = 0X01;
  PCMSK0 = 0X01;
}

ISR(PCINT0_vect)
{
  count++;//Counting the received pulser
}

void Read_Value(void) // read analog value
{
  char i;
  for (i = 0; i <8; i++)
  {
    data = analogRead(i); // read analog port voltage i
    data = ((data * Vr) / 1024); // convert the analog value
  }
  huntline_deal();// call subroutine hunt deal
}


void huntline_deal(void) // Hunt
{
  if (key_1 == 6)
  {
    if (data [0] > (value [0] - 1) && data [1] > (value [1] -1) && data [2] <(value [2] -1) && data [3]> (value [3 ] -1) && data [7]> (value [4] -1)) // look at the actual measured value
    {
                 
        Serial.println("Set 1"); 
    }
    else if (data [0]> (value [0] -1) && data [1]> (value [1] -1) && data [2] <(value [2] -1) && data [3] <(value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 2");
    }
    else if (data [0]> (value [0] -1) && data [1]> (value [1] -1) && data [2]> (value [2] -1) && data [3] <(value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 3");
    }
    else if (data [0]> (value [0] -1) && data [1]> (value [1] -1) && data [2]> (value [2] -1) && data [3] <(value [ 3] -1) && data [7] <(value [4] -1))
    {
        Serial.println("Set 4");
    }
    else if (data [0]> (value [0] -1) && data [1]> (value [1] -1) && data [2]> (value [2] -1) && data [3]> (value [ 3] -1) && data [7] <(value [4] -1))
    {
        Serial.println("Set 5");
    }
    else if (data [0]> (value [0] -1) && data [1] <(value [1] -1) && data [2] <(value [2] -1) && data [3]> (value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 6");
    }
    else if (data [0]> (value [0] -1) && data [1] <(value [1] -1) && data [2]> (value [2] -1) && data [3]> (value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 7");
    }
    else if (data [0] <(value [0] -1) && data [1] <(value [1] -1) && data [2]> (value [2] -1) && data [3]> (value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 8");
    }
    else if (data [0] <(value [0] -1) && data [1]> (value [1] -1) && data [2]> (value [2] -1) && data [3]> (value [ 3] -1) && data [7]> (value [4] -1))
    {
        Serial.println("Set 9");
    }
  }
                 
        Serial.print(key_1);
        Serial.print("\t 1111111111111 \t");
        Serial.println(value [0]);
           
        Serial.print(data [1]);
        Serial.print("\t 2222222222222\t"); 
        Serial.println(value [1]);
       
        Serial.print(data [2]);
        Serial.print("\t 33333333333333 \t");
        Serial.println(value [2]);

        Serial.print(data [3]);
        Serial.print("\t 444444444444444 \t"); 
        Serial.println(value [3]);

        Serial.print(data [7]);
        Serial.print("\t 555555555555555 \t");
        Serial.println(value [4]);
        delay(100);

}
void key_scan(void) // Scan button
{
  if (data [5]> 4.50 && data [5] <6.00) // no button is pressed
    return; // return
  else
  {
    if (data [5]>= 0.00 && data [5] <0.50) // Button 1 press
    {
      delay (180); // debounce delay
      if (data [5]>= 0.00 && data [5] <0.50) // press button 1 does
      {

        key_1 ++;// key 1 count
        if (key_1>= 1 && key_1 <= 5) value_adjust(key_1); // value of the sensor to adjust the hunt
        if (key_1 == 6)
        {
          digitalWrite (LED_RED, LOW); // red light off
          digitalWrite (LED_GREEN, LOW); // Green OFF
        }
      }
    }
    else if (data [5]>= 0.50 && data [5] <2.00)
    {
      delay (180); // debounce delay
      if (data [5]>= 0.50 && data [5] <2.00)
      {
        if (key_1>= 1 && key_1 <= 5) // if the key key1 value between 1 to 5
        {

          value [key_1-1 ]++;// sensor tracks threshold to distinguish Gaga (adjusted)
          value_adjust(key_1); // actual value comparison with
        }
      }
    }
    else if (data [5]>= 2.00 && data [5] <3.00)
    {
      delay (180); // debounce delay
      if (data [5]>= 2.00 && data [5] <3.00)
      {
        if (key_1>= 1 && key_1 <= 5) // If the key key1 value between 1 to 5
        {

          value [key_1-1 ]--;// sensor tracks threshold resolution and reduction (adjusted)
          value_adjust(key_1); // actual value comparison with
        }
      }
    }
  }
}
void value_adjust(unsigned char num) // adjust the sensor values ??hunt
{
  if (num == 1) // adjust the sensor first hunt
  {
    if (data [0] > value [0])
    {
      digitalWrite (LED_RED, HIGH); // the current value of the red light
      digitalWrite (LED_GREEN, LOW);
    }
    else
    {
      digitalWrite (LED_RED, LOW);
      digitalWrite (LED_GREEN, HIGH); // green light
    }
  }
  if (num == 2) // adjust the sensor the second hunt
  {
    if (data [1]> value [1])
    {
      digitalWrite (LED_RED, HIGH); // the current value of the red light
      digitalWrite (LED_GREEN, LOW);
    }
    else
    {
      digitalWrite (LED_RED, LOW);
      digitalWrite (LED_GREEN, HIGH); // green light
    }
  }
  if (num == 3) // adjust the third hunt sensor
  {
    if (data [2]> value [2])
    {
      digitalWrite (LED_RED, HIGH); // the current value of the red light
      digitalWrite (LED_GREEN, LOW);
    }
    else
    {
      digitalWrite (LED_RED, LOW);
      digitalWrite (LED_GREEN, HIGH); // green light
    }
  }
  if (num == 4) // adjust the sensor fourth hunt
  {
    if (data [3]> value [3])
    {
      digitalWrite (LED_RED, HIGH); // the current value of the red light
      digitalWrite (LED_GREEN, LOW);
    }
    else
    {
      digitalWrite (LED_RED, LOW);
      digitalWrite (LED_GREEN, HIGH); // green light
    }
  }
  if (num == 5) // adjust the sensor fifth hunt
  {
    if (data [4]> value [4])
    {
      digitalWrite (LED_RED, HIGH); // the current value of the red light
      digitalWrite (LED_GREEN, LOW);
    }
    else
    {
      digitalWrite (LED_RED, LOW);
      digitalWrite (LED_GREEN, HIGH); // green light
    }
  }
}
void Obstacle_Avoidance(void)
{
  char i;
  for(i=0;i<20;i++)//Left infrared transmit Send 20 38khz pulse
  {
    L_Send38KHZ();   
  }
  if(count>20)//If the counting is more than 10, there is obstacle avoidance 
  {
        count=0;
        Motor_Control(BACK,60,BACK,120);//Backward
        delay(750);//delay for 0.5 second
        Motor_Control(FORW,120,FORW,60);//turn left
        delay(500);//delay for 0.5
  }
  else
  {
    Motor_Control(FORW,100,FORW,100);//Forward
  }

  for(i=0;i<20;i++)//Right infrared transmit Send 20 38khz pulse
  {
    R_Send38KHZ();   
  }
  if(count>20)
  {
        count=0;
        Motor_Control(BACK,60,BACK,120);//Backward
        delay(750);//delay for 0.5 second
        Motor_Control(FORW,120,FORW,60);//turn left
        delay(500);//delay for 0.5
  }
  else
  {
    Motor_Control(FORW,100,FORW,100);//Move forward
  }
}

void setup() {
  // initialize serial communications at 9600 bps:
  Serial.begin(38400);
  char i;
  for(i=4;i<=7;i++)            //configure the pin mode
  {
    pinMode(i,OUTPUT);
  }
  pinMode(BUZZER, OUTPUT);    // Set the number of IO pins control the mode, OUTPUT is the output
  pinMode(LED_RED, OUTPUT);  // set the red LED light control digital IO pin mode, OUTPUT is the output
  pinMode(LED_GREEN, OUTPUT); // green LED setting controls the number of IO pin mode, OUTPUT is the output
  pinMode(L_IR,OUTPUT);      //Set the L_IR pin to output mode
  pinMode(R_IR,OUTPUT);      //Set the R_IR pin to output mode
  pinMode(IR_IN,INPUT);      //Set the IR receiver pin to input mode
  pcint0_init();              //Init the interrupter
  sei();                      //Enable the interrupter

}

void loop() {
    data [1] = 0X00;  // store the 8-channel ad conversion value
 
    key_1 = key_1++; 
  Read_Value();// read the value of analog port
  key_scan();// scan button
  delay(1000);
     
    if(data [0] < 2 || data [1] < 2 || data [2] < 2 || data [3] < 2 || data [7] < 2) // look at the actual measured value
    {
        //Motor_Control(BACK,60,BACK,120);//Backward
        //delay(3000);//delay for 0.5 second
        //Motor_Control(FORW,120,FORW,60);//turn left
        //delay(1000);//delay for 0.5 second 
      //      analogWrite (LED_RED, HIGH);
    //  analogWrite (LED_GREEN, HIGH);


    }
    else
    {
        Obstacle_Avoidance();//Obstacle avoidance
                                      // number of keys pressed to count 1

  } 
}
2011-12-11 09:38:39 Thanks Hector!

It actually was just a case of low batteries. I recharged them and then all of my tests worked suddenly. He stops when he sees the edge now.  Thank you again!

Adam
userHeadPic adamcrumpton
2011-12-07 14:22:55 I know the MiniQ is limited on space to add any other components. But maybe you can find a spot to add some capacitors:


[font=verdana]use a three capacitor filter on each motor, as shown here[/font][font=verdana]http://www.beam-wiki.org/wiki/Reducing_ ... _Filtering[/font][font=verdana].[/font]

[font=verdana]This might not be the best solution for the miniQ. The wiki article explains a whole lot about motor noise.[/font]

[font=verdana]Another reason might be that your batteries are too low. You might consider changing them or recharging them.[/font]

userHeadPic Hector