Category Archives: News

Want to participate in a research study?

Where: Leon Root, MD Motion Analysis Laboratory at the Hospital for Special Surgery in New York City

Who: Healthy subjects age 20-45 with no injuries in the past 6 months that affect walking.  You must be able to walk independently for at least 30 minutes.  You must also routinely engage in at least 20 minutes of any aerobic activity (walking, active commuting, bicycling, jogging, swimming, etc.) twice a week.  You must not be pregnant, be a professional athlete, or have an illness that affects metabolism (thyroid issues, diabetes, etc.).

When: You will visit the lab once for approximately 2 ½ hours.

What: During your visit to the lab, your walking motion will be collected using motion analysis cameras and your energy expenditure will be measured using a machine that evaluates the air you breathe while you’re walking on a treadmill.

It is not known how general motion contributes to energy expenditure.  The purpose of this study is to develop and validate a model of energy expenditure during walking, a commonly studied activity, that can be extended in the future to more general motions.

You will receive $50 at the end of the study visit.

Why: To help me with my PhD dissertation work!

If interested, please contact me by email at dustyn@nyu.edu or by phone at 201-452-1583.

Why I love teaching at ITP and Shel Silverstein

We’re more than half way through semester at the Interactive Telecommunications Program (ITP) at NYU that I teach at – a two year graduate program that has been described as art school for engineers, and engineering school for artists. I’ve been going through all of my students’ blogs in more detail in preparation for handing out midterm grades, and am reminded of how far they have all come since day one. I also thoroughly enjoy this exercise because of the perspective the students there have and the creativity they inject into activities that might sound somewhat simplistic to others. As an engineer by training, I find it’s easy to get bogged down with theory and technical jargon behind the scenes. But the sense of wonder that is captured in the project documentation of the students reminds me why I love teaching at ITP – the students remind me to look at things from a different angle.

This reminds me of a Shel Silverstein poem. I had Where the Sidewalk Ends growing up, and my sister had A Light in the Attic. We both read them cover to cover several times, and memorized some of the poems. Here is the one I thought of:

New World

Upside-down trees swingin’ free,
Buses float and buildings dangle:
Now and then it’s nice to see
The world– – from a different angle.

– Shel Silverstein

the tell-tale heart… pumpkin

Halloween is a great time for some nerdy fun. Last week I got this awesome open source hardware pulse sensor for Arduino. Then I realized pumpkin hacking was coming up at my hackerspace, NYC Resistor. I remembered some creepy Edgar Allen Poe stories about a heart beating under floorboards, so I figured I could combine the pulse sensor with pumpkin hacking for a creepy combo.

After getting the pulse sensor working (they have all the Arduino and even Processing code written for you) I found a cartoonish picture of the anatomy of a heart that I could copy onto the pumpkin.

I printed out the picture, cut it out, and selected a pumpkin it would fit on. I taped it to the pumpkin and traced around it, then removed the picture and filled in the lines freehand as best I could.

Then I used an exacto knife and some old dull linoleum carving tools Shelby had (good call Shelby – genius!) to carve off some pumpkin layers to allow light through.

I tested it with my red blinking bike light and it looks pretty great, but was impossible tog get a good picture or video of. I found a piece of an RGB LED strip I’m going to use to make it blink from my actual heart rate, then have an automatic mode were it beats when I’m not attached to it.

I rarely have time for fun random projects that don’t have something to do with work these days, so this was a welcome distraction. I highly recommend hacking your own pumpkin – there are still a few days left before Halloween!

Robotic Arm class at 3rd Ward

My awesome friend Stefani Bardin, who was the new Director of Education at 3rd Ward, invited me to teach a new class there and this is what we came up with:

Here are some steps we’ll be taking in class from the code side to get up and running…

If you haven’t already done so, download and install the appropriate Mac/Windows/Linux version of Arduino on your computer from here. I’ll have Mac and Windows versions on a thumb drive as well, and we’ll start with making sure everyone has this installed. You may need to download or point to drivers (especially on Windows, depending on the operating system) so follow the appropriate getting started guide below.

