My first attempt - platform ready

[mbateman]

Nuisance-3 V1.00
First robotic attempt
Matt Bateman - 11/4/08



Goals:

• Build a robotic platform that is simple enough that I could build it with no prior experience yet flexible enough that it could be used to test several configurations and algorithms
• Use parts with sufficient capabilities that they could be used for subsequently more complex projects.
• Build from off-the-shelf parts, but not kits. This would allow me to learn the structural, mechanical, power, electronic, software, and control portions of the project.
• Design in a pan/tilt mechanism for eventual use with camera to allow for working with vision algorithms

Mechanical

• Plexiglas base and servo mount
• Differential drive
• DC motors w/ integrated gearing
• R/C Monster truck tires and wheels
  
• Omniwheel rear wheel

Power

• Common ground bus
• Electronics: 6v 2800mAH NiCad
  
  • Unregulated 6v bus powers Axon
  • 5v regulated power to servos and sensors via Axon
  • 5V regulated provided to breadboard via 7805
  • 470uF capacitor between unregulated bus and ground bus
• Drive motors: 12v (2x6v 2800mAH NiCad in series)
• Separate power switches and indicator LEDs for electronics and motors.
• Power switches are DPST to switch batteries between robot and charger connections

Electronics

• Axon controller
• Sabertooth 2x10 R/C
• Breadboard for prototyping and debugging

I/O

• Ping))) ultrasonic mounted on servo for forward view
• Sharp IR Rangefinder mounted on servos for sides and rear view
• Push buttons
  
  • Large red one for reset
  • Smaller black one for general purpose debugging
  • Pulled low by external resisters
  • Pulled high by button contact
• LEDs for visual debugging on object detection for front and sides (see Map below)
• LEDs for visual debugging on FSM (see FSM below)
• Each servo can be trimmed with a #define to get it perfectly centered

R/C

• Airtronics 6 channel radio and receiver
• Receiver read by Axon using external interrupts
• Interrupts capture timestamp of rising and falling edges of signal on each channel
• Value calculated by the difference in time between edges. To keep the interrupt routine short, the calculation is done asynchronously by a scheduled function (see Scheduler below)
• Since there can be interference with older FM radios, I used a state machine and counter to determine when I have seen enough good signals to consider the radio on. I also will ignore up to 3 bad signals before declaring the signal to be lost.
• When radio is on, the right joystick (ch 1 & 2) controls speed and turn. Left joystick (ch 3 & 4) controls pan and tilt of the head. Aux (ch 6) is used for various debugging functions. Gear (ch 5) is currently unused.
• If radio is off, robot functions in autonomous mode

PWM

• Sabertooth and precision servos require 50Hz base frequency pulses to stay synchronized. Software PWM was insufficiently consistent to work.
• Used building PWM mode on timers 4 and 5 of the atMega640 to drive 6 PWM outputs
• Sensor servos work fine with software servo mode as long as the R/C interrupts are turned off for the duration of the pulse output. Otherwise, the length of the pulse can be changed enough by the interrupt to alter the desired value. Used the following macro instead of the one provided..
     - #define myServo(port,number,position)   PCMSK2=0;PORT_ON(port,number); delay_cycles(position); PORT_OFF(port,number);PCMSK2=rcPinMask

Scheduler

• I used a scheduler routine to allow many functions to run at appropriate intervals in an asynchronous manner. Functions can be added and removed from the scheduler stack in an ad hoc fashion and each with its own period
• Since the scheduler is done without interrupts and multiple functions can fire in the same pass, it is possible to have starvation of some functions. I added debugging code to help determine if and how often starvation occurs. The scheduler is only used for functions that can tolerate occasional delays in running.
• All inputs and sensor servos are handled by the scheduler as are some mapping functions

Map

• A map is created in memory using a 32x32 grid
• Each grid represents a 3 inch square.
• There are actually two maps, one to store actual sensor data, and one to keep track of which grid locations are visible by sensors. This second map is a mask that helps the software know where there are blind spots
• When an object is seen by a sensor, a fixed number is stored in the grid location corresponding to the location. At the same time, all the points from the sensor to the object are marked as visible in the mask map.
• For sensors with a wide field of view (such as the PING) 3 points are plotted corresponding to the left, center, and right angles of the field of view.
• There is a scheduled process that decrements all of the numbers in the map on a regular basis. This allows older plots to fade out and for more current ones to have a higher value
• Each sensor has knowledge of its location on the robot with X and Y offsets from center as well as the angular offset of the sensor from the servo's center point. This data is used when plotting the points on the map
• The map is also marked with the outside edges of the robot itself so it knows its own dimensions
• A buffer zone is defined that is used by the state machine to know if something is too close and must be handled by specialized navigation routines.
• The fade process also updates several global variable based on the contents of the map. These are used by other functions to navigate.
  
  • closeLeft, closeRight, closeFront, closeBack are used to indicate something in the buffer zone
  • unitsClearFront, unitsClearBack are used to set the speed under normal mode. Go slower the closer something is.
  • sumLeft, sumRight are weighted sums of all the points in left and right front quadrants. The closer or newer the point the higher the weight.
