Sunday, February 17, 2013

Wheel Encoders and Collision Detection

In order for Robo-Magellan to know where it's going, it needs to know where it's come from.

GPS, while useful, has its limitations in determining Magellan's location relative to a cone or waypoint--together, the time to acquire a connection with positioning satellites and an error factor up to three meters from absolute position make a combination of locally acquired odometry and compass heading superior for our purposes.

The previous solution for measuring change in distance comprised of an IR emitter and collector pair placed inside the robot's wheel hubs, recording alternating high and low readings as the wheels turned and the emitted IR was either absorbed (black electrical tape) or reflected (hard plastic) by the wheel. Water, dust, dirt and mechanical vibrations were culprits for error in addition to floating odometry caused be deformation of the air-filled tires across various terrains.

An overhaul of the drivetrain and sensors were required for a more reliable method of measuring distance traveled without constantly relying on GPS. The tires were first filled with dense foam and then glued to the wheel hubs to eliminate skewed encoder counts from tire deformation or wheels slipping on the hub. We then set ourselves to the task of building the new rotary encoders.

Rotary Encoder Components:
- Switching Signal (Neodymium Magnet)
- Motion (Rotary)
- Switching Device (Solidstate Hall Effect)
- Signal Processing (Arduino Nano)

Using pairs of hall effect switches and strong rare earth magnets, we developed a method of recording odometry that was less prone to interference and had much higher sensor resolution.
The strength of the magnets are important to overcome the EMI from the brushed electric drive motors and for increasing the maximum distance required to activate the switching devices.

The gear with the holes was chosen as the rotary carrier for the magnets

By gluing three magnets to face of a gear inside the platform's drivetrain transmission we achieved higher resolution than before, as the digital signal could be measured approximately 17 times per unhindered wheel revolution against the 3 times per second seen with the optical encoders.

Two hall effect switches, with colored heat shrink-covered wires, installed on transmission case

With the hall effect switches hot glued to the outside of the transmission case, side-by-side, the magnetic field from the magnets on the gear were strong enough to engage both switches at the same time, which is important for determining rotary increments and direction of rotation. These signals are then stored and processed by software to determine the magnitude of the robot's linear distance (calculated from wheel diameter and angular velocity) and cooperates with the compass heading to reach a waypoint or move the robot within optimal distance for the cone-finding routine.
Additionally, encoder odometry is used to measure how far out of an intended path the robot travels when avoiding a collision with non-cone entities.

When Magellan believes it has found a cone, it utilizes vision to align itself with the cone and "bump switches" on the front of the platform (made to look like antennae) to detect when it has successfully touched a cone. At current, the bump switches are cherry switches with flexible steel wire extensions glued to their levers and oriented such that it can detect a touch on the left or right sides of the front of the platform, with plans to integrate a third switch to detect collisions in the rear as Magellan sometimes unintentionally collides with a cone by backing into it during the cone-finding routine.

- - - - - - - - - -

video
First test: Single Hall Effect switch and magnet installed on gear
(signal state tied to LED via Ardduino)


/*
 Hall Effect Switch
 
 Turns on and off a light emitting diode(LED) connected to digital  
 pin 13, when Hall Effect Sensors, connected to digital pins 2 and
 3 are triggered by a magnet.
  
 This example code is in the public domain.
 
 http://www.hobbytronics.co.uk/arduino-tutorial8-hall-effect

 Modified by Thomas M. R. Thomas
*/

// constants won't change. They're used here to set pin numbers:
const int hallPin1 = 2;     // the number of the first hall effect sensor pin
const int hallPin2 = 3;     // the number of the second hall effect sensor pin
const int ledPin =  13;
const int ledPin2 = 4;// the number of the LED pin
// variables will change:
int hallState1 = 0;          // variable for reading the first hall sensor status
int hallState2 = 0;          // variable for reading the second hall sensor status

void setup() {
  // initialize the LED pin(s) as an output:
  pinMode(ledPin, OUTPUT);
  pinMode(ledPin2, OUTPUT);  
  // initialize the hall effect sensor pins as an inputs:
  pinMode(hallPin1, INPUT);     
  pinMode(hallPin2, INPUT);
}

void loop(){
  // read the state of the hall effect sensors:
  hallState1 = digitalRead(hallPin1);
  hallState2 = digitalRead(hallPin2);
  if (hallState1 == LOW) {     
    // turn LED on:    
    digitalWrite(ledPin, HIGH);  
  } 
  else { digitalWrite(ledPin, LOW);
  }
  if (hallState2 == LOW) {
      // turn LED on:    
      digitalWrite(ledPin2, HIGH);  
      }
   else {
      // turn LED off:
      digitalWrite(ledPin2, LOW); 
    }
  }

video
Second Test: Two Hall Effect switches, four possible signal states; increments and direction achieved
(signal states tied to LEDs via Arduino) 