Then we’ll move on to the Blink example in class to make sure everyone has a working board before we move on to the robot. If you’d like to try it on your own, feel free to check out the getting started guide for Windows or Mac.

If there are any issues with getting blink examples to run on all your boards, we’ll be looking through these trouble shooting tips while I work with individuals.

Then we’ll teach the robots to strike a pose, wiggle, and do some awesome math to create other, more predictable motions!

Strike_a_pose: try this code on both of your motors by changing the pin number from 5 to 3. Change the angles and upload them several times, and take note of 1) the angle of the shoulder when it’s down on the wood, 2) the angle of the shoulder when it’s at the other extreme before it crashes into your breadboard, 3) the angle of the elbow when it’s perfectly straight, 4) the angles of the elbow at each of it’s extremes.

Strike_a_pose_doubletime

Make sure to take note of which direction your motor spins as you increase the angle, and what the angles are when the robot is straight and against the board.

Now, make it wiggle. Here’s how to wiggle the shoulder motor:
wiggle_wiggle_wiggle_shoulder

Can you figure out how to make both motors wiggle at once? If you’re stuck, try this:
wiggle_wiggle_wiggle

Now, see if you can adapt the code in this Instructable for your servo motor controlled arm.

If you did that, download Processing, then copy and paste the code from here and run it. Cool huh?

Now, thanks to David Cummings (a student in the 3rd Ward class), you can make the Processing Sketch actually drive the robot arm! Load this code onto the Arduino, then this code into Processing. You will probably need to adjust the angles depending on how your robot arm was built.

Oh, and here’s the Robotic arm BOM.

Resources:
Arduino (pay particular attention to the Forum area and the Reference tab)
NYU ITP Physical Computing
NYU ITP Mechanisms (also see Resources tab there)
Book – Making Things Move
Book – Getting Started with Arduino
Open Hardware Summit

Arduino Leonardo Speed Test

I posted previously (here) about an Arduino speed test I did with the Uno board. The rate that it could print the time, a comma, and an analogRead value was about 1150 Hz. I just got my hands on a Leonardo board, and wanted to run the same code to compare. The Getting Started Guide details how the serial buffer is likely to fill up fast and result in a visible lag in the serial monitor window. It suggests adding a delay, but I don’t want to do that, so I went into CoolTerm to try to read the data. The same code got me a rate of about 1370 Hz! So with this simple test, it looks like the Leonardo is about 20% faster than the Uno on this sketch. I’ll take it.

Git and GitHub for Windows

I’ve been whining about wanting to learn GitHub for a while now so after they just released a shiny new Windows Graphical User Interface (GUI), I figured it was time to jump on board. Everyone I know who uses Git is a Mac/Ubuntu/Linux, and I’m a PC, so it’s been tricky to get someone to look over my shoulder and push through the initially steep learning curve. I’m also not comfortable with command line stuff, and prefer GUIs. The old me would go to GitHub to find code and then click the ZIP button while no one was looking. Not anymore. So, in case it helps anyone out there, here are the steps I took and my best explanation of what this whole Git/GitHub thing is anyway. It’s also a guide to myself to refer to in the future.

WTF is Git?
Git is a distributed version control system that’s free and open source. I’m still learning what that means too. But from what I gather, instead of doing my normal “versioning” as I iterate through writing Arduino or Processing code, or even Word documents, that usually look like this:
RobotArm_v1
RobotArm_v1_withCurrentSensing
RobotArm_v2_2motors
etc…
Git will allow me to work with one master copy, while allowing you to revert back to old ones if needed, and giving you a way to compare different versions (“diffs”). Git is local to your computer.

WTF is GitHub?
GitHub is a free, open source way to share code online. You can work on stuff locally and when you are ready for the world to see it, you can “push” the changes to GitHub. This enables collaboration with others on the same code or document, without having to email each other different versions back and forth (the way I used to do this). The next person can “pull” down your changes, work on it, then push it back. Also, if your laptop gets stuck in a downpour or something, you know your code is still alive and well online.

A while back I followed the instructions on this great post by Nate at Sparkfun. However, it’s a bit outdated now, and the whole workflow was still throwing me for a loop. It also left me with some questions – why Tortoise Git? Are there alternatives? etc. It is a great intro to how to get it done on Windows, and is written in beginner friendly language, and I do have a GitHub account now, so it is still recommended reading.

