We are using ROBOTC.
How would be make our robot hold a specific arm position?
How would we add “presets”?
We would like to be able to press a button, then the arm lifts to a certain height. How would this be done?
Thanks all!
You need sensor feedback from the arm, either encoder or potentiometer.
You need to drive the motors so that, based on the feedback, you maintain the desired position. The common way to do this is with PID control (although most of the time the “I” and “D” can be ignored), there are lots of threads on PID control and example code has been posted more than once.
When you press a button a new “target position” is sent the the controlling code, the arm is moved until the sensor feedback matches the target.
How successful PID control is depends on many factors beyond just the code. Every arm system has slightly different mechanical properties, are elastics being used, how many motors, the gearing, the friction etc. Have you finished the 4-bar you were building? Have you calculated it’s weight and the force on the motors due to that and the sacks being lifted? Are you able to send a small value to the motors and have the arm hold it’s position without tripping the motor or cortex PTCs? These are all things you probably need to do before jumping into the code and assuming it will work.
Edit: Adding some links for you
https://vexforum.com/t/preset-arm-heights-in-easyc-v4/20223/1 (talking EasyC but same idea)
http://vamfun.wordpress.com/2010/10/02/sample-auto_lift-robotc-program/
There’s also some good discussion on this topic in these threads:
https://vexforum.com/t/having-a-motor-hold-its-position/19855/1
Alright. We have a chain bar again (scratched the 4-Bar). It is just like the other arm we had built, but stronger/lighter. We still have to do some more testings and all that. We do have 2 potentiometers on each side of the lift to use.
https://vexforum.com/t/how-to-program-a-preset/21090/1
Thank you!
Alright, I have been working on the arm code. Here is what I have so far.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
// Arm Function/Commands Etc.
//
//
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const int Arm_High = 0, 1214;
const int Arm_Trough = 1552, 0;
const int Arm_Bottom = 3242, 251;
// Arm functions
void MoveArm(int speed, int time);
{
motor[LiftLeft] = speed;
motor[LiftRight] = speed;
wait1Msec(time);
motor[LiftLeft] = 0;
motor[LiftRight] = 0;
}
void ArmSpeed(int speed);
{
motor[LiftLeft] = speed;
motor[LiftRight] = speed;
}
void ArmPosition(int speed, int position)
//{
// int ActualPower;
// int Error = abs(Position - SensorValue[ArmPot]);
// // Determine if the arm needs to go up or down from its current position
// if (Position > SensorValue[ArmPot])
// {
// ActualPower = Power;
// if (Error < 100)
// ActualPower = (int)(ActualPower / 2);
// while (SensorValue[ArmPot] < Position)
// {
// ArmPosition = SensorValue[ArmPot];
// motor[LeftLiftHP] = ActualPower;
// motor[LeftLift] = ActualPower;
// motor[RightLiftHP] = ActualPower;
// motor[RightLift] = ActualPower;
// }
// }
// else
// {
// ActualPower = -1 * Power;
// if (Error < 200)
// ActualPower = (int)(ActualPower / 2);
// while ((SensorValue[ArmPot] > Position))// && (SensorValue[ArmAtTop] == 0))
// {
// ArmPosition = SensorValue[ArmPot];
// motor[LeftLiftHP] = ActualPower;
// motor[LeftLift] = ActualPower;
// motor[RightLiftHP] = ActualPower;
// motor[RightLift] = ActualPower;
// }
// }
// motor[LeftLiftHP] = 0;
// motor[LeftLift] = 0;
// motor[RightLiftHP] = 0;
// motor[RightLift] = 0;
//}I figured out the right amount of “clicks” the arms needs to be at to be at the high goal, trough, and low goal. We would like the arm to not go any farther once it reaches the “High Goal” position, and not go any farther down when it reached the “Floor” position.
The “MoveArmToPosition” function I copied from last years code (I didn’t write it, and I don’t really understand it). The other functions I just created.
I also get a TON of “x’s” when I compile it. I have our main file include the Lift_Functions.c file.
Here are the errors:
With only fragments of code it is very difficult to provide much advice, but here is something I can see that is pretty obvious and potentially widespread in your code which is leading to a lot of compiler errors. For example when you declare a function you put a semicolon at the end of the first line
void MoveArm(int speed, int time);
should be
void MoveArm(int speed, int time)
{
your function code ;
with each line ;
terminated with a semicolon;
}
the first set of errors indicate you are missing ‘;’ at the end of a line.
I can’t do much more without seeing the whole of your code. But I think given the errors I see there are some basic common mistakes in your coding syntax.
Cheers Kb
In addition, almost all of the ArmPosition function is commented out. The “//” comments out the rest of the line.
The essence of your ArmPosition function is as follows (in pseudocode)
if( the arm is too low)
while( the arm is too low )
{
if (the distance to move is small )
power = half motor power
else
power = full power
send power to motors
}
else
if( the arm is too high)
while( the arm is too high )
{
if (the distance to move is small )
power = (-1) * half motor power
else
power = (-1) * full power
send power to motors
}
This way can work, but the code (even when uncommented) is not going to work as is, for example, the “power” variable I don’t see declared anywhere. Anyway, I can not give much more feedback today as I will be at the school working with my own team. Perhaps tomorrow I can work with you and get you going.
Some other things I see
const int Arm_High = 0, 1214;
const int Arm_Trough = 1552, 0;
const int Arm_Bottom = 3242, 251;
What’s this ? you cannot initialize a variable with two numbers. Are these the pot values? Is the second number the right hand pot? If so that pot is going through 0 and will not work, it may even damage itself. You need to mount the pots so that their range is approximately 500 through 3500 (a smaller range is fine). Rotate that left hand pot to get it away from 0 (if I understand what you have here), currently it is 0 through 3242, rotate it a little so it is 300 through 3542 (or thereabouts).
Ok, got to go now.
Oh, didn’t know 
Sorry…
How do I figure out the right value if I have 2 pots?
Using both potentiometers is probably unnecessary. Only use one and send the speed that you calculate to both motors. If you find it necessary to use both (one side of the arm is at a very different height than the other), have two separate control loops, with the left potentiometer controlling the left side and vice-versa.
I’m going to post some code that demonstrates a very simplistic way of doing what you want. This is not the code we use on our robots and is not PID control, however, you can use it as a first step to greater things. The code allows using a joystick for manual control of the arm and three buttons to select between three presets. The code is a bit over commented so should be self explanatory but I will cover the main concepts here.
First some definitions, not all the “magic” numbers are pulled out but these are the most important.
#pragma config(Sensor, in1, , sensorPotentiometer)
#pragma config(Sensor, dgtl7, armPot, sensorQuadEncoder)
#pragma config(Motor, port1, , tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// Joystick for manual control
#define JOY_ARM vexJSRightV
#define JOY_THRESHOLD 10
// buttons for preset arm control
#define ARM_PST_BUTTON_1 Btn8U
#define ARM_PST_BUTTON_2 Btn8R
#define ARM_PST_BUTTON_3 Btn8D
// Preset positions and limits
#define ARM_LOWER_LIMIT 500
#define ARM_LOW_POS 600
#define ARM_MED_POS 1000
#define ARM_HIGH_POS 2000
#define ARM_UPPER_LIMIT 2500
#define ARM_HOLD_POWER 0I was testing with an encoder, you will need to set the “armPot” sensor to your potentiometer. The manual joystick axis is defined as well as the three preset buttons that correspond to the LOW, MED and HIGH presets. The ARM_HOLD_POWER could be set so that it sends a minimum power to the motor always.
We need a function to send the final control values to the motors. Add additional motors to this function as needed.
/*-----------------------------------------------------------------------------*/
/* Drive arm motors */
/*-----------------------------------------------------------------------------*/
void
DriveArmMotor( int value )
{
// drive all the arm motors here
motor port1 ] = value;
}
This is the code that implements a crude for of P control, the drive is proportional to the error between the current sensor value and a target value. This code allows different constants to be used to calculate drive for upwards and downwards movement.
/*-----------------------------------------------------------------------------*/
/* Create arm drive based on a requested position */
/*-----------------------------------------------------------------------------*/
int
ArmAutoDrive( int target_position, int arm_position )
{
int arm_drive;
// 100 is the tolerance of the acceptable error
// too small and the arm will oscillate
if( abs( target_position - arm_position ) < 100 )
{
// here is where you set a non zero value to hole the arm if necessary
// any larger than 20 and you are in danger of tripping PTCs
arm_drive = ARM_HOLD_POWER;
}
else
// Do we need to go up
if( target_position > arm_position )
{
// this is proportional control, Kp is 0.1 here
arm_drive = 0.1 * (float)(target_position - arm_position);
// limit to thr maximum speed you want
if(arm_drive > 127)
arm_drive = 127;
// Limit to the minimum speed that will move your arm
if(arm_drive < 32)
arm_drive = 32;
}
else
// Do we need to go down
if( target_position < arm_position )
{
// move downwards, use different Kp if necessary
arm_drive = -0.1 * (float)(arm_position - target_position);
// limit to maximum downward speed here
if(arm_drive < (-127))
arm_drive = (-127);
// Limit to the minimum speed that will move your arm
if(arm_drive > (-32))
arm_drive = (-32);
}
else
arm_drive = 0; // error
// return the calculated value for the arm drive
return arm_drive;
}Here is the task that controls everything. The preset buttons only work if the joystick is near the center. The presets are one button per preset, using two buttons to move to the next upper or lower is left as an exercise for you if that’s what you want. If the arm is moving to a preset using the joystick will cancel that operation.
/*-----------------------------------------------------------------------------*/
/* Driver control for arm */
/*-----------------------------------------------------------------------------*/
task DriverArmTask()
{
int arm_drive;
int arm_position;
static int arm_target = 0;
// Basic Arm control
while( true )
{
// Get arm joystick value
arm_drive = vexRT JOY_ARM ];
// Get arm potentiometer value
// down should be low values, up should be high values
// if it is inverted then use arm_position = 4095 - SensorValue( armPot );
arm_position = SensorValue( armPot );
// Ignore joystick near center
if( (arm_drive <= JOY_THRESHOLD) && (arm_drive >= -JOY_THRESHOLD))
{
if( vexRT ARM_PST_BUTTON_3 ] == 1 )
arm_target = ARM_LOW_POS;
if( vexRT ARM_PST_BUTTON_2 ] == 1 )
arm_target = ARM_MED_POS;
if( vexRT ARM_PST_BUTTON_1 ] == 1 )
arm_target = ARM_HIGH_POS;
// else no drive - send holding power for manual
arm_drive = ARM_HOLD_POWER;
}
else
{
// clear target position
arm_target = 0;
}
// if using a preset get arm_drive
if( arm_target != 0 )
arm_drive = ArmAutoDrive( arm_target, arm_position );
// send at least the holding power to the motor
if( (sgn(arm_drive) == 1) && (arm_drive < ARM_HOLD_POWER))
arm_drive = ARM_HOLD_POWER;
// limit drive so we stop at absolute top and bottom
if( ( arm_position > ARM_UPPER_LIMIT ) && (arm_drive > 0) )
arm_drive = 0;
if( ( arm_position < ARM_LOWER_LIMIT ) && (arm_drive < 0) )
arm_drive = 0;
// Drive motor
DriveArmMotor( arm_drive );
// don't hog CPU
wait1Msec( 25 );
}
}And finally a main task that starts the above.
task main()
{
StartTask( DriverArmTask );
while(1)
wait1Msec(25);
}
All this code is a very much simplified version of code used on last years gateway robot. The drive would be done in another task, same with intake or add it to this one. You will need to tweak the thresholds for the sensor detection to work with your robot, these numbers worked well for us but it was different.
Full program is attached.
ArmPresetCtl_1.c (5.17 KB)
Jesse,
Thank you so much for asking this question. we were just looking at the exact problem today. This helps us so much.
Thank you so much to everyone for their very detailed responses.
Here is some code we are using for the arm. It’s not letting us use it.
Can you point out some things that need changed?
I also still did not understand how we would use the code you posted @jpearman. Do we just stick our values in there?
Jesse
Let me get back to you with some code in the next couple of days, you still have lots of errors, did you write this code yourself or copy it from elsewhere? It is sort of along the right lines but there are some fundamental problems with it.
Thank you so much! I’m sorry for being sort of a pest.
Oh, I copied this from last years code.
Jesse, you are not being a pest, it would obviously be a lot easier to sit down together and work on your code but that’s not practical so I will try and first explain what is wrong with your existing code line by line.
// Arm functions
void MoveArm(int speed, int time);
{
motor[LiftLeft] = speed;
motor[LiftRight] = speed;
wait1Msec(time);
motor[LiftLeft] = 0;
motor[LiftRight] = 0;
}
void ArmSpeed(int speed);
{
motor[LiftLeft] = speed;
motor[LiftRight] = speed;
}So here you have two simple functions not related to moving the arm to a given position. The only syntactical error in these is the “;” following the function definition. This
void MoveArm(int speed, int time);
should be this
void MoveArm(int speed, int time)
This
void ArmSpeed(int speed);
should be this
void ArmSpeed(int speed)
Both functions should work as is. MoveArm will send a control value to the motors, wait a given amount of time (in 1/1000 second units) and then stop the motors. The second function simply sends the control value to the motors and then returns.
The third function you had is this
void ArmPosition(int speed, int position)
{
int ActualPower;
int Error = abs(Position - SensorValue[LeftPot]);
// Determine if the arm needs to go up or down from its current position
if (Position > SensorValue[LeftPot])
{
ActualPower = Power;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < Position)
{
ArmPosition = SensorValue[LeftPot];
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}
}
else
{
ActualPower = -1 * Power;
if (Error < 200)
ActualPower = (int)(ActualPower / 2);
while ((SensorValue[LeftPot] > Position))// && (SensorValue[ArmAtTop] == 0))
{
ArmPosition = SensorValue[LeftPot];
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}
}
motor[LiftLeft] = 0;
motor[LiftRight] = 0;
}The definition would imply that this should move the arm to a given position at a certain speed. Lets simplify this so as to see the basic structure.
void ArmPosition(int speed, int position)
{
int ActualPower;
int Error = abs(Position - SensorValue[LeftPot]);
// Determine if the arm needs to go up or down from its current position
if (Position > SensorValue[LeftPot])
{
// do something
}
else
{
// do something else
}
motor[LiftLeft] = 0;
motor[LiftRight] = 0;
}
You have a variable called ActualPower
You have a variable called Error which is set to the absolute difference between the parameter position and the value of a sensor called LeftPot.
You have an if-then-else statement that compares the parameter position with the value of a sensor called LeftPot and executes one or another block of code.
Finally you stop the motors by sending them a value of 0.
Well so far so good.
now lets dig into the first block after the if statement
ActualPower = Power;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < Position)
{
ArmPosition = SensorValue[LeftPot];
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}
You set ActualPower to the value of something called Power, however, Power is not defined anywhere in the code so this causes a compiler error.
You then test the value of the variable called Error and divide ActualPower by 2 if Error is less than 100.
Next you have a while loop, this compares the value of the LeftPot sensor to Position. It then moves the value of the LeftPot sensor into a variable called ArmPosition. ArmPosition is not defined as a variable, this can not be done as ArmPosition is the name of your function, this will not compile. Next you send ActualPower to each motor. Presumably the arm will move and the value of the LeftPot sensor increase until it is equal to or greater than Position, the while loop will then exit.
I think what you were trying to with this code is a follows.
ActualPower = speed;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < position)
{
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}The essence is to send a control value of speed to the motors until they have moved such that the sensor value is greater than the requested position.
The else statement has inverted logic to this code, send a negative control value until the sensor value has decreased below the requested position. Revised code that will compile is as follows.
void ArmPosition(int speed, int position)
{
int ActualPower;
int Error = abs(position - SensorValue[LeftPot]);
// Determine if the arm needs to go up or down from its current position
if (position > SensorValue[LeftPot])
{
ActualPower = speed;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < position)
{
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}
}
else
{
ActualPower = -1 * speed;
if (Error < 200)
ActualPower = (int)(ActualPower / 2);
while ((SensorValue[LeftPot] > position))
{
motor[LiftLeft] = ActualPower;
motor[LiftRight] = ActualPower;
}
}
motor[LiftLeft] = 0;
motor[LiftRight] = 0;
}As you have a function called ArmSpeed an improved version may be as follows.
void ArmPosition(int speed, int position)
{
int ActualPower;
int Error = abs(position - SensorValue[LeftPot]);
// Determine if the arm needs to go up or down from its current position
if (position > SensorValue[LeftPot])
{
ActualPower = speed;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < position)
{
ArmSpeed( ActualPower );
wait1Msec(10);
}
}
else
{
ActualPower = -1 * speed;
if (Error < 200)
ActualPower = (int)(ActualPower / 2);
while ((SensorValue[LeftPot] > position))
{
ArmSpeed( ActualPower );
wait1Msec(10);
}
}
ArmSpeed( 0 );
}
I also added a wait1Msec to the while loop, there is no need to run this loop quickly and hog the cpu. A final compiling version of your code would be as follows.
// Arm functions
void ArmSpeed(int speed)
{
motor[LiftLeft] = speed;
motor[LiftRight] = speed;
}
void MoveArm(int speed, int time)
{
ArmSpeed(speed);
wait1Msec(time);
ArmSpeed(0);
}
void ArmPosition(int speed, int position)
{
int ActualPower;
int Error = abs(position - SensorValue[LeftPot]);
// Determine if the arm needs to go up or down from its current position
if (position > SensorValue[LeftPot])
{
ActualPower = speed;
if (Error < 100)
ActualPower = (int)(ActualPower / 2);
while (SensorValue[LeftPot] < position)
{
ArmSpeed( ActualPower );
wait1Msec(10);
}
}
else
{
ActualPower = -1 * speed;
if (Error < 200)
ActualPower = (int)(ActualPower / 2);
while ((SensorValue[LeftPot] > position))
{
ArmSpeed( ActualPower );
wait1Msec(10);
}
}
ArmSpeed( 0 );
}It’s still not going to do what you want but that will be covered in part 2.
So you have a function called ArmPosition which if you give it a position and speed will move your arm motors until the arm potentiometer has passed that position. The function would be used as follows to send the arm to a preset.
task main()
{
while(1)
{
// Send to a position - these are blocking functions
if( vexRT Btn8U ] == 1 )
ArmPosition( 100, Arm_High );
else
if( vexRT Btn8R ] == 1 )
ArmPosition( 100, Arm_Trough );
else
if( vexRT Btn8D ] == 1 )
ArmPosition( 100, Arm_Bottom );
else
// manual on right joystick
ArmSpeed( vexRT Ch2 ] );
wait1Msec( 10 );
}
}This code calls ArmPosition with one of your three presets depending on which button is pressed. There are a number of possible problems and issues.
-
Your code assumes that positive motor commands will increase the sensor value. You may have this backwards as you pot values are smaller when lifting the arm. Easy fix would be to reverse the pot (ie. take it off the axle and put it on the other way). You will then need new preset positions.
-
Once you call the ArmPosition function it does not return until the arm position is reached. During that time you are not able to perform any other action in that task, we call this type of function a blocking function. If the drive control is part of this task then you not be able to drive. If the arm motors trip the PTC device and stall then you will not have any arm control. Once the arm is moving then you cannot change your mind about where it is going until the first action has been achieved.
Thank you SO much!
So, in autonomous, if I wanted the arm to go to the High Goal position, I would have to do something like this?
ArmPosition(127, Arm_High)
Then so on with the other functions? Also, our operator didn’t really want any presets (I am going to put them on just to let him see though).
Thank you again! Still learning!
That’s right. Remember I said that you may need to reverse the motor command or pot, the logic may all be backwards, it depends on exactly how your robot is setup.
Learning is good 
Play with the code, let us know what happens and then perhaps improve it to become a better solution.
Thank you! What exactly do you mean when you say “reverse”? Like make it negative?
Should I test then see?