• The difference between the two variables is used to determine which way to turn and how much.
• When in R/C mode, the Aux switch can be used to take a snapshot of the maps and global variable for output to the USB port when connected back to the computer. This turned out to be a very valuable debugging tool. Here is an example of a snapshot of the internal map combined with the mask map when travelling down a hallway...

? ? ? ? ? ? ? ? ? ? ? . . . . . . ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? . . . . . . ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? . . . . . . . ? . ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? . ? . . . . . . . ? ? ? . ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . ? . ? . ? . . . . . ? # ? o ? . ? ? ? ? ? ? ?
? ? ? ? ? . ? . . ? . . . . . . . . . . . ? . . ? . ? ? ? ? ?
? ? ? ? ? ? . ? o . . ? . . . . . . . ? # . . ? . ? ? ? ? ? ?
? ? ? ? . . ? . ? . . . . . . . . . . ? . o ? . ? . . ? ? ? ?
? ? ? ? ? . . ? # . . . . . . . . . . . # . . ? . . ? ? ? ? ?
? ? ? . . ? . . . . . . . . . . . . . . . o . . . ? . . ? ? ?
? ? ? ? ? . . . # o . . . . . . . . . . # . . . . . ? ? ? ? ?
? ? . . . . ? . ? # . . . . . # . . . . # o . . ? . . . . ? ?
? ? ? ? ? . . . . # . . . . . o . . . . . # . . . . ? ? ? ? ?
? ? . . . . . . . # . . . R R R R R . . . # # . . . . . . ? ?
? ? ? . . . . . . o . . . R R R R R . . . . # . . . . . ? ? ?
? ? . ? ? ? ? ? ? ? ? ? ? R R X R R ? ? ? ? ? ? ? ? ? ? . ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? R R R R R ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? # . . R R R R R . . . ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . . . . # . . . . . . . . . ? . . ? ? ? ? ? ? ?
? ? ? ? ? . . . . . # # . . . . . . . . . o ? . . . ? ? ? ? ?
? ? ? ? . ? ? . . . # # . . . . . . . . . . . . ? ? . ? ? ? ?
? ? ? ? ? . . ? . . # . . . . . . . . . . . . ? . . ? ? ? ? ?
? ? ? ? ? . ? . . . . . # . . . . . . . . . . . ? . ? ? ? ? ?
? ? ? ? ? ? . ? ? . ? # # . . . . . . . ? . ? ? o ? ? ? ? ? ?
? ? ? ? ? ? ? ? . ? . ? . ? . . . ? . ? . ? . ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . ? . . ? . . . . . . . ? . . ? . ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? . ? . . . ? . ? . . . ? . ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? . ? . ? . ? . ? . ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? . ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

FSM

• In order to handle many different situations, the main loop uses a Finite State Machine
• Each main state can have many sub steps
• Current state is display in binary on four LEDs for easy visual indication during debugging
• Transitions to another state or step can be based on any of the global variables or on a counter/timer set by previous states
• Whenever a state causes a significant change in location or direction, it will go to the Learning state when it is done. The Learning state waits for a sufficient period of time to allow the map to catch up to any changes and for the globals variable to contain valid data.
• Currently, there are only a few states defined:
  
   #define stateInitialize         0x00
   #define stateLearn              0x01
   #define stateRcControl          0x02
   #define stateAutoGoForward      0x03
   #define stateAutoBackUp         0x04
   #define stateAutoStopAndTurn    0x05
  
   #define stateTrapped            0x0E
   #define stateDebug              0x0F
  

Lessons Learned

