Part 1) Visualizing 3-Axis Accelerometer Readings in Processing by Christopher Hazlett
It's been a while since I've had the chance to do anything vaguely electronic. Sure, I've painted rooms in my house, installed ceiling fans, added insulation to my attic, but that's a far cry from programming in Wiring or Processing. So, thankfully, after getting my new workspace all put together, I got the chance to play with some of the parts I've had waiting in a few SparkFun boxes.
So I started playing around with a 3-Axis Accelerometer in the hopes of dreaming up some project or other. So I hooked it up to my Arduino and my Arduino to my computer and wrote a little Processing code to graph it all into pretty colors. As with all of my projects, the first step for me is understanding and since I didn't have much experience with Accelerometers a little crash course was in order. As it turns out, it's a fairly simple sensor to use (or collection of 3 sensors: x, y, z, I should say). Simply plug the VCC connector into the Arduino 3V pin (not the 5V pin. The ADXL3305 chip is only rated to 3.3V), the ground into ground and the x, y, and z pins into 0,1,2 analog pins. The code for the Arduino is simple:
The Arduino Code
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 | #define X_AXIS 0 #define Y_AXIS 1 #define Z_AXIS 2 void setup() { Serial.begin(9600); } void loop() { int x = analogRead(X_AXIS); int y = analogRead(Y_AXIS); int z = analogRead(Z_AXIS); Serial.print(x); Serial.print('|'); Serial.print(y); Serial.print(':'); Serial.println(z); } |
It takes the readings in and outputs them into a formatted string '[x]|[y]:[z]'. That's it. This is just for outputting data right now, so nothing special. It gets more interesting when we look at the processing.
The Processing Code in Action
The code that makes the sweet, sweet video above isn't necessarily complicated, but there may be a few things you haven't used in Processing before.
- map(value, low1, high1, low2, high2) - converts a value from one range into a corresponding value into another range.
- norm(value, low, high) - converts a value into a value from 0.0 to 1.0 based on the supplied range.
- pushMatrix() / popMatrix() - the pushMatrix() and popMatrix() methods allow you to apply rotation, translation, and other methods to a specific style. By issuing the pushMatrix() then calling the translate(), and rotateX, rotateY methods, you can then call popMatrix so those methods don't affect other elements being rendered by Processing.
The Processing Code
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 | import processing.serial.*; import processing.opengl.*; Serial myPort; int baudRate = 9600; int lf = 10; PFont font; int[] xAxis; int[] yAxis; int[] zAxis; int currentX = 0; int currentY = 0; int currentZ = 0; //these value were determined by taking readings from a resting position int oneGSensorValue = 400; float oneGMillivolt = oneGSensorValue * 4.9; int totalReadings = 400; int readingPos = 0; // the reading position in the array void setup(){ smooth(); size(600, 300, OPENGL); font = createFont(PFont.list()[270], 24); smallFont(); xAxis = new int[totalReadings]; yAxis = new int[totalReadings]; zAxis = new int[totalReadings]; for (int i=0; i < totalReadings; i++){ xAxis[i] = oneGSensorValue; yAxis[i] = oneGSensorValue; zAxis[i] = oneGSensorValue; } myPort = new Serial(this, Serial.list()[0], baudRate); myPort.bufferUntil(lf); noLoop(); } void serialEvent(Serial p){ String inString; try{ inString = (myPort.readString()); currentX = xValue(inString); currentY = yValue(inString); currentZ = zValue(inString); xAxis = insertValueIntoArray(xAxis, currentX, readingPos, totalReadings); yAxis = insertValueIntoArray(yAxis, currentY, readingPos, totalReadings); zAxis = insertValueIntoArray(zAxis, currentZ, readingPos, totalReadings); readingPos = readingPos + 1; // increment the array position }catch(Exception e){ println(e); } redraw(); } void draw() { background(#FEFFFC); drawGraph(xAxis, 100, color(#519050), "X - Axis"); drawGraph(yAxis, 200, color(#708CDE), "Y - Axis"); drawGraph(zAxis, 300, color(#D38031), "Z - Axis"); draw3d(currentX, currentY, currentZ); } void drawGraph(int[] arrToDraw, int yPos, color graphColor, String name){ int arrLength = arrToDraw.length; stroke(graphColor); for (int x=0; x<arrLength - 1; x++) { float normalizedLine = norm(arrToDraw[x], 0.0, 700.0); float lineHeight = map(normalizedLine, 0.0, 1.0, 0.00, 85.0); line(x, yPos, x, yPos - int(lineHeight)); } pushStyle(); smallFont(); stroke(#FFFFFF); fill(#FFFFFF); String gString = nfc(gFromSensorValue(arrToDraw[arrLength - 2]), 2); text(name + " : " + gString + " Gs", 10, yPos - 10); popStyle(); } void draw3d(int currentX, int currentY, int currentZ){ float normalizedX = norm(currentX, 0.0, 700.0); float normalizedY = norm(currentY, 0.0, 700.0); float normalizedZ = norm(currentZ, 0.0, 700.0); float finalZ = map(normalizedZ, 0.0, 1.0, 300.00, 0.0); float finalY = map(normalizedY, 0.0, 1.0, -3.5, 3.5); float finalX = map(normalizedX, 0.0, 1.0, -3.5, 3.5); pushMatrix(); ambientLight(102, 102, 102); lightSpecular(204, 204, 204); directionalLight(102, 102, 102, -1, -1, -1); shininess(1.0); translate(500, finalZ); rotateY(finalY + 1.0); rotateZ(finalX); fill(#E2E8D5); noStroke(); fill(#B76F6F); float heightWidth = finalX * 1.8; box(65, 65, 50); popMatrix(); } int xValue(String inString){ int pipeIndex = inString.indexOf('|'); return int(inString.substring(0,pipeIndex)); } int yValue(String inString){ int pipeIndex = inString.indexOf('|'); int colonIndex = inString.indexOf(':'); return int(inString.substring(pipeIndex+1, colonIndex)); } int zValue(String inString){ int colonIndex = inString.indexOf(':'); return int(inString.substring(colonIndex + 1, inString.length() - 2)); } /* This little method creates a running tally of all the incoming sensor readings and then, when it reaches the end of the array, it pops the first one of the beginning and inserts a new value in at the end...thus keeping a running tally of the last 400 readings (it can be for any length array, that's just what it's set to for this project). This works a lot like an RRD graph where my inspiration came from. */ int[] insertValueIntoArray(int[] targetArray, int val, int pos, int maxLength){ if(pos > (maxLength-1)){ // if the pos == maxSize, shift the array to retain the original value int[] returnArray = subset(targetArray, 1, maxLength-1); returnArray = expand(returnArray, maxLength); returnArray[maxLength-2] = val; return returnArray; }else{ targetArray[pos] = val; return targetArray; } } /* This conversion will vary from project to project and if you're project is relying on battery power the reading may need to be adjusted to give you true one G as your battery power decreases. All of this is due to the output of the X,Y, and Z sensors and their coorelation to the incoming voltage at VCC Check out the specs for the ADXL335 (part of the break out board from Sparkfun.com) here: http://www.analog.com/en/sensors/inertial-sensors/adxl335/products/product.html */ float gFromSensorValue(int sensorValue){ //convert analog value into millivolts float mvValue = sensorValue * 4.9; return mvValue/oneGMillivolt; } void smallFont(){ textFont(font, 24); } void mediumFont(){ textFont(font, 30); } void largeFont(){ textFont(font, 40); } |
This is just the first step of a larger project to create a DIY radio control using an xBee and this 3-axis accelerometer.
Happy Coding.
- Chris
Arduino LED Shield by Jimmy P. Rodgers by Christopher Hazlett
Jimmy P. Rodgers over at jimmieprodgers.com (appropriately enough) spent some hard earned brain power developing a 126 LED shield for an Arduino. Nice work sir, nice work.
Take a look at it over at jimmieprodgers.com.
- Chris
Beacon Locating Robot – Powered by Arduino and IR Transceiver by Christopher Hazlett
After I built my first Arduino-based robot, I wanted to graduate to a little more advanced behavior, and I had it my mind to make a robot that would find a home base via an IR Beacon. Plus I saw that Pololu had the perfect parts for the job, the IR Beacon Transceiver pair. My wife was gracious enough to get me some IR distance sensors, a servo and the transceivers for my birthday in August. If she keeps supporting my hobbies like this, she'll have more robots than she can handle. As always, I'll start with the parts.
The Parts
I, of course, re-used parts from my last robot, but I'll include the links here as well. My robots are nothing if not a source of parts for the next robot.
- Arduino (1 @ $30.00 from Adafruit.com) - I actually got the Arduino Starter kit from Adafruit.com which also came with the 9V battery holder you see in the pictures and videos below.
- Arduino Motor Shield (1 @ $19.50 from Adafruit.com) - I used the motor shield again in this project because I only needed access to the six analog pins for the sensors on the Arduino and it's just so darn easy to use.
- RP5 Tracked Chassis (1 @ 49.95 from Pololu.com) - This chassis has proven to be very versatile and big enough to support a wide array of sensors.
- IR Beacon Transceiver Pair (1 @ 49.95 from Pololu.com) - Fantastic kit from Pololu.com (my favorite store for all things robotic. This acts as the core of the robot's sensors.
- Sharp GP2Y0A02YK0F Analog Distance Sensor 20-150cm (1 @ 13.95 from Pololu.com) - Pretty much your standard IR distance sensor from Sharp.
- GWS PICO Sub-Micro Servo (1 @ 15.95 from Pololu.com) - Great little servo. There's not much more to it than that.
- Pololu RP5 Expansion Plate RRC07A (Narrow) Solid Black (1 @ 6.95 from Pololu.com) - Pololu.com just recently started offering this gem and makes changing and building a robot on the RP5 chassis a little more like geek nirvana than it was before.
- Pololu RP5 Expansion Plate RRC07B (Wide) Solid Black (1 @ 11.95 from Pololu.com) - The bottom plate on the robot.
- Aluminium Standoffs (2 @ 1.95 from Pololu.com) - Little posts...nothing more, nothing less.
- Tamiya 70164 Universal Metal Joint Parts (2 @ 3.90 from Pololu.com) - These are great construction pieces to have lying around. I didn't actually use both kits, but I have two and I know I used scews and nuts from both.
I also used various crimped and un-crimped wires lying around that I use regularly, but that's a separate post.
The Robot
![]()

It's a pretty simple concept, place a home base somewhere and then have the robot find said home base from it's current location and make it's way there. When I first started thinking about how to execute this project, I thought it was going to be much more complicated to institute the seeking behavior I was looking for. The IR transceivers from Pololu.com were much better than I thought they'd be and had a lot of power. As you can see in the video below, the beacons are powerful enough to work around corners, so I never ended up putting a seeking algorithm in place. The beacon took care of that for me. All I had to do was make sure it avoided walls on its way to the home base. To accomplish that, I put the servo and Sharp distance sensor on the front and performed a sweep with the sensor and read the values from analog input 0. That's the bare bones of the robot.
The Home Base

For the home base, I took one of the transceivers and raised it to the height of the robot's head. This ended up being unnecessary. The robot would find the beacon if it was at any level. The home base didn't have to do anything, so I just powered it up and let it run.
The Code
There are a couple of gotchas when writing the code. Because the IR transceivers are always reading, I had to do a comparison of each reading to determine which of the directional sensors was the highest. You can't just read the input as an on or off. The second and harder portion of the code (and actually the most fun) was determining which direction to make the robot move. At first, I just let the robot move after determining which direction was currently being read from the beacon. In practice, this seems completely fine, but you'll see in the videos that the beacon itself changes directions quite frequently. This made the robot indecisive. To combat this problem, I figure out the Mode of the readings. Essentially, I took the last ten readings and counted what the most prevalent direction was. This smoothed out the robot's behavior and it behaved as expected.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 | #include <Servo.h> #include <AFMotor.h> #define TOPSPEED 200 // SERVO SCANNING VARIABLES // Servo myservo; // create servo object to control a servo int servoPosition = 0; // variable to store the servo position const int SERVO_PIN = 10; int floorState = 0; long frontReading = 0; boolean scanIncrement = true; //increase position? byte servoIncrementValue = 6; byte servoDecrementValue = 6; // MOTOR VARIABLES // AF_DCMotor rightMotor(4, MOTOR12_8KHZ); AF_DCMotor leftMotor(3, MOTOR12_8KHZ); // TRANSCEIVER VARIABLES // int notFoundSensitivity = 500; int west = 0; int south = 0; int east = 0; int north = 0; int dir = 0; boolean detected = false; // TRANSCEIVER DIRECTION MODE VARIABLES // const int NUM_READINGS = 10; int directionReadings[NUM_READINGS]; int modeOfDirections = 1; int index = 0; void setup() { Serial.begin(38400); myservo.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object myservo.write(0); rightMotor.setSpeed(200); leftMotor.setSpeed(200); delay(1500); } void loop() { scan(); move(); } void move() { if(servoPosition >= 0 && servoPosition <= 84 && frontReading > 550){ //Object on the left turnLeft(); }else if((servoPosition >= 85 && servoPosition <= 105) && frontReading > 600){ // Object in Front turnAround(); }else if((servoPosition >= 106 && servoPosition <= 180) && frontReading > 550){ // Object on the Right turnRight(); }else{ moveTowardBeacon(); } } void moveTowardBeacon() { readTransceiverandSetDirection(); if(modeOfDirections == 3 || modeOfDirections == 4){ //South or West turnRight(); } else if(modeOfDirections == 2) { // East turnLeft(); } else if(modeOfDirections == 1){ //North moveForward(); } } void scan() { scanIncrement ? servoPosition+=servoIncrementValue : servoPosition-=servoDecrementValue; //increment or decrement current position if (servoPosition >= 180){ scanIncrement = false; servoPosition = 180; } else if (servoPosition <= 1){ scanIncrement = true; servoPosition = 1; } frontReading = measureFront(); //Serial.print(servoPosition); //Serial.print("|"); //Serial.print(frontReading); //Serial.print(";"); myservo.write(servoPosition); delay(15); } long measureFront() { return analogRead(0); } void moveForward(){ Serial.println("Move Forward"); rightMotor.run(FORWARD); leftMotor.run(FORWARD); } void speedUp(){ for (int i=0; i==TOPSPEED; i++) { rightMotor.setSpeed(i); leftMotor.setSpeed(i); } } void slowToStop(){ for (int i=TOPSPEED; i==0; i--) { rightMotor.setSpeed(i); leftMotor.setSpeed(i); } rightMotor.run(RELEASE); leftMotor.run(RELEASE); } void turnLeft(){ Serial.println("Turn Left"); rightMotor.run(BACKWARD); leftMotor.run(FORWARD); } void turnRight(){ Serial.println("Turn Right"); rightMotor.run(FORWARD); leftMotor.run(BACKWARD); } void stop(){ Serial.println("Stop"); rightMotor.run(RELEASE); leftMotor.run(RELEASE); delay(500); } void turnAround(){ stop(); delay(500); moveBackward(); delay(300); turnLeft(); delay(300); stop(); //runAway = true; } void moveBackward(){ Serial.println("Move Backward"); rightMotor.run(RELEASE); leftMotor.run(RELEASE); rightMotor.run(BACKWARD); leftMotor.run(BACKWARD); } // BEACON LOGIC // void readTransceiverandSetDirection(){ west = analogRead(2); south = analogRead(3); east = analogRead(4); north = analogRead(5); getDirection(); setModeOfDirections(); } boolean foundBeacon(){ if(west < notFoundSensitivity and east < notFoundSensitivity and south < notFoundSensitivity and north < notFoundSensitivity){ return false; }else{ return true; } } void getDirection(){ int minValue = 1200; if(minValue > west){ minValue = west; dir = 4; } if(minValue > south){ minValue = south; dir = 3; } if(minValue > east){ minValue = east; dir = 2; } if(minValue > north){ minValue = north; dir = 1; } addDirectionToReadings(); //Serial.print("W:"); //Serial.print(west); //Serial.print(" | S:"); //Serial.print(south); //Serial.print(" | E:"); //Serial.print(east); //Serial.print(" | N:"); //Serial.println(north); //Serial.println("================================="); //if(dir == 1){ // Serial.println("North"); //}else if(dir == 2){ // Serial.println("East"); //}else if(dir == 3){ // Serial.println("South"); //}else if(dir == 4){ // Serial.println("West"); //} } void addDirectionToReadings(){ directionReadings[index] = dir; index = (index + 1); if (index >= NUM_READINGS) // if we're at the end of the array... index = 0; } // ========================================= // In order to smooth out the directions readings from the // IR transceiver, You have to take the mode (most prevalent number in a collection) // of the directionReadings Array. This allows the program to determine which // direction is being read the most from the device. // Otherwise, the readings make the robot squirrelly. // ======================================== void setModeOfDirections(){ int currentValue = directionReadings[0]; int counter = 1; int maxCounter = 1; int modeValue = modeOfDirections; int directionCounts[4] = {0,0,0,0}; //{North(1), East(2), South(3), West(4)} for (int i = 1; i < NUM_READINGS; ++i){ Serial.print(directionReadings[i]); Serial.print("|"); ++directionCounts[directionReadings[i]-1]; } //Determine mode of directions from count array Serial.println(""); int modeCount[2] = {1,directionCounts[0]}; //This array holds the current maximum count and the direction it points to. for(int i = 0; i < 4; ++i){ //Serial.print(directionCounts[i]); //Serial.print("|"); if(modeCount[1] >= directionCounts[i]){ modeCount[0] = i + 1; // set direction modeCount[1] = directionCounts[i]; //set count } } modeOfDirections = modeCount[0]; Serial.println(""); Serial.print("Direction Mode: "); Serial.println(modeOfDirections); } |
There you have it. Let me know if you have any questions about the code or construction.
- Chris
Light-Seeking Robot using the Arduino by Christopher Hazlett
This is a repost from my (Chris's) old blog www.integratechange.com on May 5, 2009.
I've always been interested in physical computing. Even though I love creating software, I've always felt that creating websites and software doesn't go far enough to satiate my creative desire. Enter the Arduino, a great open-source micro processor and prototyping board for around $30. This post marks the first, in what I hope will be a long line of tutorials and projects using the Arduino to develop robots and sensor driven applications that I hope to release into the wild. This post details a light-seeking robot with an Arduino brain, kind of the "Hello World!" application for autonomous robotics.
The Parts
I got a number of parts from several different sources. I had some parts lying around, but I'll also provide links where you too can buy yourself all the light-seeking robot parts your heart desires.
- Arduino (1 @ $30.00 from Adafruit.com) - I actually got the Arduino Starter kit from Adafruit.com which also came with the 9V battery holder you see in the pictures and videos below.
- Arduino Motor Shield (1 @ $19.50 from Adafruit.com) - You don't actually need the motor shield, but it provides a great base for prototyping and I recommend it if you're doing repeated prototyping with motors and servers.
- RP5 Tracked Chassis (1 @ 49.95 from Pololu.com) - I picked this chassis because it has all the necessary parts: geared motor assembly, tracks (who doesn't like tracks), battery pack, etc. You can make your own chassis, of course, but for those interested, this is a nice one that's virtually plug and play.
- CDS Photoresistors (2 @ $1.50 from Adafruit.com) - For the eyes. You can get these from anywhere, but if you order from Adafruit.com, you might as well get some of these too.
- 10 Kohms Resistors (4) - You can get these from anywhere.
- Momentary Switch - (1 @ $3.95 for a Pack of 4 from Radio Shack) - It's a tiny little push button switch, and you can use any you would like...this is just the one I used.
Prototype I - Proof of Concept
While I was waiting for my chassis to arrive, I put together an initial prototype to test the circuit and code. The first prototype was rough, to say the least, and it didn't move, but the code worked. I used the prototype board you see in the picture (available at Radio Shack) as a base to mount (tape) the motors to. I put tape on the axles to see if the code was actually making the motors turn the right way. It was.

And here's a video, showing it work in all it's glory.
As it says in the video, I initially gave the robot seeking and avoiding behavior (you can see it in the code below in the function foundSource()), but I realized that there are times when the robot will never find it's source (especially if that source is the sun) and that it was needless behavior. I left the runAway variable in the code through so you could easily turn the robot into a photophobe. You could also mount a switch on the robot that changes the behavior from photovore to photophobe as well.
Prototype II
Once I got the chassis from Pololu, I put together a better prototype, one that moves and turns around when it runs into something...you know, typical robot behavior. In the following pictures, you can see the very crude method of assembly, but it works...and that was really the point.

I literally moved the prototype board in the first picture above to the top of the chassis and hooked up the motors to the motor shield. I added a small switch for the motor power so I could stop screwing and unscrewing the motor's power supply. Everything is just taped down. This is, again, more of a proof of concept than a finished robot. I plan on making a custom motor driver for the chassis and better mount for the Arduino and other sensors.

To give the push button more surface area, I added a plow to the front, with an "ultra-strong" electrical tape hinge. Obviously, when the robot runs into something, it pushes the button and the turnAround() function executes.

There's not a lot of room in the chassis, but you could replace the AA battery pack with a 9V to give you some more space. But there are mounting holes on the corners which is probably a preferable location for your robot's brains.
Here it is in action:
The Circuit
The circuit is fairly simple, especially because I'm using the Motor Shield. This circuit diagram illustrates the sensor circuitry and status led you see on the white bread board above. The motors are attached to the M1 and M2 points on the motor shield (positive leads toward the inside of the board). I used OmniGraffle to draw up the circuit, but I'm still looking for better templates so my circuits are prettier, more accurate, etc, but this drawing should illustrate the connections just fine.

The Code
You program the Arduino in Wiring, which is basically a library in C++. Forgive me any trespasses below. C++ is not a language I've worked in a lot, and the code below could most likely use some optimization. I originally had many delays written into the loop because that's what I saw a lot of other code doing. That resulted in erratic behavior and a lot of running into stuff even though the object's shadow should have caused the robot to avoid it. When I removed the delay and averaged the light sensor readings, I got a very responsive robot. I've also left a lot of code in the source because I'm working on building a robot base that I can use over and over again as I install more sensors and create more interesting behavior.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 | #include <AFMotor.h> #define NUMREADINGS 5 #define LEFTSENSOR 3 #define RIGHTSENSOR 5 #define BUMPSENSOR 2 #define TOPSPEED 200 int i; int valLefat; int valRight; int valCenter; int oldLeft = 0; int oldRight = 0; int sensitivity = 40; int threshold = 50; int topThreshold = 650; boolean runAway = false; int leftReadings[NUMREADINGS]; int rightReadings[NUMREADINGS]; int index = 0; // the index of the current reading int leftTotal = 0; // the running total int leftAverage = 0; // the average int rightTotal = 0; // the running total int rightAverage = 0; // the average AF_DCMotor rightMotor(2, MOTOR12_8KHZ); // create motor #2, 64KHz pwm AF_DCMotor leftMotor(1, MOTOR12_8KHZ); // create motor #1, 64KHz pwm void setup() { Serial.begin(9600); rightMotor.setSpeed(TOPSPEED); leftMotor.setSpeed(TOPSPEED); pinMode(BUMPSENSOR, INPUT); moveForward(); delay(500); for (int i = 0; i < NUMREADINGS; i++){ leftReadings[i] = 0; // initialize all the readings to 0 rightReadings[i] = 0; } } void loop() { checkForBump(); averageReadings(); checkLightandMove(); } void checkLightandMove(){ if(valLeft > threshold && valRight > threshold){ if((valLeft > valRight) && (valLeft - valRight > sensitivity)){ if(runAway == false){ turnRight(); }else{ turnLeft(); } }else if((valLeft < valRight) && (valRight - valLeft > sensitivity)){ if(runAway == false){ turnLeft(); }else{ turnRight(); } }else{ moveForward(); } }else{ //turnAround(); stop(); } if(valLeft > topThreshold && valRight > topThreshold){ runAway = false; } //delay(500); } void checkForBump(){ int bumped = digitalRead(BUMPSENSOR); Serial.println(digitalRead(BUMPSENSOR)); if(bumped == HIGH){ Serial.println(); turnAround(); } } boolean foundSource(){ return oldLeft > valLeft && oldRight > valRight; //the robot has reached the source of the light, or the point of maximum brightness } void moveForward(){ Serial.println("Move Forward"); rightMotor.run(FORWARD); leftMotor.run(FORWARD); } void speedUp(){ for (i=0; i==TOPSPEED; i++) { rightMotor.setSpeed(i); leftMotor.setSpeed(i); } } void slowToStop(){ for (i=TOPSPEED; i==0; i--) { rightMotor.setSpeed(i); leftMotor.setSpeed(i); } rightMotor.run(RELEASE); leftMotor.run(RELEASE); } void turnLeft(){ Serial.println("Turn Left"); rightMotor.run(BACKWARD); leftMotor.run(FORWARD); } void turnRight(){ Serial.println("Turn Right"); rightMotor.run(FORWARD); leftMotor.run(BACKWARD); } void stop(){ Serial.println("Stop"); rightMotor.run(RELEASE); leftMotor.run(RELEASE); delay(500); } void turnAround(){ slowToStop(); delay(500); moveBackward(); delay(2000); turnLeft(); delay(2000); stop(); //runAway = true; } void moveBackward(){ Serial.println("Move Backward"); rightMotor.run(RELEASE); leftMotor.run(RELEASE); rightMotor.run(BACKWARD); leftMotor.run(BACKWARD); } void averageReadings(){ leftTotal -= leftReadings[index]; // subtract the last reading leftReadings[index] = analogRead(LEFTSENSOR); // read from the sensor leftTotal += leftReadings[index]; // add the reading to the total rightTotal -= rightReadings[index]; // subtract the last reading rightReadings[index] = analogRead(RIGHTSENSOR); // read from the sensor rightTotal += rightReadings[index]; // add the reading to the total index = (index + 1); // advance to the next index if (index >= NUMREADINGS) // if we're at the end of the array... index = 0; // ...wrap around to the beginning valLeft = leftTotal / NUMREADINGS; valRight = rightTotal / NUMREADINGS; Serial.print(valLeft, DEC); // prints the left sensor value Serial.print(" | "); Serial.println(valRight, DEC); // prints the right sensor value } |




