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 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 automaticMode(); //controls automatic mode
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”:
task automaticMode()
{
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;
//start tasks
startTask(moveShoulder);
startTask(moveElbow);
startTask(moveWrist);
startTask(moveWaist);
startTask(moveRoll);
//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:
task moveWaist()
{
//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.
task waistControl()
{
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.
Nice product placement, too. We need more Vex robots in Dr. Pepper commercials.