#define encoder0PinA  2
#define encoder0PinB  5
#define encoder1PinA  3
#define encoder1PinB  6
 
volatile int encoder0Pos = 0;   //signed
int encoder0temppos = 0;
volatile int encoder1Pos = 0;    //signed
int encoder1temppos = 0;

char command;
int encoder1speed =0;
int encoder0speed =0;
int encoder1tempspeed =0;
int encoder0tempspeed =0;
int encoder1acc =0;
int encoder0acc =0;
int dLeft = 0;
int dRight = 0;
float davg = 0;

void setup() {
 pinMode(encoder0PinA, INPUT);
 pinMode(encoder0PinB, INPUT);
 pinMode(encoder1PinA, INPUT);
 pinMode(encoder1PinB, INPUT);
  
 attachInterrupt(0, doEncoder0, CHANGE);               // encoder pin on interrupt 0 - pin 2
 attachInterrupt(1, doEncoder1, CHANGE);                                   // encoder pin on interrupt 1 - pin 3
 Serial.begin (115200);
}
 
void loop(){
 OutputEncoders();
 delay(100);
}
 
int OutputEncoders(){   
  //

 
 encoder1speed = abs(encoder1temppos-encoder1Pos);
 encoder0speed = abs(encoder0temppos-encoder0Pos);
 encoder1acc = abs(encoder1tempspeed-encoder1speed);
 encoder0acc = abs(encoder0tempspeed-encoder0speed);
 
 if (encoder0acc <10)
 {
  encoder0temppos = encoder0Pos;
  encoder0tempspeed = encoder0speed;
 }
 else
 {
  encoder0Pos = encoder0temppos;
  encoder0speed = encoder0tempspeed;
 }

 if (encoder1acc < 10)
 {
  encoder1temppos = encoder1Pos;
  encoder1tempspeed = encoder1speed;
 }
 else
 {
  encoder1Pos = encoder1temppos;
  encoder1speed = encoder1tempspeed;
 }
 if ((encoder1acc < 5)&&(encoder0acc <5))
 {
  davg = (encoder0Pos + encoder1Pos)/2;
  //Serial.println(davg);
  if( Serial.peek( ) == '&' )
  {
   //this just gets rid of the & char
   Serial.read( );
   //command is the next char in buffer
   command = Serial.read( );
   if ( command == '1' )
   {
    Serial.print(encoder0Pos);
                                Serial.print( " " );
                                Serial.print(encoder1Pos);
                                Serial.print( " " );
                                Serial.println(davg);
   }
   else if ( command == '2' )
   {
    encoder0Pos = 0;     //signed
    encoder0temppos = 0;
    encoder1Pos = 0;      //signed
    encoder1temppos = 0;
   }
   else
    Serial.println( -10 );
  }
  //get rid of anything that's not part a command
  else if( Serial.peek( ) != -1 )
  {
   Serial.read( );
  }
 }
}
 
void doEncoder0(){
 if (digitalRead(encoder0PinA) == HIGH) {                                  // found a low-to-high on channel A, check channel B to see which way encoder is turning
   if (digitalRead(encoder0PinB) == LOW) {encoder0Pos = encoder0Pos - 1;}  // CCW
   else {encoder0Pos = encoder0Pos + 1;}                                   // CW
 }
 else {                                                                    // found a high-to-low on channel A, check channel B to see which way encoder is turning 
   if (digitalRead(encoder0PinB) == LOW) {encoder0Pos = encoder0Pos + 1;}  // CW
   else {encoder0Pos = encoder0Pos - 1;}                                   // CCW
 }
}

void doEncoder1(){
 if (digitalRead(encoder1PinA) == HIGH) {                                  // found a low-to-high on channel A, check channel B to see which way encoder is turning
   if (digitalRead(encoder1PinB) == LOW) {encoder1Pos = encoder1Pos + 1;}  // CCW
   else {encoder1Pos = encoder1Pos - 1;}                                   // CW
 }
 else {                                                                    // found a high-to-low on channel A, check channel B to see which way encoder is turning 
   if (digitalRead(encoder1PinB) == LOW) {encoder1Pos = encoder1Pos - 1;}  // CW
   else {encoder1Pos = encoder1Pos + 1;}                                   // CCW
 }
}


- - - - - - - - - -

During a practice run for a presentation at a Robotics Society of Southern California (RSSC) meeting, held CSU Long Beach campus, RoboMagellan successfully combined magnetic compass heading, odometry, collision avoidance and object swerving (via sonic sensors), visual-based cone finding and cone collision detection (bump switches) to demonstrate its current abilities.

video
Minor adjustment to collision-avoidance required

With more fine-tuning, RoboMagellan would go on to have an even more impressive final run for its presentation to the members of RSSC.


**UPDATE**

A member of RSSC was kind enough to upload a video of the final run to YouTube: https://www.youtube.com/watch?v=ycHwPwjI0lM

No comments:

Post a Comment