Robotics 1
Final Challenge: Pick and Place Manipulator
You have now made it through the entire Robotics 1 course, and you are about to test your complete set of knowledge by making a working pick and place manipulator.  You've already learned everything you need to know to do this; you just need to put it all together.  Here is what your final device should do:

1. First, you run the PSoC code.  PSoC will wait quietly until it receives two values (X and Y).
2. Run your Python code.  The Python code will take a 'background' picture of the robot workspace, then alert the user to place the 'pick' object.
3. You will place the 'pick' object (any ferromagnetic object) in the robot workspace, and click a button in Python.  After the button is clicked, the Python code will take a 'foreground' picture, subtract the background, and find the position of the 'pick' object IN THE BASE FRAME.
4. The Python code will offset the values, if neccessary, in order to make them positive, and send the values to PSoC over UART.
5. Upon receiving the values, PSoC will use inverse kinematics to find the joint variable values which place the end-effector in that location, move to the location, turn on the electromagnet, move the electromagnet down with the rack and pinion, move the electromagnet back up, move the end-effector to the 'drop' location, and turn off the electromagnet.

In order to accomplish this, I recommend that you do the following five steps in order.  As you go, TEST EACH STEP alone, then test each step working together with all of the previous steps.  Don't move on to the next step until you have verified that the current step is working right.

There are five steps, and five in-class days to work on this.  Try as best as you can to keep up with the schedule, and you should be ready to go by the time Finals Week arrives.

You have all the right pieces to build Articulated, spherical, or SCARA manipulator types, but I recommend that you build a SCARA type.  We built this manipulator with just the first two joints in the Kinematic Diagrams part of class, in the second video, 'Set the Angles of Servos'.  The first servo should move between 0 and 180 degrees, and the second servo should move between -90 and 90 degrees.  You should be able to input some joint angles, and have the servos move to those angles.  In this step, you will want to get your servo 'tuning' as close as possible: check 0, 45, 90, 135, and 180 degree angles for the first servo, and check -90, -45, 0, 45, and 90 degree angles for the second servo.  You want these angles to be very accurate.  Then, all you have to do is add the rack and pinion so that it moves vertically.  When you attach the rack and pinion, attach it so that the two mounting holes on the rack are pointing down.  Next, attach the electromagnet in its clip to the rack using two of the long M3 screws and two nuts. 

Now, when you wire everything up, you want to make one change from how we did it in the Kinematic Diagrams section: we want to wire the servo power to the external power supply, instead of wiring it to VDD on the PSoC.  This will give our servos more current even though we have more parts drawing current now.  Make sure that ground of the power supply is connected to ground of the PSoC.

Then, wire up the motor using the motor driver chip.  We learned this in the DC Motors Torque and Speed section, in the third video 'Torque/Speed Curve'. 

Finally, wire up the electromagnet.  Use one M/F wire to connect one end of the electromagnet to a PSoC pin, and use another to connect the other end of the electromagnet to VDD.  When you set the pin low, the electromagnet will go on.  When you set the pin high, the electromagnet will go off.

Next, you need to be able to move the rack and pinion between two positions ('down' and 'up').  To do this, you can use any type of feedback control we learned - on/off or proportional.  Since we need this positioning code to run as a part of a larger code, you might want to limit the time spent in the control loop.  For example, suppose that in your positioning control loop, you have a delay of 10ms.  You might put the whole positioning control code inside of a 'while' loop that runs 300 times, thereby allowing 3 seconds for the rack to get into position.  After the while loop exits, immediately turn the motor off.  We wrote code like this for the first time in the On/Off Control part of class.

To test this part of the project, you might want to make sure you can do the following in a repeating loop:

1.  Move the rack and pinion into the 'up' position
2.  Move the two servos both to 45 degrees
3.  Move the rack and pinion into the 'down' position
4.  Wait 2 seconds
5.  Move the rack and pinion into the 'up' position
6.  Move the two servos both to 0 degrees
7.  Move the rack and pinion into the 'down' position
8.  Wait 2 seconds

This simulates the motion you will need to perform the whole pick-and-place operation, and makes a great framework to build upon in the next steps.

In this step, instead of setting two ANGLES for the servos as we did in the last step, you will set two X,Y positions.  We learned how to do this in the Inverse Kinematics for Position part of class

At this point, it is very useful to use your dry-erase marker to mark the robot workspace on your board.  If your inverse kinematics equations are correct, your robot will behave very strangely if you accidentally tell it to move to an X,Y position that is outside of its workspace.  When you mark the workspace, keep in mind that you can do 'elbow down' configurations ONLY with the equations we found in the Inverse Kinematics section of class.  Also, we can only reach positions in the first quadrant.  You will need to calculate other equations to reach positions in the second quadrant, or to use elbow-up configurations.

To test this step, try doing the same testing loop we had in the last step, except this time type in an (X,Y) position instead of two angles for the servos.  As you are testing this part, you might have to tweak your values for the link lengths to increase the positioning accuracy.  Also, keep in mind that being within about 1cm of the target location is usually close enough to pick up the object.

Now, we're going to take a break from the PSoC code for a while and start working on our Python code.  Look back at what we did in the Image Subtraction and Object Localization section of class, and the Camera Coordinates section of class.  Your Python code will need to take a picture of the 'background', give you a message that it is ready for you to place the 'pick' object, then take another picture, and process the image.

Processing the image will have to include:

(1) Converting to grayscale,
(2) Converting to black-and-white,
(3) Using the 'center of mass' approach to find the location of the object in units of 'pixels',
(4) Use a 'pixel-to-centimeter' conversion to find the location of the object in units of cm,
(5) Use a homogeneous transformation matrix to convert the position from the camera coordinate system to the base-frame of the manipulator

Test this process one step at a time.  When it is working correctly, you should be able to place the 'pick' object anywhere in the workspace, and Python will tell you the X,Y location of the object in the manipulator's base frame.

Now that the PSoC code and the Python code are both working correctly independently, we need to make them work together.  Add some code to your PSoC program to wait and do nothing until it receives a value over UART.  We learned how to do this in the UART Communication part of class.  Now, copy and paste that code one more time - the first time you get a UART value, you want to use it as the 'X' position.  The second time you get a UART value, you want to use it as the 'Y' position. 

Next, add some code to your Python program to send the X and Y values found from Step 4 over UART to the PSoC.  We did this in the same UART Communication part of class.  Here, you have to be careful about the values you are sending.  Remember that in UART communication, we are sending one 8-bit byte at a time.  An 8-bit byte can be between 0 and 255, and can only be an integer.  If you want to be able to send negative values, you can do so by first 'offsetting' the value before sending it, then 'offsetting' again after the value is received.  For example, suppose you want to be able to send values between -10 and 10.  You could first add 11 to the value, then send it, then subtract 11 in your PSoC code after the value is received. (Why 11 and not 10?  Remember that in your PSoC code, if you get the value 0 over UART, that is the same as not receiving a value at all.  So, you need to make sure you don't actually try to send the value 0.) 

Once you finish this step, your whole system should be working!