• Bumpers - Soft bumpers are essential, especially during debugging and development phases. Even so, I still ended up with many marks and scrapes on the walls.
• Weight distribution - Originally, the drive wheels were much closer to the center of mass. This was fine unless stopping quickly or accelerating quickly in reverse. I moved the wheels forward and that helped a lot.
• R/C signal interference - The older FM R/C radio/receiver I have is susceptible to interference. With the radio off but the receiver on, I got many false signals. I used some software filtering and a minimum number of required good pulses to work around the problem.
• Isolation of power leads on Sabertooth - The Sabertooth puts out its own Vcc even on the input connectors. This caused power problems when interfacing with the Axon since one was powered by 12v and one by 6v. To get around this, I simply removed the Vcc connector pin from the housing on the Sabertooth inputs.
• R/C pinouts - Older R/C servos had many different kinds of connectors each with its own pinouts. I had to build customer connectors to convert from these to the current universal connector.
• Servo pulse requirements - Each servo has different requirements for pulse width. This is especially true of older servos from before the move to a universal standard. I also found that the HiTec HS-422 servos that came with the pan/tilt module are also not standard. Rather than the standard 1ms - 2ms, they required .5ms - 2.5 ms to get full range.
• Alignment - When I moved the wheels forward, I did not dismantle the robot completely, so I was not able to use my precision tools to mark and drill the new motor mount holes. The end result was a slight misalignment causing a left turn when told to go straight. I was able to correct the alignment with shims.
• Unplug USB before moving - The Axon USB connector was not able to withstand the violent force of yanking the USB cable out sideways when moving the robot off the desk before unplugging the cable. With help from SoR Admin, I was able to glue it back down and re-solder the connections. I now have a big orange ribbon attached to the cable so I can't miss seeing it when moving the robot.
• Personality - Until I get the camera, I have put a purple-haired troll head on the pan/tilt module. I found people were much more at ease and interested in the robot with the head attached. It can now "look" around and appears to have a limited personality. If nothing else, it is fun to watch.
• Limitations of Omniwheel - While the Onmiwheel is a great solution for smooth surfaces, it does not work well in rough or soft surfaces or with small obstacles. I am considering a large swiveling caster or going to 4WD to solve this issue.
• Turn off as many interrupts as possible during servo pulse output - When may interrupts are enabled, the software servo code can produce pulses of different durations. This causes significant jitter in the servos. I redefined the servo macro to turn off the external interrupts during the output pulse.
• Timer overflow counters - I found that the timer overflow counters depend on global interrupts to function. I initially turned off all interupts during the servo pulse, but found that this cause many problems. The most noticeable was with the Ping input routine that uses timer0. The overflow counter was not getting updated, so at approximately 10' the timer would overflow but my calculation would not see the overflow and think the distance was close to zero.
• Limits of sensors - If fired too quickly, the Ping sees lots of ghost returns. Slowing down the rate of firing helped considerably. Also, the Ping has a wide field of view, so this needs to be taken into account when mapping the point on the map. I solved this by mapping 3 points for each return on the Ping: left, right, and center of the current servo angle. Also, none of the sensors are picking up objects above or below their level. This causes the robot to run into desk chair legs, furniture with rails, and to try to drive under the bed only to hit its head. I am going to use some whisker switches to try to detect some of these objects and plot them on the map. Pillows and blankets are also not detected well.
• Sabertooth R/C sensitivity to base period - The Sabertooth R/C is not nearly as forgiving as normal servos to the base period of the PWM input. If the pulses are a bit late or too far apart, it will randomly change the output to the wheels. To correct this, I used the hardware PWM functions of the atMega640 to drive the PWM to the Sabertooth.

Next Steps

• Detect stairs - I have yet to find a good way to determine when it is about to plummet down the stairs or any other abrupt edge like that
• Whisker switches - I plan to attach whisker switches in front and above the bumper to try to detect low and high objects that the sensors miss
• 4WD - I am going to experiment with adding 2 more drive wheels and removing the Omniwheel. I suspect this will improve the ability to run over small things and deal with rough or soft terrain. But it will also likely hinder the turning performance on some surfaces where slip will not happen easily. Stay tuned...
• Camera - Long term plans are to add a camera and experiment with computer vision

[mbateman]

I forgot to list my very next step. I need to tune the turn and speed algorithms to get rid of the oscillations caused by changing location or direction faster than the map updates can keep up.

[Trumpkin]

NICE JOB!

[Webbot]

Excellent - have seen a purple haired Gonk for a while.

So if the map is 32x32  x 3inches then it looks from the video that the robot must be going off the map. What happens in that case.

You don't mention encoders - so how do you calculate your current position?

Good job

[mbateman]