Then I recently went to the official Git site and downloaded the latest version (1.7.11.1 in my case) with one click on the front page. Most beginner guides I find, including the front page of this site, talk about how Git is better than other versioning systems. Well, I’ve never used one before, so comparisons aren’t helpful to me, and I won’t put you through that either.

Then I followed the set up guide on the GitHub site. After downloading, the next step was the link that says “… take a crash course.” So I did. Ignore all references to other version control systems. I got lost somewhere around staging area but I figured if I read everything I was supposed to it would make sense later.

Now back to the GitHub site. In the next section, Set Up Git, it immediately jumps into stuff you’re supposed to type into the command line. What they don’t say, is if you’re a PC like me, you have to navigate to the newly installed Git folder in your Start–>All Programs menu and select Git Bash to get a black screen to type in these new commands.

Follow the rest of the tutorial on that page to get set up. You only have to do this stuff once.

WTF is a Repository?
A repository or “repo” is just a fancy name for a folder that holds stuff, and part or all of it can be put under your ninja version tracking control.   I just created some Processing code I want to upload yesterday that traces a path of a two degree of freedom robotic arm, so I’ll make a repo for that. This site, this site, this site, and most others will tell you how to add stuff to a repo using the command line. I hate the command line, so I looked for an alternative. THIS is where Tortoise Git comes in (now I get it Sparkfun, thanks!). I also found this guide which is a good walk through of the process. So follow the links to download the latest version of Tortoise Git, select all the defaults, then come back.

Okay now when you right click on any folder on your computer, you’ll see a few new options and a little turtle:

We have tell the Git system to pay attention to a particular folder. So, we’ll click the Git Create Repository here option. Click okay (leaving the box unchecked in the first window that pops up), then if you initialized Git in a folder that already had stuff in it like I did, click Proceed on the next window. All that did was add a little .git folder inside:

Download the Windows GUI
From this link. This new GUI claims to be the easiest way to use Git on Windows. Period. (We’ll see). Once you do this, you’ll see a new folder in your Start–>All Programs menu called GitHub, Inc. with a couple of icons inside. Click on the one that says GitHub.

Now if you go back and right click the folder again, and scroll down to TortoiseGit, you’ll see all kinds of options.

I want to add files to be tracked that I already have in the folder, so I go to Add

… and select the files I want to put under version tracking to also share on GitHub. This time, it’s just the P_2DOF_Arm.pde file. After selecting the file and clicking okay, you’ll see this screen

then click on Commit, and write a note in the box that pops up

Commit is Git talk for save to your local version tracking system. It’s a good idea to write little notes to yourself every time this happens so you can track progress.

NOW is when we get to use the shiny new Windows GUI (although the last few add/commit steps can also be done here). Open it up and drag the version tracked folder onto the main screen area, and you’ll see it added in gray

Now if you click on the right pointing arrow next to name of the folder you just dragged over, it will say open this repo. Do that and you’ll see all the content of the folder, and up top it will say push to github. Yeah! Let’s do that. When that’s done, you still have to hit publish to get the code onto GitHub. So do that. NOW you can use your browser to navigate to your GitHub account, and see the file in there!

Done and done. For now.  I’ll probably write a follow up post clarifying how the various functions of the Windows GUI work once I play around with it a little more.

Robotic Arm in Processing

I’ve been doing some pretty heavy reading and writing for the last few days, so I took some time “off” this afternoon to work on a mini project that I had been thinking of for awhile. I wanted to write a program in Processing that would allow me to choose points in the workspace of my actual robotic arm. The screenshot above shows the result so far.

If you download Processing and run the code below, you’ll see that the x and y values of the end of the arm print out on the screen under the code. My next step is to get these numbers to hover near the end of the arm as I’m moving it around. I’d also like to be able to put in joint constraints and have the reachable workspace of the robotic arm mapped out in a different color than the background, so I know what points I can use to drive the real robotic arm easily. Right now, the line segments representing the robotic arm just disappear when I get out of range! But that’s fun too.

