# Summer Blog: Principles of Over Engineering

Hello, everyone. I just finished my first year of college for Automation and Robotics, and I decided that a great way to help improve my skills would be to work on making a robot over the summer. Specifically, I am making a 6 axis spherical robotic arm on a linear slide base (it has x,y,z, pitch, roll, and limited yaw via waist and positioning of the LSB, plus the LSB allows for about 40" of movement along the Y axis).

The purpose of this thread is to share the adventure so you can see what I’m doing and be inspired to create bigger and more powerful robots.

Goals for the project:

1. Design a build a working robotic arm.
2. Write a program for it allowing the end user to be able to use high level commands to tell it how to move (i.e. gotoPostion 1, openGripper, etc.)
3. Have options for both joint and cartesian positions (already did the math and solved this problem XD), and also give more options for how points are defined.
4. Have a variety of movement styles (position, linear, circular)
5. Eventually, with everything working, I would like to make multiple cortexes talk to each other and create a CIM cell type setup.

Construction has already begun and I had the opportunity to test some of my subsystems. I’ll likely post a video showing progress later, but for now, please enjoy the sight of me unboxing my mountain of parts:

Video of both axis moving:

So, that subsystem is working fairly well. The IME isn’t working on the LSB motor, so I’m going to have to look into that (I’m aware that people tend to have problems with it). The waist joint is actually doing a basic while pot < X kind of movement. The power settings are actually very low. The LSB is only running at half power and the waist is running at about 38. And this is using the little 2000mA battery.

As for the arm… my first design or two weren’t working right, so I got some inspiration and I redesigned it almost from scratch. I haven’t quite finished everything yet, but I have high hopes for this iteration. I know the arm is going to be slower than tiny tim riding a unicycle, but that’s okay. Perhaps I’ll have some footage of it tomorrow.

Today, I have been grinding away at the arm section once more. The aforementioned iteration was a flop:

But it has a pretty cool gear train, so I thought it was worth showing.

Fear not, though. With every failure comes learning. I continued to identify problems and solved them. I scrapped the idea behind the arm shown and started from scratch. I liked the design so much it is now attached to my LSB. I’ll show you some pictures later. It basically needs the wrist unit attached, and it needs to have a couple gearboxes installed, but it’s the closest thing I’ve gotten to a working prototype yet.

UPDATE
So, my new prototype seems to be almost exactly what I need, I just made it way too long. Tomorrow I will swap out the long bars for short ones, and I will move the vertical stands to the front to give it more reach and make room for the gearboxes. But, I have pictures and a video of the assembled arm to show you how it moves:

Video Demonstration:

By the way, feel free to comment and ask questions here.

“Cutting down the arm length”

Wait, don’t do that.

You have overcome a number of problems, so building enough torque to lift the arm shouldn’t be that hard. You can stack the smaller gears and drive the arm that way. Since you have mastered the use of the chain, you can chain drive the claw and move the motor farther up the arm.

Nice use of the “{even} bar linkage” to keep things in the same relative positions"

Testing the shoulder joint with style. This was the very first test, so I didn’t even know if it would work or not…

Built the gear box. Here is a video showing the joints in motion:

Had a couple days of downtime while I waited for my next batch of parts to come in. They arrived today and I started working again. While the giant arm was certainly cool and was basically working, I have indeed begun the switch to the smaller arm. There are two main reasons for the switch. One, the work area I want the arm to have was way smaller than the arm itself, and that didn’t leave any room for cool things for the arm to interact with. Second, the arm was very close to the torque limits of the gears and I found the start of some damage to my worm gears on the shoulder joint (probably from the “arm straight out” test). However, cutting the arm length by almost half essentially doubles the amount of torque I have to play with, given similar gearing.

So, this will be a good thing in the long run, and I’m glad I went ahead and tried to make it work with the big arm. I actually did learn a lot about the limits of my system and what I needed to improve.

Video showing the current progress: https://youtu.be/DgG65SSC6EQ

I’ve been silent for a while… that means it is time for a big update!

And, of course, a video! This shows shoulder, elbow, and wrist in action while the gripper holds a full pop can: https://www.youtube.com/watch?v=_sKj7Z5uFGU

And two pics that didn’t fit into the first post

I had the chance to sit down and program the robot to do joint degree value movements: https://www.youtube.com/watch?v=e7PkYJJ7zBM&index=8&list=PLv8xCaoGmnITarFWLohU2D5mh-_50ijD5

The robot can currently switch between automatic and manual move modes. When switched into automatic, it executes the stored program in the automatic task. In the video, it contains three basic move commands to different points I had pre-defined using a typedef struct (explained more in the video).

The robot also has an eStop, which saved me right when I first tried my code and found out that the elbow joint needed its motor value signs flipped.

Looks good! Nice product placement, too. We need more Vex robots in Dr. Pepper commercials.

You know that moment when you bought a bunch of large wire bundles and you ended up falling 6 inches short on the last wire you needed for a fully working robotic arm?

That picture is what sadness looks like. On the other hand, I’ve been making some great progress. That limit switch that didn’t quite make it is an addition to the gripper which trips when the claw is fully opened. This means I have three limit switches on the gripper which define fully open, fully closed, and product detected (trips only when the gripper closes on something). This gives me great potential for automatic control of the arm and should hopefully prevent crushing. I also found a spot to put a limit switch for resetting the IME on the wrist roll joint. The wrist will have about 270* of roll starting parallel to the base.

You can also see that I have tried to lay out my wires fairly nicely. I color coded the motors and numbered the sensors to help keep track of what wire was from what device.

And here is a video of the waist, shoulder, elbow, and wrist (pitch) working in automatic mode: https://www.youtube.com/watch?v=pOLKlDPCLx8&list=PLv8xCaoGmnITarFWLohU2D5mh-_50ijD5&index=9

I ordered some more wires and should be able to move forward later in the week.

And, since I’m bored, I’ll cover the general process of how I make the robot move and have points (I alluded to it previously).

I start with a typedef struct{} command which has variables for the degree values for each joint. Then I can use the newly created structure type (position) to globally define position variables. This means that all parts of the program have access to the positions. Then, I have a function “void movePos(position destination)”. The argument passes a position variable to the function. Why pass a global variable? Because it lets you pass the specific one you want in the function call and makes things easier to understand.

The movePos() function then takes the position it was passed and copies it to a unique position called targetPosition (this gets the values back to being global since the struct passed as an argument becomes local). Then, it sets a series of global bool values for whether each joint has finished moving to false, and then it starts an individual task for each joint’s movement. Finally, it waits for all of the bool values to return true before ending.

The movement tasks determine their current location, then use the corresponding targetPosition value and a conversion formula to their respective sensor types and gear ratios to figure out where they need to go to in terms of the sensor. Then it checks to make sure the target is within an acceptable range, and if it is, then it figures out which direction it needs to go in, and then it starts the motor in the appropriate direction. When it finishes, it stops the motor and changes the corresponding global bool to true.

The net effect of all this is that the person writing the “program” gets to use a simple command like, “movePos(positionOne);” and the robot automatically figures out where it needs to go from there and can move all of its motors independently and wait for all motors to finish moving before the program goes to the next step.

I’d post the code but it is over 500 lines long now between the manual control and all the automatic stuff. Maybe I’ll post snippets later.

I’m getting very close to being able to implement the amazing 11 equation process of feeding Cartesian coordinates and getting angle values back to set the arm in the correct position and the slightly-less-amazing-but-still-cool set of 3 equations to feed in angle values and get back Cartesian coordinates.

This is some really impressive work with VEX. The programming is some serious deal. Well, just like me – I’m still more of a control oriented person and I can make do with some simple mechanism with cool control. Not saying I can do any better but perhaps with some CAD you can possibly make it look better and have those shaft better protected against twisting. Just some of my thoughts.

Plus, I am guessing the reason people are not responding a lot to this is because this is too cool and too serious that we basically have nothing to add… haha.

Plus Plus I love your signature and although I don’t plan to explode my future instructors’ heads, I do plan to do some serious damage, rhetorically speaking…

Thanks a bunch, Martin! And FMM, too. That means a lot coming from you. I’ve certainly seen your posts and you have done a lot of stuff that is really cool. Heck, looking at some of the stuff that people have come up with on this forum, I feel like my project is too simple sometimes. Then again, this is going for major autonomous style movements and most of the stuff on the forum is based on driver control. Then again, some of the auton work people have done for NBN has been pretty cool, too.

Regardless, this has been an amazing project for me to work on since it is using pretty much every useful thing I learned over the past year. AFAIK, my instructor doesn’t know that I’m actually doing it, so I hope to get a great reaction. The thing is, after the first time I melted his brain, it has become almost impossible. I think I set the bar too high XD. You see, after having access to the SCORBASE ER-4U arms for a week, I took the final project and then took it to the next level. The project was just supposed to be drawing a house (ala stereotypical 5 year old style box with a triangle on top), then raising up and drawing it again. I said that was boring, so I wrote a program that you give three numbers to define the size and starting position of the house, and then it figures out where all the positions need to be automatically. Then, it uses functions with trigonometry (I call them trigonomegnomes in a magical forest when explaining them to non-technical people) to redefine the sets of positions to do a rotation that raises the angle of the house from 0 to 30 to 60 to 90 (like a barn raising). When I showed him the program, he entered a state of shock, face palm, and disbelief. If only I had it on video.

Anyway, I hear what you mean about the twisting. There is only so much I could do with the materials I had and the way everything fit together. There is a bit of twisting at the wrist joint. If I were to custom make parts for it, I could absolutely make everything stronger and more efficient, including redesigning the drive trains. However, as I am in an apartment and any mechanical or fabrication work would have to be done elsewhere (and in places other than my usual spots since they don’t have the material or machines I need), I opted to just go straight vex with the understanding that it might not be perfect. Also, in the event that the college I attend (which has a huge amount of VEX parts) wants one of these, they would be able to make one of their own and probably wouldn’t have to buy much. Perhaps I will make something even better for my final semester project in the spring.

All right, I’m feeling mathy now. I think I’ll try and draw up an explanation of cartesian to angle values and have it up in the next day or two. For now, most of the code:

First, this is how much stuff has to be done before you even get to main:

``````

task waistControl(); //manual control for waist
task shoulderControl(); //manual control for shoulder
task elbowControl(); //manual control for elbow
task wristControl(); //manual control for wrist
task eStopControl(); //controls eStop, should always be active
task modeSwitchControl(); //switches between manual and auto control
task moveShoulder();//controls automatic movement of shoulder
task moveElbow(); //controls automatic movement of elbow
task moveWrist(); //controls automatic movement of wrist
task moveRoll(); //controls automatic movement of wrist roll
task moveWaist(); //controls automatic movement of waist

//global variables
bool shoulderMoveDone = false; //false when not finished moving, true when finished
bool elbowMoveDone = false;
bool wristMoveDone = false;
bool waistMoveDone = false;
bool rollMoveDone = false;
bool gripperClosedProduct = false; //true only when gripper detects product
bool gripperClosed = false; //true only when gripper is completely closed.
bool gripperOpen = false; //true only when gripper has been opened all the way.
bool manualMove = true; //true when manual mode is active, false when automatic mode is active

//global constants
float shoulderPotConversion = 28.33;
float elbowPotConversion = 18.43;
float wristPotConversion = 18;
float waistPotConversion = 16.96;
float rollEncoderConversion = 8.71;
int waistPotBaseline = 1800;
int shoulderPotBaseline = 1135;
int elbowPotBaseline = 313;
int wristPotBaseline = 2052;

//global structures
typedef struct{
float shoulderDeg;
float elbowDeg;
float wristDeg;
float waistDeg;
float rollDeg;
float lsbPosition;
} position;

//target position declaration. Don't touch!
position targetPosition;

//point list
position positionOne;
position positionTwo;
position positionThree;

//function prototypes
void movePos(position destination);
void openGripper();
void closeGripper();

``````

This is what the program in the video looks like in terms of the high level “end user”:

``````
{
movePos(positionOne);
wait(1);
movePos(positionTwo);
wait(1);
movePos(positionThree);
}//automaticMode

``````

That starts our journey into the dark depths of the program.

``````
void movePos(position destination)
{
//copy the passed structure to targetPosition
targetPosition.shoulderDeg = destination.shoulderDeg;
targetPosition.elbowDeg = destination.elbowDeg;
targetPosition.wristDeg = destination.wristDeg;
targetPosition.waistDeg = destination.waistDeg;

//reset bools for movement completion
shoulderMoveDone = false;
elbowMoveDone = false;
wristMoveDone = false;
waistMoveDone = false;
rollMoveDone = false;

//give time for tasks to start
wait1Msec(500);

//wait until all tasks are complete
while((shoulderMoveDone==false)||(elbowMoveDone==false)||(wristMoveDone==false)||(waistMoveDone==false)||(rollMoveDone==false))
{
wait1Msec(200);
}//while any of the bools are false

}//movePos

``````

and then the tasks go a little something like this:

``````
{
//where the joint is
int currentPotValue = SensorValue[waistPot];
//where the joint is going. Converts the deg value into a pot reading.
int targetPotValue = (targetPosition.waistDeg*waistPotConversion)+waistPotBaseline;
//if the target position is inside the acceptable range
if((targetPotValue < 3100)&&(targetPotValue > 800))
{
if(targetPotValue > currentPotValue)
{
//then increase pot until equal to target
while(SensorValue[waistPot]<targetPotValue)
{
startMotor(waistMotor, 127);
}
stopMotor(waistMotor);

}//if targetPotValue greater than or equal to currentPotValue
else //else if the targetPotValue is less than the currentPotValue
{
while(SensorValue[waistPot]>targetPotValue)
{
startMotor(waistMotor, -127);
}//while
stopMotor(waistMotor);
}//else if target less than current
}//if target in range

waistMoveDone=true;
}//moveWaist

``````

Yeah, and there is so much more. I need one of those move functions for each joint. Then there is a manual move function for the first four joints, and those all need their own tasks too.

``````
{
bool turnRight = true;

while(true)
{
if(SensorValue[waistButton]==1)//if button pressed
{
if(turnRight==true)
{
while((SensorValue[waistButton]==1))
{
startMotor(waistMotor, 127);
wait1Msec(100);
}//while button is pressed
turnRight=false;//change boolean
stopMotor(waistMotor);
}//if turnRight is true
else//if turnRight is false
{
while((SensorValue[waistButton]==1))
{
startMotor(waistMotor, -127);
wait1Msec(100);
}//while button is pressed
turnRight=true;//change boolean
stopMotor(waistMotor);
}//else if turnRight is false
}//if button pressed
else
{
stopMotor(waistMotor);
}//else button not pressed
}//infinite
}//waistControl

``````

Aaaannddd… maybe I should just go to bed.

My stuff is cool? Haha thanks for the compliment!

So you are referring to actually drawing with this arm right? Because it reminds me of this, and your project is just the 3D version of it.

``````
``````

Nice. I love prototypes!!!

``````
task waistControl(); //manual control for waist
task shoulderControl(); //manual control for shoulder
task elbowControl(); //manual control for elbow
task wristControl(); //manual control for wrist
task eStopControl(); //controls eStop, should always be active
task modeSwitchControl(); //switches between manual and auto control
task moveShoulder();//controls automatic movement of shoulder
task moveElbow(); //controls automatic movement of elbow
task moveWrist(); //controls automatic movement of wrist
task moveRoll(); //controls automatic movement of wrist roll
task moveWaist(); //controls automatic movement of waist
``````

12 tasks? Cool! Just a reminder, I think the maximum number of tasks ROBOTC can have is 20.

Also, is this just open loop sensor control? No love for PID? Come on man!!! I mean given that you use heavy gearing and worm gears, the motors appear to hold the positions well. But some little PID controls will certainly blow your instructor’s mind even more.

But… because this is such complicated and powerfully geared mechanism, and you’re just starting with closed loop feedback update control (I am just guessing ignore me if I’m wrong), try PID with something smaller first. If you plan to do more control on it.

Awesome stuff so far!

Similar to that video, but just using the arm and tracing it out in the air. My friend has a video of it but it is on facebook, not youtube, and I apparently don’t have the video myself.

Kind of funny though, after I did that house project, I asked for different work and they handed me an old Yamaha rectangular pick and place robot and told me to go figure out how it worked and to write a manual for it and then make it draw a yin-yang, and once I did that, they offered me a work-study position. The point being, the Yamaha has basically the same style of control as that drawing robot you showed.

That’s good to know regarding the tasks. What I learned looking it up just now is that the code can only have 20 simultaneous tasks running (main + 19 others). Luckily, I know that tasks take up CPU time so I had already programmed it to turn off/on the appropriate tasks when switching between manual and automatic control modes. So, when I switch into automatic mode, all of the manual control tasks get turned off except for the one that monitors the button which switches between the two. That means I should have as many as 10 tasks free at a time since during an automatic movement, there would currently be 10 tasks running (main, estop, mode switch, automatic task, lsb, waist, shoulder, elbow, wrist pitch, and wrist roll). Opening and closing the gripper are simple steps which are done between movements, so those are contained in functions.

I am familiar with closed loop vs. open loop. I’ve been wondering what PID meant for a while now, so I just googled it. For most of my joints, I don’t think it would be useful. I’m less concerned with speed as I am with position. As you said, the big joints have very high gear ratios and they use worm gears to prevent them from being back-driven. I’m honestly not sure how wise it would be to allow the power to be varied significantly given that it needs all the oomf it can get right now, especially under load, and making the arm constantly trying to “home” to a target position can lead to unwanted shaking and movements. Now that I think about it, perhaps I could just make an error variable that detects the difference between where it ended and where it was trying to go that updates at the end of the move and is applied as a modifier to the target position? While the program is running, every time you tell it to go to a position, it will continuously recalibrate how far off it was and make that the new error value to make the motor stop moving sooner or later.

``````
//global int
int waistPotError = 0;

{
//where the joint is
int currentPotValue = SensorValue[waistPot];
//where the joint is going. Converts the deg value into a pot reading.
int targetPotValue = (targetPosition.waistDeg*waistPotConversion)+waistPotBaseline;
//if the target position is inside the acceptable range
if((targetPotValue < 3100)&&(targetPotValue > 800))
{
if(targetPotValue > currentPotValue)
{
//then increase pot until equal to target
while(SensorValue[waistPot]<targetPotValue-waistPotError)
{
startMotor(waistMotor, 127);
}
stopMotor(waistMotor);

}//if targetPotValue greater than or equal to currentPotValue
else //else if the targetPotValue is less than the currentPotValue
{
while(SensorValue[waistPot]>targetPotValue+error)
{
startMotor(waistMotor, -127);
}//while
stopMotor(waistMotor);
}//else if target less than current
}//if target in range
wait1Msec(200);
waistPotError = targetPotValue-SensorValue[waistPot];

waistMoveDone=true;
}//moveWaist

``````

However, for the wrist roll and lsb, I might consider using PID control for position to constantly make sure they stay put since they do not have worm gears and can therefore be knocked out of place.

PID stands for Proportion, Integral and Derivative. I have a crappy PID ROBOTC tutorial on my channel, just to give you some basic ideas… I must make an updated version sooner.

Yup. That pretty much sums up what PID does for you. With the correct terminology! (error, target, etc.) What you described looks like the proportion part of PID, which is just sending a scaled down version of error to your motor as control value.

Target control is mainly just used to hold position and eliminate overshoot, both of which don’t seem to be concerns in your project. But if you are interested in further exploring some motor control algorithm, start with something small and manageable. It’s a whole new sort of fun from building something cool – you’re writing something cool.

Thanks, Martin. I’ll look into it more and keep it in mind for later on.

And now it is time to unleash the trigonomegnomes!

How to convert joint degree values relative to the base into Cartesian coordinates:

Given that you know the position of your LSB, B*(waist), S*(shoulder), E*(elbow), and P*(wrist pitch), which you should know by reading off the sensors, then:

Why is E positive going down and P negative? Because the user sees P when they are working. E is dealt with by me, and I think it is easier to deal with it as a positive value in this case.

Essentially, what you are doing is using basic trigonometry to take all the X components of each section of the arm and adding them together to find the X coordinate, and then doing the same with the Y components for Y and the Z components for Z. Pretty straight forward.

And then we try and go backwards and things get a little more complicated. This is how you take a Cartesian coordinate (x,y,z,pitch (P*), yaw (B*), roll (not actually relevant but may as well include it here) and figure out where the LSB should be and what angle S* and E* need to be set to:

It took me a few weeks to conceptualize this. I had a really weird epiphany while I was trying to figure it out. I had just redrawn the problem and the line I labeled Q popped out at me. Then a little voice spoke in the back of my head and said, “law of cosines”. And I was like, “nah, I don’t even remember what the LoC is.” And then after a moment I came back and was like, “huh, I wonder what the law of cosines is again?” so I googled it and it just so happens that it was exactly what I needed to figure out this problem. Basically, because of everything we know, we can figure out the length of a three-dimensional vector between the shoulder joint and the wrist joint, which completes the three sides of a triangle, which means the law of cosines can figure out what all the angles are.

I hope people enjoy those sets of equations. I’m sure someone can find a good use for them.

edit
Oh, I forgot to mention. The Cartesian algorithm above is written in terms of absolute position on the base with limited 6-axis movement, and not in terms of the LSB position. I will eventually make a few variations that represent different coordinate schemes, but I feel this is the one with the most utility.

I do. Very impressive work.

You know, for we engineers, studying math equations and physics is more like, when I encounter a problem, I remember something in math or physics that’s relevant. And I am able to do research on it, understand it and use it to solve the problem. Which is what you did. Great.

Question: It appears that one set of xyz can be achieved by multiple combinations of angles. So I assume your algorithm selects one of those possible combinations?

Also, did you make any mistakes with this and do any damage to your machine because of that mistake? Because a single value that’s out of range can cause the heavy gearing to destroy themselves. As what happened with most of my projects… If not, then I can only say, great calculation skills. Good job.

I haven’t actually implemented this yet. Once I finish the commands for taking a joint and moving to a specified degree value, then I can write code that uses this formula to figure out what degree values are needed.

You make an interesting observation. What you stated would normally be correct. There are, in fact, two sets of angles which could make the equation work. These two sets are mirror images of each other across Q in the diagram. Thankfully, this makes it very easy to work with. It starts with Stemp (the arccos bit in the equation for S). Because we use the law of cosines, the angle you get is actually between sides Q and A. Additionally, because the law of cosines is not based on the 3D orientation of the graph, it always will return a positive value for the angle since we aren’t looking at negative values for length. Then, to get it in terms of the base, we add the value of theta (the angle between the base and Q). That’s because theta is in terms of the base and Stemp is in terms of Q, so Stemp+theta=true S* above base.

Then, because of all of that, we can declare the direction of movement that corresponds with the automatically positive angle of S* to be up from parallel to the base, and E* will naturally fall where it needs to be since it follows similar rules and uses the value of Strue when being solved for. Therefore, it is automatically assumed that the angle of Stemp is measured up from Q.

You can also note that in my diagram, I show the wrist being higher than the shoulder, but the equation works exactly the same when it is lower.

In regards to making mistakes, I haven’t implemented it yet so I don’t know. But, I can tell you that the first thing I did when I started to implement the automatic movements was to make an eStop button that stopsAllTasks when pressed. In fact, the very first time I started the program up, I had made one of my motors run in the wrong direction and it was trying to go to a pot value in the opposite direction, which meant it couldn’t stop on its own. Thankfully, I had my hand on the eStop like a good tester and stopped it before it could do any harm.

Just remember to include this task and start it before any other tasks:

``````