The map is always relative to the robot. i.e.: the robot is in the center of the map at all times. It is really more of a radar screen than a map. Sensor inputs are plotted on the map and then fade away over about a second unless updated by a new sensor input. So it shows the immediate area around the robot. That is why any state that causes it to stop and turn quickly forces it back to the learning state so the map has time to catch up.

[Admin]

Nice! Reminds me of my old HyperSquirrel and Fuzzy robots back in the day

Got closeup pics and code to post? I put your bot on the Axon examples page.

Suggestion: double up each servo scanner with another Sharp IR to double your map generation speed.

[Razor Concepts]

Please post the code! I'm trying to figure out how to read the outputs a receiver puts out, and it looks like you've done it sucessfully.

[mbateman]

Here is most of the R/C code. Some items are in my hardware.c file, but the hard parts are all here. Sorry about the formatting. The code output here seems to strip all the tabs. ;-(

The rcDoUpdate() command will need to be run on a regular basis to convert the pulse times into useful data. I did not do that in the interrupt routine since it takes a bit of time and is not needed for every pulse. You just need to do it before using the data.

Also, set the PCMSK2 to 0x00 during any servo pulses and then back on when done. If you don't, then this interrupt routine will take enough time away from your servo command that it will change the pulse out and make your servos jitter. I defined my own servo command...

#define myServo(port,number,position)   PCMSK2=0;PORT_ON(port,number); delay_cycles(position); PORT_OFF(port,number);PCMSK2=rcPinMask

Code:

#define rcVer 1
#define rcRev 0

#define rcMaxChannels 6 // Maximum R/C channels
#define RC_NO_SIGNAL -127 // Value to use if no signal is present
#define rcSignals2BeValid 8 // Must see at least this many pulses to consider radio to be on
#define rcMaxDroppedSignals 3 // Must lose this many pulses to consider radio off

// The following min/max are used to filter out noise and interference.
#define rcBasePeriodMin 21800 // Min base period in uS
#define rcBasePeriodMax 22200 // Max base period in uS
#define rcPulseTimeMin 950 // Min pulse period in uS
#define rcPulseTimeMax 2050 // Max base period in uS

#define rcPinMask 0x3F // Mask to turn on all R/C pins.

// Connect R/C inputs to Bank K 0-7 (input pins 8-15)
// R/C channel 1 -> K0
// R/C channel 2 -> K1
// R/C channel 3 -> K2
// R/C channel 4 -> K3
// R/C channel 5 -> K4
// R/C channel 6 -> K5
// R/C channel 7 -> K6
// R/C channel 8 -> K7



// Build a structure to hold the data for each pin
struct  {
unsigned long int startTime;
unsigned long int pulseTime;
unsigned long int basePeriod;
  signed      int signalsUntilValid;
  signed   int pct; // Value range from -100 to 100
} rcPin[rcMaxChannels];

// Global variable that can be used to determine if the radio is on or off.
// #define of OFF and ON in main.h
// #define ON 1
// #define OFF 0
int rcState=OFF;


signed int rcGetChannel(unsigned int channel) {
/***************************************************/
/* Get the current value of the requested channel. */
/* Returns percent as -100 to 100                  */
/***************************************************/
return(rcPin[channel-1].pct);
}

int rcIsOn(void) {
/******************************/
/* Returns state of R/C radio */
/******************************/
return rcState;
}


unsigned long int rcGetDiff(unsigned long int now, unsigned long int last) {
/*************************************************/
/* Get the time difference between now and last  */
/* Must divide by 2 since prescale only /8 and   */
/* CPU is 16Mz. getTimeDiff defined elsewhere... */
/* #define getTimeDiff(now,last) ((last>now)?(65535-last+now):(now-last)) */
/*************************************************/
return (getTimeDiff(now,last)/2);
}

ISR (PCINT2_vect){
/********************************/
/* Handle pin changes on PORT K */
/* via interrupts.              */
/********************************/
u08 pulseDefRise,pulseDefFall;
static u08 prevState; // Holds the previous state of K7-0
u08 currState=PINK; // Get current state of K7-0
u08 mask;
unsigned long int now=TCNT5; // Timer5. Rolls over at 65535


pulseDefRise = (~prevState) & ( currState); // Rising edges
pulseDefFall = ( prevState) & (~currState); // Falling edges

for (int pin=0;pin<rcMaxChannels;pin++) {
mask=0x01<<pin;
if(pulseDefRise & mask){ // Rising edge
/* Rising edge to rising edge = base period */
rcPin[pin].basePeriod=rcGetDiff(now,rcPin[pin].startTime); // Calculate base period
rcPin[pin].startTime=now;
} else if(pulseDefFall & mask){ // Falling edge
/* Rising edge to falling edge = pulse time */
rcPin[pin].pulseTime=rcGetDiff(now,rcPin[pin].startTime); // Calculate pulse time
/* Determine if the pulse is good */
if (rcPin[pin].basePeriod > rcBasePeriodMin
&& rcPin[pin].basePeriod < rcBasePeriodMax
&& rcPin[pin].pulseTime > rcPulseTimeMin
&& rcPin[pin].pulseTime < rcPulseTimeMax) {
// Possible good signal
if (rcPin[pin].signalsUntilValid > -rcMaxDroppedSignals) {
rcPin[pin].signalsUntilValid--;
if (rcPin[pin].signalsUntilValid==0) {
// Saw enough to be good
rcPin[pin].signalsUntilValid=-rcMaxDroppedSignals;
}
}
} else {
// Possible lost signal
if (rcPin[pin].signalsUntilValid < rcSignals2BeValid) {
rcPin[pin].signalsUntilValid++;
if (rcPin[pin].signalsUntilValid==0) {
// Lost too many in a row
rcPin[pin].signalsUntilValid=rcSignals2BeValid;
}
}
}
}
}
prevState=currState;
}

void rcDoUpdate(void) {
/***************************************************/
/* Sanity check and convert R/C inputs.            */
/* This process should be run on a regular basis   */
/* based on the update freqency required for       */
/* reading the R/C values.                         */
/***************************************************/
int tmpState=OFF;
for (int pin=0;pin<rcMaxChannels;pin++) {
if (rcPin[pin].signalsUntilValid<0) {
tmpState=ON;
}
if (rcPin[pin].signalsUntilValid==-rcMaxDroppedSignals) {
// Last pulse was valid so convert it to a percentage
rcPin[pin].pct=rcReceiverConvert(rcPin[pin].pulseTime);
} else {
// Last pulse may not be valid so don't use
rcPin[pin].pct=RC_NO_SIGNAL;
}
}

if (tmpState) {
LED_on();
} else {
LED_off();
}
rcState=tmpState;
}

void rcInit(void) {
/*****************************/
/* Set up interrupts for R/C */
/*****************************/
rprintf(" R/C:      \t%d",rcVer); rprintf(".%d\t",rcRev);
// Clear all channel info
for (int pin=0;pin<rcMaxChannels;pin++) {
rcPin[pin].pct=RC_NO_SIGNAL;
rcPin[pin].signalsUntilValid=rcSignals2BeValid;
}
// Enable PCINT2
PCICR=0x04;
// Turn on for 6 channels
PCMSK2=rcPinMask;
// 00 A off, toggle on compare
// 00 B on, toggle on compare
// 00 C on, toggle on compare
// 00 Low part of WGM: None
TCCR5A=0x00;
// 0 No input noise cancelling
// 0 Input edge select does not matter
// 0 Not used
// 00 High part of WGM: None
// 010 Prescale 8
TCCR5B=0x02;
// Clear TCNT
TCNT5H=0x00;
TCNT5L=0x00;
// Enable timer5 overflow interrupt
// TIMSK5=0x01;
// Schedule update
schedAdd(rcDoUpdate,timerRcDoUpdate);
// Done
rprintf("done\r\n");
}

[mbateman]

Hah, the code looks just fine once posted. Just the preview has the tabs gone.

[Rebelgium]

Excellent post, great robot.
I really enjoyed reading it, the "lessons learned" part was helpfull.
I'm actually prototyping with a Hitec HS422 right now, and I was just wondering whe the F it didn't turn the whole 180°...Thanks for the info.

a slight misalignment causing a left turn when told to go straight. I was able to correct the alignment with shims.

What's a shim?

[Trumpkin]

What's a shim?

http://en.wikipedia.org/wiki/Shim_(engineering)

[ryan]

Nice robot..congrats

[Dscrimager]

Fantastic, it's pretty close to what I am shooting for with my shiny new AXON. I really like the 'radar screen' world map approach... let no good idea go unused!

Doug

[cooldog]

• Detect stairs - I have yet to find a good way to determine when it is about to plummet down the stairs or any other abrupt edge like that

irobot rummba uses ir rangefinders pointing at the ground to detect stairs

[SeagullOne]

Extremely impressive Robot, mbateman! Very clever use of code and your 'radar' screen approach! Nicely done indeed!