No problem.
That’s what I linked for you originally.
If all you want is hold for the auto arm stuff then you could simply send a non zero hold value.
<<snip>>
if( !autoArmControl )
{
// stop motor
SetMotor ( 8 , motor[8] = 10 ) ;
SetMotor ( 9 , motor[9] = 10 ) ;
}
// Read potentiometer on arm
armPot = GetAnalogInput ( 1 ) ;
// Control arm automatically
if( autoArmControl )
{
// Automatic control
if( Abs( desiredArmPosition - armPot ) < 10 )
{
// done
autoArmControl = 0;
SetMotor ( 8 , motor[8] = 10 ) ;
SetMotor ( 9 , motor[9] = 10 ) ;
}
else
<<end snip>>
(get rid of all the debugging motor = YY code if you want, it’s not needed).
Don’t go above perhaps 20 or you will have motor overheat issues.
Well if you really want PI control then try this. It only implements for the button presents and not for manual.
I did not compiled this and have not tested either, may have bugs, but it gives you an idea of PI control.
void MyUserControl()
{
int joyCh1, joyCh2, joyCh3, joyCh4;
int but8U, but8D, but8R, but8L;
int butPressed;
int buttonIsPressed = 0;
int autoArmControl = 0;
int desiredArmPosition = 0;
int armPot;
int motor[10]; // for debugging
int error = 0;
int drive = 0;
int integral = 0;
float Kp, Ki;
// Init LCD
InitLCD ( 1 ) ;
SetLCDLight ( 1 , 1 ) ;
// Do forever
while(1)
{
// simple arcade drive
joyCh3 = GetJoystickAnalog( 1 , 3 ) ;
joyCh4 = GetJoystickAnalog( 1 , 4 ) ;
joyCh1 = GetJoystickAnalog( 1 , 1 ) ;
SetMotor ( 1 , (joyCh3 + joyCh4) / 2 ) ;
SetMotor ( 10 , (joyCh3 - joyCh4) / 2 ) ;
Arcade4 ( 1 , 3 , 4 , 2 , 1 , 4 , 3 , 1 , 1 , 0 , 0 ) ; // Primary 4 Wheel Drive Controls
JoystickToMotor ( 1 , 1 , 5 , 1 ) ; //Strafing Controls for Motor 5
JoystickDigitalToMotor (1 , 5 , 1 , 127 , 2 , -127 , 6 ) ; //Induction Roller Controls - Fill
JoystickDigitalToMotor (1 , 5 , 1 , -127 , 2 , 127 , 7 ) ; //Induction Roller Controls - Release
// find out which buttons are down
but8D = GetJoystickDigital ( 1 , 8 , 1 ) ;
but8U = GetJoystickDigital ( 1 , 8 , 2 ) ;
but8L = GetJoystickDigital ( 1 , 8 , 3 ) ;
but8R = GetJoystickDigital ( 1 , 8 , 4 ) ;
// combine into a single variable
butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;
// Decide what to do
switch( butPressed )
{
case 0:
// No buttons
buttonIsPressed = 0;
break;
case 1:
// Right button
if( !buttonIsPressed )
{
buttonIsPressed = 1;
desiredArmPosition = ARM_MID_POS;
autoArmControl = 1;
}
break;
case 2:
// Left button
break;
case 4:
// Up button
if( !buttonIsPressed )
{
buttonIsPressed = 1;
desiredArmPosition = ARM_HIGH_POS;
autoArmControl = 1;
}
break;
case 8:
// Down button
if( !buttonIsPressed )
{
buttonIsPressed = 1;
desiredArmPosition = ARM_LOW_POS;
autoArmControl = 1;
}
break;
default:
// All other cases
break;
}
// See if we are overiding any buttons
joyCh2 = GetJoystickAnalog( 1 , 2 ) ;
// don't use joystick near 0
if( Abs( joyCh2 ) > 10 )
{
// no automatic control
autoArmControl = 0;
// manual control of motor
SetMotor ( 8 , motor[8] = joyCh2 ) ;
SetMotor ( 9 , motor[9] = joyCh2 ) ;
// for debugging
desiredArmPosition = 9999;
}
else
if( !autoArmControl )
{
// stop motor
SetMotor ( 8 , motor[8] = 0 ) ;
SetMotor ( 9 , motor[9] = 0 ) ;
}
// Read potentiometer on arm
armPot = GetAnalogInput ( 1 ) ;
// Control arm automatically
if( autoArmControl )
{
// error is positive if desiredArmPosition > armPot
error = desiredArmPosition - armPot;
// calculate integral and limit to avoid windup
integral = integral + error;
if( integral > 100 )
integral = 100;
if( integral < (-100) )
integral = -100;
// I put these here for clarity
// edit as needed
Kp = 1.0;
Ki = 0.1;
// calculate motor drive, PI control
drive = (error * Kp) + (integral * Ki);
// limit out of habit
if(drive > 127)
drive = 127;
if(drive < (-127))
drive = (-127);
// but the pot is installed backwards so we invert
drive = -drive;
SetMotor ( 8 , motor[8] = drive ) ;
SetMotor ( 9 , motor[9] = drive ) ;
// autoArmControl will never be cleared in this version
}
// display arm pot,requested position and motor speed
SetLCDText ( 1 , 1 , "pot %d mot %d", armPot, motor[8] ) ;
SetLCDText ( 1 , 2 , "req %d", desiredArmPosition ) ;
// wait
Wait ( 25 ) ;
}
}