NXShield line follower robot

This is a robot I made to test, and experiment with, the OpenElectrons NXShield-M. I am using an Arduino Mega as the controller, so I program it using the standard Arduino programming environment (with the NXShield library). The robot uses the mindsensors’ LineLeader to “see” the line. I programmed it to use a PD algorithm to calculate the motor speeds, based on the line position.

Here is the Arduino program it’s running:

#include <Wire.h>
#include <NXShield.h>

#include <LineLeader.h>
#include <PSPNx.h>

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

//
// declare the NXShield(s) attached to your Arduino.
//
NXShield    nxshield;

//
// declare the i2c devices used on NXShield(s).
//
LineLeader LL_Nx;
PSPNx PSP_Nx;

void setup()
{
  delay(500);
  char str[256];
  Serial.begin(115200);  // start serial for output
  Serial.println (" ");
  Serial.println ("Initializing the devices ...");
  //
  // NXShield provides multiple protocols
  // Initialize the protocol for NXShield
  // It is best to use Hardware I2C (unless you want to use Ultrasonic).
  //
  nxshield.init( SH_SoftwareI2C );

  //
  // Initialize the i2c sensors.
  //
  
  LL_Nx.init( &nxshield, SH_BBS2 );
  PSP_Nx.init( &nxshield, SH_BBS1 );
  
  // initialize the analog sensors.

  //
  // reset motors.
  //
  nxshield.bank_a.motorReset();
  nxshield.bank_b.motorReset();
  Serial.println ("Press < to calibrate black");
  Serial.println ("Press > to calibrate white");
  Serial.println ("Press GO button to continue");
  while(true){
    if(nxshield.getButtonState(BTN_GO)){
      break;
    }
    
    if(nxshield.getButtonState(BTN_LEFT)){
      while(nxshield.getButtonState(BTN_LEFT)){delay(1);}
      Serial.print("Calibrating black ...");
      LL_Nx.calibrateBlack();
      delay(500);
      Serial.println(" Done");      
      Serial.println ("Press < to calibrate black");
      Serial.println ("Press > to calibrate white");
      Serial.println ("Press GO button to continue");      
    }
    
    if(nxshield.getButtonState(BTN_RIGHT)){
      while(nxshield.getButtonState(BTN_RIGHT)){delay(1);}
      Serial.print("Calibrating white ...");
      LL_Nx.calibrateWhite();
      delay(500);
      Serial.println(" Done");
      Serial.println ("Press < to calibrate black");
      Serial.println ("Press > to calibrate white");
      Serial.println ("Press GO button to continue");      
    }
  }
  randomSeed(analogRead(0));  
}

//Scales a range of numbers to a new range.
//Input value, lowest possible, highest possible, scale from this, to this. It returns the result.
float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  float Result = (DesiredMax - DesiredMin) / (ValueMax - ValueMin) * (Value - ValueMin) + DesiredMin;
  return Result;
}

byte kp = 9;
byte kd = 14;
float error;
int error_i;
int error_last;
int derivitive;

float turn;

int MLS, MRS;

byte LL_Bool;
byte LL_Avg;

byte ly, ry;
byte ButtonSet1, ButtonSet2;

byte LL_Active_Control;
byte LED_On = 1;

void ButtonSet1NotPressed(){
  while (PSP_Nx.readByte(PSPNx_ButtonSet1) * (-1) + 255){
    delay(10);
  }   
}

void loop()
{
  char str[256];
  nxshield.bank_b. writeByte(0x7a, 0);
  nxshield.bank_b. writeByte(0x7b, 0);
  LL_Active_Control = 0;
  ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;
  while (true) {   
    
    ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;

    if (ButtonSet2 & 0x80) {LL_Active_Control = 1;}
    else {
      if (ButtonSet2 & 0x40) {
        LL_Active_Control = 0;
      }
    }
    
    ButtonSet1 = PSP_Nx.readByte(PSPNx_ButtonSet1) * (-1) + 255;
    
    if (ButtonSet1 & 0x10) {
      kp++;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x20) {
      kp--;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x40) {
      kd--;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x80) {
      kd++;
      ButtonSet1NotPressed();
    }    
    
    
    if (ButtonSet2 & 0x10) {
      while (ButtonSet2 & 0x10){
        delay(10);
        ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;
      }      
      if(LED_On)LED_On = 0;
      else LED_On = 1;
    }
    
    if (LL_Active_Control){
      LL_Bool = LL_Nx.getResult();
      LL_Avg = LL_Nx.getAverage();
      
      error = LL_Avg - 10;  //The error, between -5 and 5
      error /= 7;
      error -= 5;
      error *= (-1);
      
      if (LL_Avg==0){                       //if it is not all white, scale to -5 to 5
        error = 0;
      }
      
      error = error * (abs(error)/3*2);
      error_i = error;
      derivitive = error - error_last;
      turn = error_i * kp + derivitive * kd;
      
      error_last = error;
      
      MLS = 75 - turn;
      MRS = 75 + turn;
      
      MLS = Clip(MLS, 0, 100);
      MRS = Clip(MRS, 0, 100);      
    }
    else{
      error_last = 0;      //Reset so that it is fresh for when it resumes following the line
      ly = PSP_Nx.getYLJoy();
      ry = PSP_Nx.getYRJoy();      
      MLS = ScaleRange(ly, 0, 255, 100, -100);
      MRS = ScaleRange(ry, 0, 255, 100, -100);
    }
    
    if (MLS>=3||MLS<=(-3)){    
    //nxshield.bank_b.motorSetBothSpeed(MRS, MLS);
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, MLS);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, 0);
      nxshield.bank_b.motorStop(SH_Motor_1, SH_Next_Action_Brake);
    }
    if (MRS>=3||MRS<=(-3)){      
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, MRS);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, 0);
      nxshield.bank_b.motorStop(SH_Motor_2, SH_Next_Action_Brake);
    }    
    
    sprintf (str, "LL-Nx Average: %d Error: %d LL-Nx Bits: %d%d%d%d,%d%d%d%d", LL_Avg, error_i, LL_Bool&0x80?1:0, LL_Bool&0x40?1:0, LL_Bool&0x20?1:0, LL_Bool&0x10?1:0, LL_Bool&0x08?1:0, LL_Bool&0x04?1:0, LL_Bool&0x02?1:0, LL_Bool&0x01?1:0);
    Serial.println(str);
    
    sprintf (str, "ly %d MLS %d ry %d MRS %d kp %d kd %d", ly, MLS, ry, MRS, kp, kd);
    Serial.println(str);
    
    if (LED_On) nxshield.setRGB(random(9), random(9), random(9));
    else nxshield.setRGB(0,0,0);
  }
}
Advertisements
This entry was posted in Electronics, Mindsensors, Mindstorms, Technic and tagged , , , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s