/*
Planar Robotic Arm Visualizer
 
 by Dustyn Roberts 20120622
 */

//constants 
float a1 = 198; // shoulder-to-elbow "bone" length from Solidworks (mm)
float a2 = 220; // elbow-to-wrist "bone" length from Solidworks (mm) - longer c bracket

boolean elbowup = false; // true=elbow up, false=elbow down

int Xoffset = 425;
int Yoffset = 425;

//variables
float theta1 = 0.0; // target angles as determined through inverse kinematics
float theta2 = 0.0;

float c2 = 0.0; // is btwn -1 and 1
float s2 = 0.0;

float joint1X;
float joint1Y;

float joint2X;
float joint2Y;

void setup() {

  size(850, 650, P3D);
  background(157, 6, 50);
}

void draw() {
  background(123, 82, 171);
  strokeWeight(4);
  
  rotateX(PI); // make the y axis point up 
  translate(Xoffset, -Yoffset); // lower down the arm and move it to the middle so we can see it

  // figure out the joint angles needed to get to mouseX, mouseY position
  get_angles(mouseX-Xoffset, -mouseY+Yoffset);  
  //figure out the joint coordinates of joint1 to draw link 1
  get_xy();
  line(0.0, 0.0, joint1X, joint1Y);
  line(joint1X, joint1Y, joint2X, joint2Y);
  
  // print out the x and y values of the end effector
  print(mouseX-Xoffset);
  print(',');
  println(-mouseY+Yoffset);
}

// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(float Px, float Py) {
  // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px, 2) + pow(Py, 2) - pow(a1, 2) - pow(a2, 2))/(2*a1*a2); // is btwn -1 and 1

  if (elbowup == false) {
    s2 = sqrt(1 - pow(c2, 2)); // sqrt can be + or -, and each corresponds to a different orientation
  }
  else if (elbowup == true) {
    s2 = -sqrt(1 - pow(c2, 2));
  }
  theta2 = degrees(atan2(s2, c2)); // solves for the angle in degrees and places in correct quadrant

  // now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
}

void get_xy() {
  joint1X = a1*cos(radians(theta1));
  joint1Y = a1*sin(radians(theta1));

  joint2X = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
  joint2Y = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
}

Seeing Sensors: How to visualize and save Arduino-sensed data

Sometimes when you’re prototyping a project it’s nice to see what’s going on with your sensors or variables in your code. Sure you can watch analogRead() values scroll at warp speed on your Arduino Serial Monitor, but what if you want to save that data? Or look at a graph you make in real time instead of numbers? Maybe you want to do some self tracking and visualize your Fitbit stats, or compare two different light sensors or force sensing resistors? In this class I’ll demonstrate two workflows that help us do this…

The Basics
Check out the Analog In lab. Using this code:

int sensorPin = A0;    // analog input pin to hook the sensor to
int sensorValue = 0;  // variable to store the value coming from the sensor

void setup() { 
  Serial.begin(9600); // initialize serial communications 
}

void loop() {
  sensorValue = analogRead(sensorPin); // read the value from the sensor
  Serial.println(sensorValue); // print value to Serial Monitor
  delay(50); // short delay so we can actually see the numbers
}

With a potentiometer (or other sensor) hooked up to analog pin 0, you should be able to click the magnifying glass icon on the top right of the Arduino screen and see something like this:

Graphing Serial Data in Processing
Okay so now you have data streaming in from your Arduino through your USB cable, and can see the values on the Serial Monitor. But unless you have Matrix-like visual skillz, these streaming numbers might be hard to keep track of. The next step is to use another program called Processing to visualize this data.

Check out step 4 of the Serial Out Lab. Load this code onto the Arduino:

int sensorPin = A0;    // analog input pin to hook the sensor to
int sensorValue = 0;  // variable to store the value coming from the sensor

void setup() { 
  Serial.begin(9600); // initialize serial communications 
}

void loop() {
  sensorValue = analogRead(sensorPin)/4; // read the value from the sensor
  Serial.write(sensorValue); // print bytes to serial
  delay(10);
}

Then use this code in Processing:

