Friday, June 12, 2015

(Simulated) Success!

If my robot does this at the competition, it will represent full mission success: scoring at least one point.
The left is a spreadsheet plot of the simulated position, and the right is the simulated GPS data dropped into Google Earth.

I had gone into this thinking that the software was good and the hardware flaked out last year. I made a bunch of changes to the software, and it got worse before it got better, but I suspect there were enough bugs in the old code that it wouldn't have worked even if the hardware had worked. Last year, it did try to turn, though.

This proves the value of a good simulation. I have simulated the race more than 100 times, including times when the robot just drove around in circles. The above run was the first which actually successfully sought a waypoint. More about the simulation below the fold.


Using robot code

The world is divided up into '''robot code''' and '''sim code'''. Since the point of this whole exercise is to test the robot code, we use as much of it as possible. When possible for each module, we use both the .cpp and .h file from the robot code. When the robot code cannot work in sim mode (mostly because of hardware interactions) we use the .h and write a new .cpp called sim/sim_*.cpp . This routine will have the same functions, but the bodies will be replaced by code which does work in the simulated environment.

For instance, the SD card driver is replaced with sim/sim_sdhc.cpp . The original driver implements a fairly simple block interface, which copies a block of memory to a sector of the SD card (write) or vice versa (read). The sim code, instead of dealing with SPI and LPC2148 registers, uses a 16GB file on the host, and uses fseek, fread, and fwrite to implement the block interface. The file was preformatted using the normal host mkfs.vfat, so all the structures are in place.

As a consequence, all the partition and filesystem code for the robot was directly able to be used. This allowed testing of those drivers, which detected certain bugs I had seen before, mainly dealing with writing file timestamps.

In practice, I am able to use almost all the header files as-is. Exceptions are:


  • gpio.h - this had static inline functions dealing with GPIO. The sim version replaces these with normal function prototypes, for functions defined in sim/sim_gpio.cpp
  • LPC214x.h - This is the most heavily hardware-dependent file. It naturally consists of a bunch of inline functions which return int references to particular hard-coded memory addresses, representing the registers. The sim version replaces these with normal function prototypes, the functions themselves being in sim/sim_LPC214x.cpp (which has no corresponding LPC214x.cpp on the robot). Only registers which are actually needed (proven by the code not compiling without them) are implemented, and even then often just by returning zero.
  • Time.h is a symlink to the real Time.h. This file uses LPC214x.h, lives in libraries/System, and if we used that file directly, it would include the LPC214x.h in libraries/System instead of sim. 

The following files replace modules from the robot to implement the sim interface:


  • sim_gpio.cpp - This implements blinklock() by printing a message then terminating the simulation.
  • sim_HardSPI.cpp - A lot of modules are implemented as classes with virtual methods. Even if these methods are never called, there must be at least empty bodies for all of them. HardSPI is one of these. If any of these routines are called, the simulator prints an error message and terminates.
  • sim_hmc5883.cpp - This one (will) simulate the compass. It currently forges a correct whoami and writes configuration information to a packet. The read(x,y,z) will interface with the simulator state to get the magnetic field vector.
  • sim_l3g4200d.cpp - Likewise with the gyroscope
  • sim_LPC214x.cpp - As explained above, this does all of the registers which have been proven to be used. This includes the timer (TTC) and real-time clock (RTC) registers, along with a couple others which are just dumped into a config packet.
  • sim_pwm.cpp - This is the interface to the steering and throttle. The setServo() function is intercepted to run the steering and throttle commanding in the simulator.
  • sim_sdhc.cpp - As explained above, this implements the interface using a normal file and C standard I/O. It also fakes the SDHC_info structure.
  • sim_Serial.cpp - This one intercepts the write(char) function and uses printf to print it to the host stdout. It also intercepts the available(), peek() and read() calls to feed data from the simulated GPS to the real NMEA parser.
  • sim_StateTwoWire.cpp - More blank functions to fill in the virtual method table
  • sim_Time.cpp - Since the robot Time.cpp is heavily hardware-integrated, this module implements just the bare-bones in a way compatible with the host. Right now that consists of a straight-up copy of set_rtc() enabled by the fact that sim/sim_LPC214x.cpp simulates the RTC registers.
  • sim_Wire.cpp - More virtual method table stuff.

The following files are used directly from the robot code, and are therefore tested. This represents the majority of the robot code.

  • Yukari3/main.cpp
  • Yukari3/navigate.cpp
  • Yukari3/guide.cpp
  • Yukari3/control.cpp
  • libraries/packet/packet.cpp
  • libraries/Partition/Partition.cpp
  • libraries/fat/cluster.cpp
  • libraries/fat/file.cpp
  • libraries/fat/direntry.cpp
  • libraries/Print/Print.cpp
  • libraries/Circular/Circular.cpp
  • libraries/gps/nmea/nmea.cpp
  • libraries/System/Stringex.cpp
  • libraries/quaternion/Quaternion.cpp
  • libraries/FileCircular/FileCircular.cpp
  • libraries/dump/dump.cpp
  • libraries/config/readconfig.cpp

Testing under simulation

With the simulation, I can do a number of things I couldn't with the live robot -- mainly use gdb to watch certain routines. The old way was print statements (or writing debug packets in the robot case) but gdb is quite a bit faster and easier. You can look and look and look at code all day, but never notice that you got the date routines wrong, that there is an extra factor of 10^7, or that you copied and pasted a line dealing with x but never changed it to y. These are much easier to see with the debugger.

No comments:

Post a Comment