Eric Drucker
Tony Snodgrass
Darren Earl
Shimmy Bot: A Figure Eight Traversing Robot
Abstract:
The goal in this project was to build a robot that would traverse a figure eight autonomously. A Lego Mindstorms kit was used to build the robot and its prototypes. The first prototype built consisted of two large wheels connected to independent motors and a flat surface that would drag on the ground. This design was a very naïve approach so touch sensors were implemented to control the rotation of each motor. Due to inaccurate results when the robot moved in a straight line, a “shimmy” was implemented as a way of movement. The shimmy moved the wheels one at a time, ensuring that each wheel moved exactly as far as it was supposed to. The program that controlled the robot consisted of a function to move straight and functions to turn right and left. These functions were called in order repeatedly to move the robot in a figure eight and have it return to its starting point. Overall, the methods used provided very accurate results.
Introduction/Background:
The purpose of this project was to build and program a robot to traverse a figure eight autonomously and return to its starting point. A smooth flat surface was used as the field in which the robot to perform its task. The figure eight to be traversed consists of eight straight lines greater than one foot long and all of equal length. Finally, the success of the robot was determined by how close it came to stopping at its exact starting point.
The path we needed to traverse is as pictured:
A Lego Mindstorms version 1.0 was used to build the robot. Two motors, two touch sensors, medium sized wheels, wires, the RCX Brick, and various connecting pieces were the Mindstorms kit pieces used for this project. The sensors available on this task were simple touch sensors that operate on a binary touched-untouched basis, insensitive to pressure. The Mindstorms kit is controlled entirely by the RCX brick, which is powered by a Renesas H8/300 microcontroller (WI2006). The RCX has three output connections and three input connections. To write, compile, and send code to the RCX, Bricx Command Center (BCC) 3.3 was used. BCC is an IDE for the RCX and uses the programming language Not Quite C (NQC). The motors can only be controlled by time intervals by the RCX, causing them to be very inconsistent. Also, the touch sensors are slightly inconsistent when pressed multiple times very quickly.
Approach:
First Robot Attempt
Initially, we constructed a very simple robot comprising two motors independently powering the rotation of two large wheels that were geared down in order to provide a minimum number of wheel rotations per maximum number of motor rotations. The robot dragged along with it a small, pivoting wheel to offset the drag, while the two independent wheels provided the power for the motion of the robot. It became apparent to us after several rudimentary tasks that the motors, themselves were not equal in speed or power. This necessitated a scrapping of the initial, simple robot and a redesign. In order to both travel a straight path (without pulling to the right or left) and also to make perfect 90-degree turns, we needed to design a way to keep track of exactly how many rotations we were getting.
Figuring out how to connect to the brick was a difficulty. Both 1.0 and 2.0 bricks were available to us, as well as both 1.0 and 2.0 infrared towers with serial and USB connections, respectively. We decided on using a v1.0 brick because of the advantage of a 12v power connection as pictured. This gave us the advantage of not rapidly depleting battery power during both development and testing.
We first attempted a connection to a 1.0 brick with a USB tower and a Sony Vaio PCG-K13, but were unsuccessful. Second, we attempted to use the same laptop to connect to the brick with a serial interface tower fitted with an LPT port adapter, but were also unsuccessful. Third, we attempted to use a Dell 3500 laptop that had the BricxCC already installed on it with the serial interface tower and met with better success.
Once we were able to establish a connection to the v1.0 brick with the Dell 3500 and the BricxCC, we eventually determined through trial and error how, exactly, the compilation, connection, and upload process worked. A Lego Mindstorms brick must have firmware uploaded to it in order to properly execute programs. If a brick loses power completely for 10 seconds or more, the firmware it contains as well as any programs that may be uploaded will be lost. After procuring the appropriate firmware for the 1.0 brick and successfully uploading it, we used the BricxCC command center to compile, upload, and run a simple program to test the touch sensors and motors. After finagling the code, we were successful. See code repository for successfully compiled, uploaded, and run NQC programs.
First Serious Attempt: Prototype 1, Vertical Pistons
In order to account for and keep track of how many rotations of the wheels we were getting, it was decided that the touch sensors would be utilized to keep a real-time tally or count of the number of rotations. This was accomplished by constructing a vertical piston system, in which the touch sensor was struck by a piston which was geared to thrust upwards a set number of times per one rotation of the wheel.
Unfortunately, as this was our first attempt at gearing together a piston system, we were a bit too zealous in the design. In order for the pistons to push upwards and strike the touch sensors, which were situated above the pistons, we had to use a complex gear system which actually redirected the kinetics of the wheels via a two gears connected perpendicular to one-another. The principle behind the design was sound, but the gear design was too complex to get the pistons to strike the touch sensors consistently. Our testing efforts met with limited success as, more often than not, the right piston would miss a strike and our robot would run around in circles continuously waiting for the piston strike to come.
The theory behind the design still held: we needed to control for the number of wheel rotations in order to prevent traversing a skewed path. A redesign was again warranted with the same concepts.
Second Serious Attempt: Prototype 2, Horizontal Pistons aka “Shimmy Bot”
Our second serious design utilized the same principle: strike the touch sensors with a piston geared to the rotating wheels in order to keep a running tally of the number of wheel rotations. However, this time the robot was constructed with the striking of the pistons in mind. Prototype 2 was built so that the kinetic motion of the wheels was never translated to another direction, making for a much more stable piston design. The pistons were positioned to strike touch sensors horizontally, utilizing the rotations from the wheels as power with as few gears abstracting the motion as possible.
Prototype 2 was our most promising design, and as such, we spent the most time working on it. We used different wheel sizes and occasionally experimented with different gear combinations to arrive at the most consistent and most easily countable number of piston strikes per wheel rotation. In the end, we settled on the wheels pictured on the robot above.
Prototype 2 was constructed with stability in mind, which accounts for the large lego bricks used to fix everything in place permanently. This only proved an unwise decision when changing batteries. However, because we were powering the robot with the 12v adapter throughout the development and testing phases, this proved to be a small issue.
Eventually it was determined that the only way to consistently count the number of rotations on each individual wheel, each wheel would rotate in turn and not in tandem. The result was a robot that would rotate it’s left wheel a set number of piston strikes, stop, and then rotate its right wheel a set number of piston strikes. This resulted in a shimmy-like walking style, hence the name ‘Shimmy Bot’ for Prototype 2.
Using the shimmy-style approach to traversing the track meant that the drag from the rear of Shimmy Bot needed to be addressed. Initially, we attempted to use a pivoting wheel as we did with our intial experiments. However, the bulky size that comes from Shimmy Bot’s frame was too much for a wheel to keep up with, resulting in a skewed shimmy. Eventually, it was decided that Shimmy Bot would simply drag his rear with a foot peg on the end which was fitted with the most low-friction lego piece we could find: a curved round piece which affixed to the bottom-side of a standard lego piece. The peg is pictured below. This resulted in a minimum amount of drag and a much more stable shimmy.
Though Shimmy Bot would prove to be our best design and succeeded eventually with a very minute error, we decided to explore alternative designs in an effort to come up with the best possible robot.
Another Serious Attempt: Prototype 3, Wheel and Spoke aka “Dragon Bot”
During the testing and development phase of Shimmy Bot, a team member was fiddling with some legos and came up with a design which utilized the holes in the largest size wheel into which perpendicular spokes could be inserted to strike a touch sensor as the wheels turned. Work on Shimmy Bot stopped temporarily while we pursued this possibility.
The robot was similar in design to our first experiment: two motors independently controlling two wheels. The exception was that now the wheels were fitted with spokes to strike the touch sensor upon every rotation. The large wheels had six potential slots into which we could insert spokes. However, it proved more effective to simple use three spokes, one inserted into every other slot. As the wheel rotated once around, there would be three strikes to the touch sensor.
This robot was colloquially named “Dragon Bot” because it too dragged around its rear side on a pivoting wheel that looked a bit like a dragon’s tail. By that tone, we also affixed small orange dragon wings to either side of Dragon Bot.
Despite the fact that our theory was sound in principle, as was the case with our first prototype, it proved implausible to pursue this design. Even using only three spokes to strike the touch sensors on each wheel, the speed of the rotation was too great for the touch sensors to accurately gage how many strikes there were to each wheel rotation. The Dragon Bot was far too fast, and the design was abandoned.
We instead returned our attentions to Shimmy Bot, solving some of our problems through creative software design. Shimmy Bot proved the most successful of all designs.
Honorable Mention: Elevator Bot
Though our success with Shimmy Bot precluded any need for another robot, we decided to attempt a fourth and final design, which was never programmed. Here is a picture of Elevator Bot:
The idea behind Elevator Bot was that the easiest way to travel a straight line was to have all the wheels of the robot powered by a single motor. If all wheels are powered by a single motor, however, in-place 90-degree turns are impossible. This could be overcome if there was an alternate way to rotate the robot in place exactly 90 degrees.
The premise was this: the robot would travel a given distance in a perfectly straight line and stop. Once stopped, the robot would lower an elevating piston that would make contact with the surface and elevate the robot off the table. Once elevated, the piston would line up with a set of gears and rotate the robot on the piston exactly 90 degrees.
Here are pictures of before and after elevations of the platform that was attached to the bottom of the piston:
The elevating portion of Elevator Bot was tricky to design. The gears had to be arranged very particularly so that a maximum number of rotations from the motor would result in a minimum amount of motion for the elevating piston. This was done because a great amount of torque was required to lift the robot.
Once the robot is fully elevated, a gear affixed to the top of the piston became in-line with another gear system attached to the third and final motor. The gear system would engage, and the robot would rotate in place. The gear synchronization is pictured below:
Once engaged, the dark gray gear would rotate the larger light gray gear which was attached to the piston.
Critical to the success of Elevator Bot was ensuring that the robot was perfectly balanced on either side of the piston. Otherwise, the robot would tip either forwards or backwards when lifting off the surface of the table. This was difficult to achieve because every additional counterbalance of weight on one side of the robot increased the total weight, necessitating a maximum amount of torque from the elevating piston. Balance was achieved, as pictured below:
While Elevator Bot was an interesting design, the success of Shimmy Bot and our approaching deadline did not require us to program and test it. However, the originality of the design warranted an honorable mention among our other design attempts.
Code:
The code went through several revisions and test phases before the final product. The basic mechanism by which the code was structured, however, was maintained throughout all revisions. The mechanism used depended upon two touch sensors used to measure the independent rotation of the two wheels. By starting the motor on a sensor hit and turning it off on the next respective sensor hit, a constant interval of distance could be achieved. This mechanism caused both of wheels to independently rotate a controlled distance despite variances in electrical power and strength in the motors.
Reset
The first task programmed was a reset program. The purpose of this program was to rotate the wheels one at a time until the respective sensors were hit. Since all of the following procedures required the pistons to be initialized in a starting position, this program automated the process for the beginning.
Walking
The first revision of the walking mechanism became affectionately termed the shimmy walk. The idea behind it was to move only one motor at a time in an effort to simplify movement. However, since only one motor moved a time the robot would pivot on each wheel rotation. Because of a lack of symmetry in movement, after one interval on each wheel the robot would be shifted slightly in the direction of the second motor to be activated. Thus in order for the robot to move in a straight path, the wheels need to be activated in the reverse order of the first set of intervals. The sequence of movements became either left right right left or right left left right. The unavoidable consequence of this procedure is a loss of precision. In order to move the robot straight, the wheels each need to complete two intervals. The code for one follows.
//WAIT_TIME is a constant integer
//*_MOTOR refers to the relevant motor
//*_SENSOR refers to the relevant touch sensor
//SOUND_CLICK is a sound identifier
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
PlaySound(SOUND_CLICK);
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
PlaySound(SOUND_CLICK);
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
PlaySound(SOUND_CLICK);
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
PlaySound(SOUND_CLICK);
Most of the code is obvious in function, however “Wait(WAIT_TIME);” could use explanation. Since each movement interval on the wheels ends on a sensor hit, the motor begins each interval on a sensor hit.
The motor must be given enough time to release the sensor so that it can complete a full movement interval before registering on the sensor again. “Wait(WAIT_TIME);” stops the code for a constant time so that in the meantime the motor can move enough to unregistered the sensor.
In an effort to provide a more conventional walking procedure, the second revision of code was created. This code allowed for the motors to be run in tandem and the differences between them to be dynamically dealt with. The basis for the procedure is that each motor completes an iteration at the same time as the other. If one motor becomes faster, that motor is stopped until the slower motor catches up. The draw back of this approach was more complex code. However the benefit gained is a conventional walk and the robot can move forward in one wheel movement intervals. The code follows.
On(RIGHT_MOTOR + LEFT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR || LEFT_SENSOR);
Off(RIGHT_MOTOR + LEFT_MOTOR);
if(RIGHT_SENSOR && LEFT_SENSOR)
continue;
if(RIGHT_SENSOR){
On(LEFT_MOTOR);
until(LEFT_SENSOR);
continue;
}
if(LEFT_SENSOR)
On(RIGHT_MOTOR);
until(RIGHT_SENSOR);
This code provided a relatively straight walk, although through experimentation the robot was seen to veer to the right on longer distances. This veering was linked to vibrations in the structure of the robot, and could not be corrected sufficiently with code. For this reason, the shimmy walk was seen as more reliable and precise. This walk however is implemented as a compile-time option in the final program.
The final walking procedure was inspired by the third prototype of the robot. This procedure tried to address specific issues of the robot; however, this prototype was ultimately abandoned and so was this procedure—only a partially implemented program exists. The problem with the prototype was that unlike the first two designs, the mechanisms that hit the sensors could not be stopped on the sensors. The idea was to allow the motors to run and keep track of sensor hits. After a motor hit the required hits, it was stopped while the other motor continued running until it reached the same number of hits. This code was never fully tested because developments on the second prototype “Shimmy bot” were more promising.
Turning
By toggling one motor into a reverse direction and then reusing the walking straight code, turning was accomplished. The walking procedure used was where both motors worked in tandem. Since both motors are running, the robot turns in place. By trial and error an approximate 90 degree angle was accomplished, although the sensor hits were not frequent enough to make a complete turn. Thus halfway through the following straight away, a single movement interval on one wheel was required to approximate a closer turn. This method was deemed accurate enough given the hardware restrictions.
The Final Program
The shimmy walk and turning procedures were turned into functions in order to modularize the programming. The turning limitations can be seen in the programming of the straight edges. The path first tested was a box. When walking around that path was satisfactory accurate, the more complex path of the figure eight was implemented. Due to lack of precision in the turning procedure, the code was not completely symmetric. While the program was not one hundred percent accurate in the goal, the robot consistently stopped in the same place. The goals of eliminating variance due to battery power and motor strength were met.
Trials and Experiments:
The first task tried was to cause the robot to walk forward in a straight line for an arbitrary but set distance. The file test1.nqc is an implementations of this task. The distance targeted is 10 move intervals (based upon ten hits of the touch sensor). Several variables were experimented within this structure: WAIT_TIME, walk procedure, and robot version. Power level was set so that the robot would move at a steady but slow and controlled pace. Each test was run several times with both power sources of a wall plug and with batteries (at levels of max and drained). On all tests there was no noticeable differences due to change in power source.
WAIT_TIME:
WAIT_TIME was a robot version dependent variable. It specifies the amount of time for the sensor to ignore input so that the sensor does not register the start as a sensor hit. If WAIT_TIME is too small, the sensor registers a false movement. If it is too large, the sensor could miss a hit and thus the wheel would rotate more than it was suppose to. All the robots tested were tested at a WAIT_TIME 0,3, 5,7, and 15 on all power levels and walk modes. Prototype one was abandoned before it could be tested in this set up.
On all walk modes, Prototype 2 would miss hits at 15 and would register false hits at values of 5 or less. These errors would cause the robot to veer from its path. At a wait time of 7 no such errors were observed and the robot only preformed intended wheel movements.
On all walk modes, Prototype 3 would miss hits at all values of WAIT_TIME, although at values of 3 or less, there was significantly less misses. At value 15 there were no registered hits. Prototype 3 performed chaotically.
Walk Procedure:
Walk procedure was a relatively robot version independent variable. If a robot's sensor hits were recorded as planned and sensor hits were rigged to wheel rotation, it would exhibit motion similar to another robot running the same program. There were two main walks implemented: shimmy walk (first version) and tandem walk (second version). A third walk was implemented for Prototype 3 and was not tested on other robots. Experimentations with Walk Procedure assumes the best value of WAIT_TIME for the robot being tested.
Prototype 2 walked accurately with Shimmy Walk. At first the walk did not alternate order of wheel movement, i.e. the wheels activated would be first left then right afterwards. This created a veer off to the right. However when the wheels were activated left right right left, no such veer was observed for the distance. Tandem walk caused Prototype 2 to visibly shake and veer off to the right. No changes in code could correct this.
Prototype 3 walked unreliably with Shimmy Walk. Due to it constantly missing sensor hits, Prototype 3 rarely walked straight in any fashion with shimmy walk. Tandem walk worked better, but the constant sensor misses and misreads caused the robot to turn unpredictably and travel for longer distances. The third walk procedure caused the robot to run the straightest until then end of the walk. Then the robot would consistently circle around by moving one wheel.
Results:
Prototype 2 “shimmy bot” gave the most accurate and consistent results with the shimmy walk with a WAIT_TIME value of 7. This configuration was chosen to be the basis for straight aways on all other tests.
Since experimentation on straightaways was finished, Prototype 2 “shimmy bot” was used for the basis of all other procedures.
Turning:
There were two procedures for turning: move only one wheel forward, and move both wheels at the same time—one moving forward, the other backward. For both procedures it took more than 8 movement intervals (8 on one wheel, or 4 on both wheels) but less than 9 intervals to make a ninety degree turn. Both procedures were similarly accurate, but only turning only one wheel displaced the robot more than tandem wheel rotation. Thus tandem wheel rotation turning was chosen because it turned the robot in place.
Box:
This was a testing stage to try and do half of the presented task. The robot was to complete a box movement moving in a counterclockwise direction, starting and ending at the bottom right corner. This experiment used shimmy bot walking the shimmy walk with tandem turning.
The first effort was simply four straightaways of equal length separated by four turns of magnitude 8 (four movement intervals for each wheel). The robot ended the program higher, and to the left of the target and the robot itself was turned to the right.
To mitigate the not quite 90 degree turn the program was modified for four straightaways with turns (in sequential order) of magnitudes of 8,9,8,9. This provided greater accuracy but the robot was still higher and to the left of the intended point.
Finally to improve accuracy more, corner turns were kept at magnitude 8 but halfway through all the straightaways besides the first an adjustment turn of 1 was added. This brought the robot within inches to the starting point. This method became the basis for the figure eight.
Figure Eight:
Code from the box trial was symmetrically applied to right turns to form a procedure for a clockwise box. The final turn from the Box procedure was removed and the clockwise box procedure was concatenated on the back. This made the robot move within a 3 inch radius of the target point. Adjustment turns ( turns of magnitude 1) were removed one by one starting from the last straightaway until the robot came well within an inch radius of the target point.
Conclusions:
The approach of measuring distance based upon wheel rotation allowed for the robot to get accurate and repeatable results despite variances in engine strength and power supply. Pistons, unlike the wheel and spoke method, provided a dependable and stable means of counting given the hardware restrictions. Methods like the wheel and spoke method need better hardware to be implemented. Simplicity often removed many of the problems that arose with more complex structures. For example, the shimmy walk only rotated one motor at a time, allowing the robot to go straighter. Also, the simpler design of the back end made up of a smooth surface proved to be a much better solution, because it allowed the back end to be pulled in any direction at any time, as opposed to requiring a back wheel to turn first.
With better implementation, the first prototype with vertical pistons may have been a better model, because it would have allowed for more control over rotation on each wheel. This would have allowed the robot to rotate both motors simultaneously, instead of using the shimmy walk, and would have completed its task more smoothly.
Future Directions:
A large majority of the time spent working on the project should be spent on coming up with a fluid design. The flaws of the motors and touch sensors should be kept in mind, and a slower, more accurate system should be developed. The upright piston prototype would have been a strong working example if the gear ratios had been changed in order to slow down the speed of the wheels and of the piston hitting the touch sensor. A more accurate system of determining amount of rotation would allow the robot to be even more consistent in moving in a straight line of exact length, and would allow it to be more precise in turning.
References:
(WI2006) Lego Mindstorms. (2006, September 6). In Wikipedia, The Free Encyclopedia. Retrieved September 6, 2006, from http://en.wikipedia.org/wiki/Lego_Mindstorms
Code Appendix:
reset.ncp initialize the robot to move
test1.ncp make the robot go straight
turn.ncp make the robot turn
figure8.ncp do the full figure 8 routine
reset.ncp
//this pushes the sensors down for the start of the real program
#define LEFT_SENSOR SENSOR_1
#define RIGHT_SENSOR SENSOR_3
#define LEFT_MOTOR OUT_A
#define RIGHT_MOTOR OUT_C
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV
#define LEFT_FWD OUT_REV
#define LEFT_REV OUT_FWD
task main()
{
#ifdef __RCX
SetSensor(RIGHT_SENSOR, SENSOR_TOUCH);
SetSensor(LEFT_SENSOR, SENSOR_TOUCH);
#endif
SetPower(RIGHT_MOTOR,1);
SetPower(LEFT_MOTOR,1);
On(RIGHT_MOTOR);
until(RIGHT_SENSOR == 1);
Off(RIGHT_MOTOR);
PlaySound(SOUND_CLICK);
On(LEFT_MOTOR);
until(LEFT_SENSOR == 1);
Off(LEFT_MOTOR);
PlaySound(SOUND_CLICK);
}
test1.ncp
//this makes the robot run in a straight line
#define LEFT_SENSOR SENSOR_1
#define RIGHT_SENSOR SENSOR_3
#define LEFT_MOTOR OUT_A
#define RIGHT_MOTOR OUT_C
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV
#define LEFT_FWD OUT_REV
#define LEFT_REV OUT_FWD
#define WAIT_TIME 10
#define shimmy
task main()
{
#ifdef __RCX
SetSensor(RIGHT_SENSOR, SENSOR_TOUCH);
SetSensor(LEFT_SENSOR, SENSOR_TOUCH);
#endif
SetPower(RIGHT_MOTOR,3);
SetPower(LEFT_MOTOR,3);
SetDirection(RIGHT_MOTOR, RIGHT_FWD);
SetDirection(LEFT_MOTOR, LEFT_FWD);
int count = 0;
int countleft=0;
int countright=0;
//On(LEFT_MOTOR + RIGHT_MOTOR);
//while(countleft <5 || countright < 5)
while(count<10)
{
// On(RIGHT_MOTOR + LEFT_MOTOR);
////////////////////////////////shimmy code
#ifdef shimmy
On(RIGHT_MOTOR); //turn right motor
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
PlaySound(SOUND_CLICK);
On(LEFT_MOTOR); //turn left motor
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
PlaySound(SOUND_CLICK);
On(LEFT_MOTOR); //turn left motor
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
PlaySound(SOUND_CLICK);
On(RIGHT_MOTOR); //turn right motor
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
PlaySound(SOUND_CLICK);
count+=2;
#endif
//////////////////////////END OF SHIMMY CODE
/////////////////////////////////non shimmy walk
// Wait(10);
#ifndef shimmy
count+=1;
On(RIGHT_MOTOR + LEFT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR || LEFT_SENSOR); //sensor hit
Off(RIGHT_MOTOR + LEFT_MOTOR); //
if(RIGHT_SENSOR && LEFT_SENSOR) //both at the same time, great, continue
continue;
if(RIGHT_SENSOR){ //allow motor to catch up to the other
On(LEFT_MOTOR);
until(LEFT_SENSOR);
continue;
}
if(LEFT_SENSOR)
On(RIGHT_MOTOR);
until(RIGHT_SENSOR);
#endif
//////////////////////////////////end of non shimmy walk
}
/* //third walk, unused
if(LEFT_SENSOR){
PlaySound(SOUND_CLICK);
countleft+=1;
}
if(RIGHT_SENSOR)
countright+=1;
}
PlaySound(SOUND_DOUBLE_BEEP);
Off(RIGHT_MOTOR + LEFT_MOTOR);
int difference=0;
difference=countleft-countright;
if(countleft>countright){
difference=countleft-countright;
for(difference;difference>0;difference-=1){
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
}
}
if(countright>countleft){
difference=countright-countleft;
for(difference;difference>0;difference-=1){
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
}
}
*/
Off(RIGHT_MOTOR + LEFT_MOTOR);
PlaySound(SOUND_CLICK);
}
turn.ncp
//this is used to experiment with turns
#ifndef MRROBOTO
#define MRROBOTO
/// sensors
#define LEFT_SENSOR SENSOR_1
#define RIGHT_SENSOR SENSOR_3
// motors
#define LEFT_MOTOR OUT_A
#define RIGHT_MOTOR OUT_C
// directions
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV
#define LEFT_FWD OUT_REV
#define LEFT_REV OUT_FWD
// const time interval
#define WAIT_TIME 7
// const direction
#define left 0
#define right 1
#define RCOUNT 8
#define LCOUNT 8
#endif
task main(){
#ifdef __RCX
SetSensor(RIGHT_SENSOR, SENSOR_TOUCH);
SetSensor(LEFT_SENSOR, SENSOR_TOUCH);
#endif
SetPower(RIGHT_MOTOR,7);
SetPower(LEFT_MOTOR,7);
SetDirection(RIGHT_MOTOR, RIGHT_FWD);
SetDirection(LEFT_MOTOR, LEFT_FWD);
int dir=right;
int x;
int y;
// if(dir==left){
for(x=0;x<RCOUNT;x+=1){
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
}
Toggle(LEFT_MOTOR);
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
Toggle(LEFT_MOTOR);
// }
// if(dir==right){
for(y=0;y<LCOUNT;y+=1){
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
}
Toggle(RIGHT_MOTOR);
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
Toggle(RIGHT_MOTOR);
// }
}
figure8.ncp
#ifndef MRROBOTO
#define MRROBOTO
/// sensors
#define LEFT_SENSOR SENSOR_1
#define RIGHT_SENSOR SENSOR_3
// motors
#define LEFT_MOTOR OUT_A
#define RIGHT_MOTOR OUT_C
// directions
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV
#define LEFT_FWD OUT_REV
#define LEFT_REV OUT_FWD
// const time interval
// const direction
#define left 0
#define right 1
#define STCOUNT 16
#define RCOUNT 8
#define LCOUNT 8
// options
#define shimmy //do shimmy walk
#define WAIT_TIME 7
#define PWR 7
#endif
task main(){
#ifdef __RCX
SetSensor(RIGHT_SENSOR, SENSOR_TOUCH);
SetSensor(LEFT_SENSOR, SENSOR_TOUCH);
#endif
SetPower(RIGHT_MOTOR,PWR);
SetPower(LEFT_MOTOR,PWR);
SetDirection(RIGHT_MOTOR, RIGHT_FWD);
SetDirection(LEFT_MOTOR, LEFT_FWD);
Wait(20);
PlaySound(SOUND_DOUBLE_BEEP);
GoStraight(STCOUNT); // --<--
Turn(left,4); // | ^
//TurnL(); // | |
GoStraight(STCOUNT/2); // | -->-X-->---|
TurnL(); // | |
GoStraight(STCOUNT/2); // ^ |
Turn(left,4); // |---<---
GoStraight(STCOUNT/2);
TurnL();
GoStraight(STCOUNT/2);
Turn(left,4);
GoStraight(STCOUNT/2);
TurnL();
GoStraight(STCOUNT/2);
TurnR();
GoStraight(0);
GoStraight(STCOUNT/2);
//TurnR();
GoStraight(STCOUNT/2);
Turn(right,4);
GoStraight(STCOUNT/2);
TurnR();
GoStraight(STCOUNT/2);
Turn(right,4);
GoStraight(STCOUNT/2);
//TurnR();
GoStraight(STCOUNT/2);
Turn(right,4);
GoStraight(STCOUNT/2);
TurnR();
GoStraight(STCOUNT/2);
}
void GoStraight(int x){
int count = 0;
while(count < x)
{
#ifdef shimmy
////////////////////////////////shimmy code
On(RIGHT_MOTOR); //right
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
On(LEFT_MOTOR); //left
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
On(LEFT_MOTOR); //left
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
On(RIGHT_MOTOR); //right
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
count+=2;
//////////////////////////END OF SHIMMY CODE
#endif
#ifndef shimmy
count+=1;
On(RIGHT_MOTOR + LEFT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR == 1 || LEFT_SENSOR == 1); //sensor hit
Off(RIGHT_MOTOR + LEFT_MOTOR);
if(RIGHT_SENSOR && LEFT_SENSOR) //both hit, great; continue
continue;
if(RIGHT_SENSOR){ //allow slow motor to catch up
On(LEFT_MOTOR);
until(LEFT_SENSOR);
continue;
}
if(LEFT_SENSOR)
On(RIGHT_MOTOR);
until(RIGHT_SENSOR);
#endif
}
Off(RIGHT_MOTOR + LEFT_MOTOR);
}
void TurnL(){ //turn just a little bit left
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
}
void TurnR(){ //turn just a little bit right
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
}
void Turn(int dir, int steps){
int x;
int y;
int count=0;
if(dir==left) //toggle means motor reverses
Toggle(LEFT_MOTOR);
if(dir==right)
Toggle(RIGHT_MOTOR);
////////////////////////////////////start of actual turn
while(count<steps){ //same as go straight, weird, huh?
count+=1;
On(RIGHT_MOTOR + LEFT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR == 1 || LEFT_SENSOR == 1);
Off(RIGHT_MOTOR + LEFT_MOTOR);
if(RIGHT_SENSOR && LEFT_SENSOR)
continue;
if(RIGHT_SENSOR){
On(LEFT_MOTOR);
until(LEFT_SENSOR);
continue;
}
if(LEFT_SENSOR)
On(RIGHT_MOTOR);
until(RIGHT_SENSOR);
}
Off(RIGHT_MOTOR + LEFT_MOTOR);
////////////////////////////////end of turn
if(dir==left) //reset the motor to foward
Toggle(LEFT_MOTOR);
if(dir==right)
Toggle(RIGHT_MOTOR);
return;
/* ////////////////////////////shimmy turn
if(dir==left){
for(x=0;x<steps;x+=1){
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
Toggle(LEFT_MOTOR);
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
Toggle(LEFT_MOTOR);
}
return;
}
if(dir==right){
for(y=0;y<steps;y+=1){
On(LEFT_MOTOR);
Wait(WAIT_TIME);
until(LEFT_SENSOR);
Off(LEFT_MOTOR);
Toggle(RIGHT_MOTOR);
On(RIGHT_MOTOR);
Wait(WAIT_TIME);
until(RIGHT_SENSOR);
Off(RIGHT_MOTOR);
Toggle(RIGHT_MOTOR);
}
return;
//////////////////// end of shimmy turn
*/
}