Our complete code, which can be found in our GitHub repository, was used to optimize a pattern of mmovement, power the servos, and control Sparky either autonomously avoiding obstacles, or a manual override.
|Python dependencies:||Serial, sys, socket, math, msvcrt, random, os, numpy, pygame|
NI LabView was used to interface with the camera above the Large Project Building Pool. A LabView VI published data on position of objects within the pool, and a python script captured the data using socket.
For our autonomous tadpole, we used an Arduino Mega as the onboard controller that could operate autonomously, or receive commands from the tadpole command center (the computer running the control scripts) through the serial port over Xbee radio. The Arduino controlled three servos, and 7 status LEDs. It received input from 3 IR sensors positioned at the front of the tadpole’s head.
There are 7 states the the tadpole itself can occupy: Stop, Auto Turn Left, Auto Straight, Auto Right, Command Left, Command Straight, and Command Right. Stop is a commanded state. When the tadpole is in the autonomous modes, it transitions between turning left, going straight, and turning right based on the input from the three IR sensors that act as range finders. If the IR sensor reading is above some threshold (meaning an object is closer than the threshold) then the tadpole will turn in the opposite direction. If the center IR is tripped, then it will turn to the side with the lower reading. The commanded states persistently perform the gait assigned to them, and only change states with user input. Each IR sensor has a corresponding status LED to indicate if its reading is above its threshold. Each state maps to some set of status LEDs. Green is straight, Turns are Yellow, Stop is Read, and the commanded states have the white light on while the auto status do not.
There are two different modes of swimming: the C gait and the S gait. In the C gait, the entire body of the tadpole (expect for the front of the head) curls in the same direction at once. The effect is equivalent to sculling in sailing. Because the head is much more massive than the rest of the body, the remaining segments are largely pushing against the head rather than exerting a torque on it. When in the turn states, the head is oriented in a certain direction, and the rest of the tadpole moves in C gait. In our initial testing over remote control, we found this to be the most effective way to turn.
In the S gait, each servo moves according to a sinusoidal signal, so the entire body of the tadpole looks like an S. This is closer to the movement of an eel or a sea snake. In this motion, every face of the fish is exerting a force on the water in proportion to its cross product with the velocity vector of the tadpole. The S gait is used in the straight modes. During testing with RC control, we were able to find a productive S gait, but it was very difficult to achieve with the setup of the controller (which was for hobby airplanes, and had 4 degrees of control). We hoped that we would be able to achieve a more consistent S pattern through control with the Arduino. In the first testing of the autonomous S gait, the hand-picked parameters seemed to work fairly well.
The most basic code Arduino code did not accept control signals over the Xbee, and was simply ‘deaf’ (but not ‘blind’, it got input from the IRs). Its functionality and process is as follows: at every loop, read in the IR values, set the directional lights, send the IR values back over serial, execute the actions associated with the tadpole state, set the state lights, smooth the servo position and write the values to the servos, and send the state back over serial. Because this “tadpole_nocomms” is probably disconnected, the serial outputs are solely for debugging.
In the Stop state, no servo positions are updated. In the turn states, the head is oriented to the direction of turning and the C gait controls the movement of the final two servos. In the auto state, if the IR sensors no longer sense an obstacle, and some number of cycles of the C gait have been completed, it transitions to Straight. In the straight state, every servo moves in a sinusoidal pattern determined by the parameterization of the S gait. In the auto state, the tadpole transitions to a turn state opposite either side IR being tripped, or to the side with greater clearance if the center IR is tripped.
Then, with each iteration of tadpole communication, additional functionality on the Arduino side is added. In tadpole_wstop, the functionality is the same as the nocomms, except the tadpole will enter the stopped state if it receives the stop command over serial, and will resume motion given the proper command. In tadpole_wcomms, the Arduino is able to read parameters for the S gait through the commands sent over Serial. This is critical for the optimization, described below.
On the Python side, we wished to build a simple interface that would allow the user to command the tadpole directly, set its parameters, run optimization on the parameters of the its gait, and let it operate autonomously. The “basicCommandCenter.py” allows users to send a command to set the parameters, take manual control over the robot (determining its state, such as turn or go straight, rather than command servos directy, because it would be difficult to produce useful motion with it). Communication happens over two Xbee radios, one attached to the computer running python script, and the other attached to the tadpole’s arduino. The commands sent over serial are then parsed and followed by the tadpole to make the appropriate updates. The three main control modes are ‘manual’, ‘auto’, and ‘optimize’. Manual and auto work as described above, the new functionality that the TadpoleCommandCenter allows is the optimization scheme.
The python script interfaces with a LabView VI that uses the pictures taken by the camera above the Large Project Building pool to get the position of ‘fiducials’ or distinct markers on objects. The VI returns position and heading of the fiducial. With this information and reports of the state from the tadpole, we’re able to run an optimization scheme, because we can find the speed over some length of time. We will attempt to optimize the speed of the tadpole during for its S gait. This should be the same thing as optimizing the forward velocity, but it is conceivable that this is not the case and we should look at the difference in distance over time elapsed, rather than capturing the velocity over many points of a run.
The S gait of the tadpole is parameterized as the frequency of all three servos, individual amplitudes of each servo, and the phase offset of the second from the first, and the third from the second. This means we have 6 degrees of freedom - not an insignificant state space, and already heavily reduced from the space of all possible parameterizations. The motivation for this parameterization (and choice of gait) is that it is found in nature, so we should merely aim to replicate for our system what we know is feasible. We reduced frequency down to just one parameter, because it doesn’t make sense for the servos to have cyclic offsets. We expect frequency and amplitude to be linked, because the frequency should determine the range of achievable amplitudes. It is conceivable that all of the amplitudes should be different because of their different positions within the linkage.
For the actual optimization algorithm, we chose to use a genetic algorithm so that we would not need to encode too much prior belief about the form of the solution (after our dimensionality reduction) and we could search over a varied statespace. We chose to use mutation, recombination, and selection operators in the generation of a new set of parameters for the next run. We included simulated annealing to gradually reduce the magnitude of mutation over time.
Extensions of the software side could have involved a GUI to go along with the command interface.