So for the start of a new year I thought I would post some code for a simple (and I mean simple) proportional controller. The question of arm control seems to come up a lot so this can be a thread for posting different techniques of achieving that. This is what we are implementing.
A potentiometer connected to the arm measures position, this can vary between 0 and 4095 but will in practice be a smaller range, in the example code I use 400 to 3000 but you need to determine this by experimentation. A single motor is driving the arm as the example uses a clawbot as it’s basis. The assumption is that positive command values will make the arm raise and the potentiometer values increase, I did not check the example code on an actual clawbot, perhaps I will do this over the weekend. If potentiometer values decrease then remove the pot and attach it the other way around so they do.
Here is the example code in ROBOTC, this code uses one task to run the P controller, the constant Kp will need adjusting depending on the arm weight and the gear ratio. I made no attempt to tune it here, again, perhaps I will test on a clawbot this weekend.
#pragma config(Sensor, in1, armPot, sensorPotentiometer)
#pragma config(Motor, port1, motorA, tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// These could be constants but leaving
// as variables allows them to be modified in the debugger "live"
static float pid_Kp = 0.5;
static float pidRequestedValue;
/*-----------------------------------------------------------------------------*/
/* pid control task */
/*-----------------------------------------------------------------------------*/
task pidController()
{
float pidSensorCurrentValue;
float pidError;
float pidDrive;
while( true )
{
// Read the sensor value and scale
pidSensorCurrentValue = SensorValue armPot ];
// calculate error
pidError = pidRequestedValue - pidSensorCurrentValue;
// calculate drive
pidDrive = (pid_Kp * pidError);
// limit drive
if( pidDrive > 127 )
pidDrive = 127;
if( pidDrive < (-127) )
pidDrive = (-127);
// send to motor
motor motorA ] = pidDrive;
// Don't hog cpu
wait1Msec( 25 );
}
}
/*-----------------------------------------------------------------------------*/
/* main task */
/*-----------------------------------------------------------------------------*/
task main()
{
// set initial position as the value of the pot
pidRequestedValue = SensorValue armPot ];
// start the PID task
StartTask( pidController );
// use joystick to modify the requested position
while( true )
{
// presets - assume joystick is centered
if( vexRT Btn8U ] == 1 )
pidRequestedValue = 1800;
else
if( vexRT Btn8D ] == 1 )
pidRequestedValue = 800;
// manual control
if( abs(vexRT Ch2 ]) > 10 )
{
pidRequestedValue = pidRequestedValue + (vexRT Ch2 ]/2);
// crude limiting to upper and lower values
if( pidRequestedValue > 3000 )
pidRequestedValue = 3000;
if( pidRequestedValue < 400 )
pidRequestedValue = 400;
}
// don't hog cpu
wait1Msec(50);
}
}
The same code in EasyC, I did not use a repeating timer for this example, this is the most basic code with everything in one loop.
#include "Main.h"
void OperatorControl ( unsigned long ulTime )
{
float pidRequestedValue;
float pid_Kp = 0.5;
float pidSensorCurrentValue;
float pidError;
float pidDrive;
pidRequestedValue = GetAnalogInputHR ( 1 ) ;
while ( 1 ) // Insert Your RC Code Below
{
pidSensorCurrentValue = GetAnalogInputHR ( 1 ) ;
pidError = pidRequestedValue - pidSensorCurrentValue ;
pidDrive = pidError * pid_Kp ;
if ( pidDrive > 127 )
{
pidDrive = 127 ;
}
if ( pidDrive < -127 )
{
pidDrive = -127 ;
}
SetMotor ( 1 , (int)pidDrive ) ;
// User input
if ( GetJoystickDigital( 1, 8, 2 ) )
{
pidRequestedValue = 1800 ;
}
else if ( GetJoystickDigital( 1, 8,1 ) )
{
pidRequestedValue = 800 ;
}
if ( ( GetJoystickAnalog( 1, 2 ) > 10) || (GetJoystickAnalog( 1, 2 ) < -10) )
{
pidRequestedValue = pidRequestedValue + (GetJoystickAnalog( 1, 2 ) / 2) ;
if ( pidRequestedValue > 3000 )
{
pidRequestedValue = 3000 ;
}
if ( pidRequestedValue < 400 )
{
pidRequestedValue = 400 ;
}
}
Wait ( 25 ) ;
//PrintToScreen ( "drive %d %d %d\n" , (int)pidRequestedValue, (int)pidSensorCurrentValue, (int)pidDrive ) ;
}
}
Joystick channel 2 is used for manual control.
Buttons 8 up and down are used for two presets.
Code is attached to save you typing it in.
enjoy.
p_controller.zip (5.35 KB)