import processing.serial.*;
Serial myPort;        // The serial port
float xPos = 0;             // horizontal position of the graph

void setup () {
  size(800, 600);        // window size

  // List all the available serial ports
  println(Serial.list());

  String portName = Serial.list()[0];
  myPort = new Serial(this, portName, 9600);

  background(#EDA430);
}

void draw () {
  // nothing happens in draw.  It all happens in SerialEvent()
}

void serialEvent (Serial myPort) {
  // get the byte:
  int inByte = myPort.read();
  // print it:
  println(inByte);

  float yPos = height - inByte;
  // draw the line in a pretty color:
  stroke(#A8D9A7);
  line(xPos, height, xPos, height - inByte);

  // at the edge of the screen, go back to the beginning:
  if (xPos >= width) {
    xPos = 0;
    // clear the screen by resetting the background:
    background(#081640);
  }
  else {
    // increment the horizontal position for the next reading:
    xPos++;
  }
}

If everything goes well, you should get a quickly scrolling graph that looks something like this:

Saving Data with CoolTerm and Graphing with Excel (or OpenOffice/Google Docs equivalent)
Sweet! Now you can “see” the numbers coming through the serial port in two different ways. Suppose you want to save that data to visualize, use as an outline for a laser cut data guru trophy, compare sensors, etc. One of the easiest ways to do this is through CoolTerm.

Check out step 3 of the Serial Out lab. Now upload this code to the Arduino:

int sensorPin = A0;    // analog input pin to hook the sensor to
int sensorValue = 0;  // variable to store the value coming from the sensor

void setup() { 
  Serial.begin(9600); // initialize serial communications 
}

void loop() {
  sensorValue = analogRead(sensorPin); // read the value from the sensor
  Serial.print(millis()); // print the time in milliseconds since the program started
  Serial.print(',');
  Serial.println(sensorValue); // print to serial
}

Notice we’re printing two values separated by a comma. This will come in handy in a minute. Now open CoolTerm, and you should see something like this:

Big deal, right? Same thing as on the Arduino Serial Monitor pretty much, right? Well as far as the viewer goes, you’re pretty much right. However, CoolTerm has a built in function to allow you to save data to a text file that we can open up later in a dozen different programs to have a look. While your data is scrolling past, navigate to Connections–>Capture to Textfile–>Start (like this):

Note the short cut keys. After you start, a screen will pop up asking you where and how to save the file. Just choose the defaults for now – the timestamp in the file name is super helpful. After you start, do something interesting to your sensor (twist the potentiometer, cover the light sensor, etc.).

Okay! Now, open Excel or your spreadsheet program of choice (OpenOffice Calc, Google Spreadsheet, etc.) – they will have similar workflows. I’m on Excel 2007 on a Windows 7 machine, fyi. Go through the motions to open the txt file you just created (note you may have to make sure All Files is selected in the drop down menu shown on the bottom right of the screenshot below, or you’ll only see .xls files and not .txt files).

Excel will be a little confused at first as to why you’re opening a txt file, so it will make you go through a few steps. The first one looks like this:

Just leave the defaults and hit next. On this screen, check the box next to Comma as shown below:

Then click on Finish. If everything worked out, you should now be looking at two columns of data neatly separated where the comma used to be. Score! Almost done. Now, let’s graph this data. Highlight columns A and B, then choose Insert–>Scatter–>Scatter with Smooth Lines:

This will insert a graph into your spreadsheet and look something like this:

DONE! Now you can save it, add a title, label your axes, whatever you want. And you have a digital diary of what was going on at a particular moment in time with your sensor.

Writer’s Block

I love this tweet so much I wanted to save it to refer to and share:

Working on a journal article right now, and this is pretty much my routine – minus the window, since there are none in my office. At least I know I’m not alone!

Current Sensing with the ACS712 Hall Effect Sensor

After a conversation with JD Warren, one of authors of the excellent Arduino Robotics book, I decided to try the ACS712 hall effect current sensor. I soldered one to a surface mount to breadboard adapters and plugged it in with some capacitor values recommended on the first page of the data sheet. I first just tried to sense the current used by the “shoulder” motor in the robotic arm I’ve posted about before. Here’s a video (it runs way smoother now, promise):

I also hooked up a quick RC low pass filter at about 7 Hz, then I ran the robotic arm (code below) and graphed shoulder angle vs. current, and it looks like this:

The graph shows several cycles of the shoulder angle increasing and decreasing while the end effector draws an imaginary vertical line. And although the current signal is periodic, it’s not exactly clear what’s causing the profile to look the way it does. Also, there is no way the current scaling is right (should be under an Amp the whole time) but I’ve read the data sheet and looked at my code and can’t find the error. Can you?

/*
Robot arm control using hacked servos and a master arm with pots as control in
master-slave system.
+y is vertical, +x is to the right
drawing line/circle:

http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino

inverse kinematics:

http://www.micromegacorp.com/downloads/documentation/AN044-Robotic%20Arm.pdf


http://www.learnaboutrobots.com/inverseKinematics.htm

*/

#include <AFMotor.h>
#include <math.h>
#include <PID_v1.h>

// Instantiate both motors
AF_DCMotor shoulder(3);
AF_DCMotor elbow(4);

// DECLARE PINS
int ShoulderPin = A0; // to potentiometer on shoulder motor
int ElbowPin = A1; // to potentiometer on elbow motor
int ShoulderCurrentPin = A2; // to first ACS712 IC

// INITIALIZE CONSTANTS
double Kp_Elbow = 20; // this is the proportional gain
double Kp_Shoulder = 20;
double Ki_Elbow = 0.1; // this is the integral gain
double Ki_Shoulder = 0.1;
double Kd_Elbow = 1.0; // this is the derivative gain
double Kd_Shoulder = 0.75;

double Elbow_neg = 970; // joint limits of robotic arm using right hand rule for sign
double Elbow_pos = 31;
double Shoulder_neg = 210;
double Shoulder_pos = 793;

const double a1 = 198; // shoulder-to-elbow "bone" length from Solidworks (mm)
const double a2 = 220; // elbow-to-wrist "bone" length from Solidworks (mm) - longer c bracket

double highY = 350; // line drawing targets
double lowY = 250;
double constantX = 200;

boolean elbowup = false; // true=elbow up, false=elbow down

float Volt_per_amp = 0.185; // 185 mV/A resolution according to hardware page
 
// VARIABLES
double rawElbowAngle = 0.0; // initialize all angles to 0
double rawShoulderAngle = 0.0;

double elbowAngle = 0.0; // initialize all angles to 0
double shoulderAngle = 0.0;

double theta1 = 0.0; // target angles as determined through inverse kinematics
double theta2 = 0.0;

double actualX = 0.0;
double actualY = 0.0;
double predictedX = 0.0;
double predictedY = 0.0;
double y = 0.0;

double c2 = 0.0; // is btwn -1 and 1
double s2 = 0.0;

double pwmTemp = 0.0;

double pwmElbow = 50.0; // between 0 to 255
double pwmShoulder = 50.0;

float shoulderCurrentRaw; // the raw analogRead ranging from 0-1023
float shoulderCurrentVolts; // raw reading changed to Volts
float shoulderCurrentAmps; // Voltage reading changed to Amps

//Syntax: PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID elbowPID(&elbowAngle, &pwmElbow, &theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, DIRECT);
PID shoulderPID(&shoulderAngle, &pwmShoulder, &theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, DIRECT);

void setup() { 
  Serial.begin(115200); // set up Serial library

  elbowPID.SetSampleTime(5); // (ms) how often new PID calc is performed, default is 1000
  shoulderPID.SetSampleTime(5);

  elbow.setSpeed(pwmElbow); // set the speed to pwmElbow
  shoulder.setSpeed(pwmShoulder); // set the speed to pwmElbow

  elbowPID.SetMode(AUTOMATIC); // modes in the PID library that have to be set
  shoulderPID.SetMode(AUTOMATIC);

  elbowPID.SetOutputLimits(-255,255); // we'll use the sign to tell us motor direction
  shoulderPID.SetOutputLimits(-255,255);

  move_to_start(); //get to starting position
}

void loop() { 
  line_y();
}

void move_to_start() {
  do {
    get_angles(constantX, lowY);
    drive_joints(); // drive joints until actual position equals expected position
  }
  while( abs(theta1 - shoulderAngle) > 2 && abs(theta2 - elbowAngle) > 2 ); // bail when it's close
}

void line_y() {
  for(y = lowY; y < highY; y += 0.5) { // draw straight line up
    get_angles(constantX,y);
    drive_joints();
  }
  for(y = highY; y > lowY; y -= 0.5) { // draw straight line down
    get_angles(constantX, y);
    drive_joints();
  }
}

// Given predicted x,y position at any time (forward kinematics)
void get_xy() {
  predictedX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
  predictedY = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
  actualX = a1*cos(radians(shoulderAngle)) + a2*cos(radians(shoulderAngle+elbowAngle));
  actualY = a1*sin(radians(shoulderAngle)) + a2*sin(radians(shoulderAngle+elbowAngle));
}

// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(double Px, double Py) {
  // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); // is btwn -1 and 1

  if (elbowup == false) {
    s2 = sqrt(1 - pow(c2,2)); // sqrt can be + or -, and each corresponds to a different orientation
  }
  else if (elbowup == true) {
    s2 = -sqrt(1 - pow(c2,2));
  }
  theta2 = degrees(atan2(s2,c2)); // solves for the angle in degrees and places in correct quadrant

  // now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
}

void drive_joints() {
  // read the actual values from all the pots
  rawElbowAngle = analogRead(ElbowPin);
  rawShoulderAngle = analogRead(ShoulderPin);

  // constrain robot arm to ignore out of range values
  elbowAngle = constrain(rawElbowAngle, Elbow_pos, Elbow_neg);
  shoulderAngle = constrain(rawShoulderAngle, Shoulder_neg, Shoulder_pos);

  // now map the angles so they correspond correctly
  elbowAngle = map_double(elbowAngle, Elbow_neg, Elbow_pos, -110.0, 85.0);
  shoulderAngle = map_double(shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);

  elbowPID.Compute();
  shoulderPID.Compute();

  // DRIVE ELBOW: CW is forward, CCW is backward
  pwmTemp = abs(pwmElbow);
  elbow.setSpeed(pwmTemp); // now use the new PID output to set the speed

  if (pwmElbow < 0) {
    elbow.run(FORWARD); // turn it on
  }
  else if ( pwmElbow > 0) {
    elbow.run(BACKWARD); // turn it on
  }
  else elbow.run(RELEASE); // stopped

  // DRIVE SHOULDER: CCW is forward, CW is backward
  pwmTemp = abs(pwmShoulder);
  shoulder.setSpeed(pwmTemp); // set the speed

  if (pwmShoulder < 0) {
    shoulder.run(BACKWARD); // turn it on
  }
  else if (pwmShoulder > 0) {
    shoulder.run(FORWARD); // turn it on
  }
  else shoulder.run(RELEASE); // stopped
  
  shoulderCurrentRaw = analogRead(ShoulderCurrentPin);
  if (shoulderCurrentRaw > 512) {
   shoulderCurrentRaw = shoulderCurrentRaw - 512; 
  }
  else {
   shoulderCurrentRaw = 512 - shoulderCurrentRaw; 
  }
  shoulderCurrentVolts = shoulderCurrentRaw * (5.0/1024.0);
  shoulderCurrentAmps = shoulderCurrentVolts/Volt_per_amp;
 
  Serial.print(millis());
  Serial.print(',');
  
  //Serial.print(elbowAngle);
  //Serial.print(',');
  Serial.print(shoulderAngle);
  Serial.print(',');
  //Serial.print(pwmElbow);
  //Serial.print(',');
  //Serial.print(pwmShoulder);
  //Serial.print(',');

  Serial.println(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.print(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.print(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.println(shoulderCurrentAmps);
  
  //Serial.print(actualX);
  //Serial.print(',');
  //Serial.print(predictedX);
  //Serial.print(',');
  //Serial.print(actualY);
  //Serial.print(',');
  //Serial.println(predictedY);
}

double map_double(double x, double in_min, double in_max, double out_min, double out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}