Lift Leveling Program

  1. 3 years ago

    Jarred

    25 Jan 2015 Greenfield, Indiana 1115A

    Hello. Our robot is a 4 motor 1:7 RD4B. We have stabilization issues. We have two potentiometers on the lift. One on each side.

    We are attempting to make it so we press a different button for different levels of skyrise. So we press 8U for example and it will bring the lift all the way down.

    However, our code (posted below) is not doing this correctly. We press the button and the lift stutters slowly toward the desired position. Even once the lift gets to where it belongs, it still continues to twitch.

    So, could anyone look at our code and find our issues? We are at our wit's end and we have a competition this saturday (January 31st). Thank you!

    (Our code will be in the next post)

  2. Jarred

    25 Jan 2015 Greenfield, Indiana 1115A

    (Our code is too long so we removed our drive code from the part below to fit within the forum post rules)

    #pragma config(Sensor, in1,    potleft,        sensorPotentiometer)
    #pragma config(Sensor, in2,    potright,       sensorPotentiometer)
    #pragma config(Sensor, dgtl1,  sonarSensor1,   sensorSONAR_cm)
    #pragma config(Sensor, dgtl3,  sonarSensor2,   sensorSONAR_cm)
    #pragma config(Sensor, dgtl5,  sonarSensor3,   sensorSONAR_cm)
    #pragma config(Sensor, dgtl7,  liftLimitSwitch, sensorTouch)
    #pragma config(Sensor, dgtl8,  pickerupper,    sensorDigitalOut)
    #pragma config(Sensor, dgtl10, ,               sensorTouch)
    #pragma config(Motor,  port1,           frontleft,     tmotorVex393HighSpeed_HBridge, openLoop, encoderPort, None)
    #pragma config(Motor,  port2,           frontright,    tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, None)
    #pragma config(Motor,  port3,           backleft,      tmotorVex393HighSpeed_MC29, openLoop)
    #pragma config(Motor,  port4,           backright,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
    #pragma config(Motor,  port5,           liftleftup,    tmotorVex393_MC29, openLoop)
    #pragma config(Motor,  port6,           liftrightup,   tmotorVex393_MC29, openLoop, reversed)
    #pragma config(Motor,  port7,           liftleftbottom, tmotorVex393_MC29, openLoop, reversed)
    #pragma config(Motor,  port8,           liftrightbottom, tmotorVex393_MC29, openLoop)
    #pragma config(Motor,  port9,           cube,          tmotorVex393_MC29, openLoop)
    //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
    
    #pragma platform(VEX)
    
    //Competition Control and Duration Settings
    #pragma competitionControl(Competition)
    #pragma autonomousDuration(20)
    #pragma userControlDuration(120)
    
    #include "Vex_Competition_Includes.c"
    
    void pre_auton()
    {
      bStopTasksBetweenModes = true;
    }
    
    //Basic Single Drive
    
    task autonomous()
    {
    
    //None
    
    }
    
    task usercontrol()
    {
    	
    //Remote Control Commands
    
    			if(vexRT[Btn6U] == 1 && vexRT[Btn6D] != 1) //If button 5U is pressed...
    		{
    			motor[cube] = 127;
    		}
    		else if(vexRT[Btn6D] == 1 && vexRT[Btn6U] != 1) //Else, if button 5D is pressed...
    		{
    			motor[cube] = -127;
    		}
    		else //Else (neither button is pressed)...
    		{
    			motor[cube] = 0;
    		}
    
    		if(vexRT[Btn5U] == 1 && vexRT[Btn5D] != 1)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn5D] == 1 && vexRT[Btn5U] != 1)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightup] = -127;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightup] = 0;
    		}
    
    /*
    		if(vexRT[Btn8U] == 1)         // If button 6U (upper right shoulder button) is pressed:
        {
          SensorValue[pickerupper] = 1;  // ...activate the solenoid.
      	}
        else                          // If button 6U (upper right shoulder button) is  NOT pressed:
        {
          SensorValue[pickerupper] = 0;  // ..deactivate the solenoid.
        }
     */
    
    
        if(vexRT[Btn8U] == 1 && SensorValue[potleft] < 4080 && SensorValue[potright] < 4080)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn8U] == 1 && SensorValue[potleft] > 4080 && SensorValue[potright] > 4080)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//8Right - Skyrise Level 1
    
    
    
    		if(vexRT[Btn8R] == 1 && SensorValue[potleft] < 3918 && SensorValue[potright] < 4077)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potright] > 4077)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potleft] < 3938 && SensorValue[potright] > 4077 && SensorValue[potright] < 4097)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//8Down - Skyrise Level 2
    
    
    
    		if(vexRT[Btn8D] == 1 && SensorValue[potleft] < 3630 && SensorValue[potright] < 3769)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn8D] == 1 && SensorValue[potleft] > 3630 && SensorValue[potright] > 3769)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn8D] == 1 && SensorValue[potleft] > 3630 && SensorValue[potleft] < 3650 && SensorValue[potright] > 3769 && SensorValue[potright] < 3689)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//8Left - Skyrise Level 3
    
    
    
    		if(vexRT[Btn8L] == 1 && SensorValue[potleft] < 3335 && SensorValue[potright] < 3447)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn8L] == 1 && SensorValue[potleft] > 3335 && SensorValue[potright] > 3447)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn8L] == 1 && SensorValue[potleft] > 3335 && SensorValue[potleft] < 3355 && SensorValue[potright] > 3447 && SensorValue[potright] < 3467)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//7Up - Skyrise Level 4
    
    
    
    		if(vexRT[Btn7U] == 1 && SensorValue[potleft] < 3056 && SensorValue[potright] < 3199)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn7U] == 1 && SensorValue[potleft] > 3056 && SensorValue[potright] > 3199)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn7U] == 1 && SensorValue[potleft] > 3056 && SensorValue[potleft] < 3076 && SensorValue[potright] > 3199 && SensorValue[potright] < 3219)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//7Right - Skyrise Level 5
    
    
    
    		if(vexRT[Btn7R] == 1 && SensorValue[potleft] < 2704 && SensorValue[potright] < 2846)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn7R] == 1 && SensorValue[potleft] > 2704 && SensorValue[potright] > 2846)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn7R] == 1 && SensorValue[potleft] > 2704 && SensorValue[potleft] < 2724 && SensorValue[potright] > 2846 && SensorValue[potright] < 2866)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//7Down - Skyrise Level 6
    
    
    
    		if(vexRT[Btn7D] == 1 && SensorValue[potleft] < 1280  && SensorValue[potright] < 1160)
    		{
    			motor[liftleftbottom] = -127;
    			motor[liftleftup] = -127;
    			motor[liftrightbottom] = -127;
    			motor[liftrightup] = -127;
    		}
    		else if(vexRT[Btn7D] == 1 && SensorValue[potleft] > 1280 && SensorValue[potright] > 1160)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn7D] == 1 && SensorValue[potleft] > 1280 && SensorValue[potleft] < 1290 && SensorValue[potright] > 1260 && SensorValue[potright] < 1170)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    
    
    
    		//7Left - Skyrise Level 7
    
    
    
    		if(vexRT[Btn7L] == 1 && SensorValue[potleft] > 2270 && SensorValue[potright] > 2410)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn7L] == 1 && SensorValue[potleft] < 2270 && SensorValue[potright] < 2410)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    		else
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    	}
    
    
    }
  3. @Jarred ... Even once the lift gets to where it belongs, it still continues to twitch. ...

    Is it possible that a deadband would help with this twitch? It's very unlikely the pot value will be exactly where you want it, so does your code keep bouncing back and forth in an attempt to get the machine precisely where you want it to be? but continuously overshoots the target point?

    Also, did you get 6U and 8U mixed up here?

    if(vexRT[Btn8U] == 1)         // If button 6U (upper right shoulder button) is pressed:
        {
          SensorValue[pickerupper] = 1;  // ...activate the solenoid.
  4. Bot15498

    25 Jan 2015 Hawaii 2932

    I've taken a snipet of your code:

    //8Right - Skyrise Level 1
    ...
    		else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potright] > 4077)
    		{
    			motor[liftleftbottom] = 127;
    			motor[liftleftup] = 127;
    			motor[liftrightbottom] = 127;
    			motor[liftrightup] = 127;
    		}
    		else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potleft] < 3938 && SensorValue[potright] > 4077 && SensorValue[potright] < 4097)
    		{
    			motor[liftleftbottom] = 0;
    			motor[liftleftup] = 0;
    			motor[liftrightbottom] = 0;
    			motor[liftrightup] = 0;
    		}
    ...

    In this case, say the left potentiometer reading is 3920 and the right potentiometer reading is 4080. Both parts of this if-else statement would be true, setting the motors to both 127 and 0. Since the power value being sent to the motor is constantly changing between the max value and 0, the motor will appear to be twitching.
    Also, once the lift had reached the desired position, the motors would be constantly switching between power values of 127 and -127, causing it to move back and forth quickly. This would probably overheat the motors to.

    You should look into implementing a pid controller with only the proportional part (finding the differences between the desired value and the actual value). This way, a lower value would be sent to the motors to keep them up once the lift was at a desired position. Perhaps you could have a button that toggles the pid controller on and off to make sure constant power isn't being sent to motors.

  5. @Bot15498 I've taken a snipet of your code:

    In this case, say the left potentiometer reading is 3920 and the right potentiometer reading is 4080. Both parts of this if-else statement would be true, setting the motors to both 127 and 0. Since the power value being sent to the motor is constantly changing between the max value and 0, the motor will appear to be twitching.

    If you use if, else if, else, I believe only one of the statements will run; not all of the statements that happen to be true.

    Also, once the lift had reached the desired position, the motors would be constantly switching between power values of 127 and -127, causing it to move back and forth quickly. This would probably overheat the motors to.

    This is more likely the problem. Like FullMetalMentor said, you need a range of acceptable heights for each setting (or some kind of proportional control), and you may need to set the motors to 10 or so in order to keep the lift up.

  6. Jarred

    25 Jan 2015 Greenfield, Indiana 1115A

    We have never worked with PID and we are kind of slow to learn programming. How exactly would we go about implementing the P potion of PID? I understand it has something to do with finding the Error. difference between the pot values but other than that i'm lost.

    Thank you for the quick replies btw.

  7. @Jarred We have never worked with PID and we are kind of slow to learn programming. How exactly would we go about implementing the P potion of PID? I understand it has something to do with finding the Error. difference between the pot values but other than that i'm lost.

    Thank you for the quick replies btw.

    It's not too complicated. You calculate the error, multiply the error by some constant, then set the motors to that product. The constant is an arbitrary number that you find (usually by trail and error) that makes the motors do what you want with different errors.

    Do you want to just balance the sides, or to get each side to a certain height? The latter is what I think you want, in which case the error=certainheight-pot

  8. jpearman

    25 Jan 2015 Moderator, ROBOTC Tech Support, V5 Beta Moderator Rockstar, Los Angeles 8888

    You have actually sort of implemented a P controller which has the proportional constant set very high so that motor drive values are full forward, full backwards or off.

    I will send some notes on the code later.

  9. @Jarred How exactly would we go about implementing the P potion of PID? I understand it has something to do with finding the Error. difference between the pot values but other than that i'm lost.

    You're right, except the error is the potentiometer reading for your desired height minus the current reading. Some pseudocode for example:

    Contained in an infinite loop...
        if button8U
            targetValue = 4077
    
        error = targetValue - potentiometer
    
        motorPower = error * 0.5
    
        motor[X] = motorPower

    This way, as the current potentiometer reading approaches 4077, the motor power decreases proportionally. This assumes one arm with one potentiometer and that positive motor values = up. Keeping two arms in sync is another level of complexity.

  10. Jarred

    26 Jan 2015 Greenfield, Indiana 1115A

    Assuming we get the P portion of the PID loop figured out and working great, how would we go about keeping the arms in sync?

  11. jpearman

    26 Jan 2015 Moderator, ROBOTC Tech Support, V5 Beta Moderator Rockstar, Los Angeles 8888

    So lets look at the code and work on a few things before jumping into more advanced topics like PID.

    First, lets get the stuttering issue out of the way. You have the classic error in this code of setting the motors all over the place, you have manual control on buttons 5U and 5D that when not pressed set the motors to 0. You also have multiple if-then-else statements that in the final else set the motors to 0. When pressing any of the preset buttons you will start moving the motors but all the other conditional statements will cause the motors to be set to 0, hence the stuttering.

    The code as written will also not stabilize the lift as it's moving and probably not when it stops. You control all four motors together, to get left and right sides moving independantly you would need to decouple control of left and right sides.

    Before changing the code to solve the stabilization issue it needs some serious cleanup. When I see this type of code written by my students I shout at them FUNCTIONS !! You need to use functions otherwise the code gets too complicated and the structure is lost. First candidate is sending control values to the lift motors as you have the same four lines all over the code.

        motor[liftleftbottom] = -127;
        motor[liftleftup] = -127;
        motor[liftrightbottom] = -127;
        motor[liftrightup] = -127;

    The value changes but the functionality does not, so put this in a function.

    // Send a control value to all four lift motors
    void
    LiftMotors( int speed )
    {
        motor[liftleftbottom]  = speed;
        motor[liftrightbottom] = speed;
        motor[liftleftup]      = speed;
        motor[liftrightup]     = speed;    
    }

    Next, the code that sends the lift to a preset position (rewritten using the LiftMotors function )

        if(vexRT[Btn8R] == 1 && SensorValue[potleft] < 3918 && SensorValue[potright] < 4077)
            {
            LiftMotors( -127 );
            }
        else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potright] > 4077)
            {
            LiftMotors( 127 );
            }
        else if(vexRT[Btn8R] == 1 && SensorValue[potleft] > 3918 && SensorValue[potleft] < 3938 && SensorValue[potright] > 4077 && SensorValue[potright] < 4097)
            {
            LiftMotors( 0 );
            }
        else
            {
            LiftMotors( 0 );
            }

    This is also used multiple times only changing the "magic numbers" for the potentiometer positions. You have a couple of special cases at each end that we can handle differently but the code more or less duplicates itself 7 times.

    Lets create a function for that.

    void
    LiftToPosition( int targetLeft, int targetRight, int positionDeadband )
    {
        int liftSpeed;
    
        if(SensorValue[potleft]  > targetLeft  && SensorValue[potleft]  < (targetLeft  + positionDeadband) &&
           SensorValue[potright1] > targetRight && SensorValue[potright] < (targetRight + positionDeadband))
            liftSpeed = 0;
        else
        if(SensorValue[potleft] < targetLeft && SensorValue[potright] < targetRight)
            liftSpeed = -127;
        else
        if(SensorValue[potleft] > targetLeft && SensorValue[potright] > targetRight)
            liftSpeed = 127;
    
        // Limit upper end
        if( SensorValue[potleft] > 4080 && SensorValue[potright] > 4080 && liftSpeed > 0 )
            liftSpeed = 0;
        // Limit lower end
        if( SensorValue[potleft] < 2270 && SensorValue[potright] < 2410 && liftSpeed < 0 )
            liftSpeed = 0;
    
        LiftMotors( liftSpeed );
    }

    I should say here that I didn't test this code yet, don't have a system setup with two pots, perhaps later.

    In the original code you compare each side to a target position and then to a position 20 values greater (in most cases). I made this a parameter called the positionDeadband.

    So now when we rewrite the main driver control code, use the functions and make sure only one part can set the motors we would have something looking like this.

    task usercontrol()
    {
        while(true)
            {
            //Remote Control Commands
        
        
            if(vexRT[Btn6U] == 1 && vexRT[Btn6D] != 1) //If button 5U is pressed...
                motor[cube] = 127;
            else if(vexRT[Btn6D] == 1 && vexRT[Btn6U] != 1) //Else, if button 6D is pressed...
                motor[cube] = -127;
            else //Else (neither button is pressed)...
                motor[cube] = 0;
        
            // Manual lift control
            if(vexRT[Btn5U] == 1 && vexRT[Btn5D] != 1)
                LiftMotors( 127 );
            else
            if(vexRT[Btn5D] == 1 && vexRT[Btn5U] != 1)
                LiftMotors( -127 );
        
            // Skyrise preset
            // --------------
            else
            if(vexRT[Btn8U] == 1)
                LiftToPosition( 4080, 4080, 0 );
        
            //8Right - Skyrise Level 1
            else
            if(vexRT[Btn8R] == 1)
                LiftToPosition( 3918, 4077, 20 );
        
            //8Down - Skyrise Level 2
            else
            if(vexRT[Btn8D] == 1)
                LiftToPosition( 3630, 3769, 20 );
        
            //8Left - Skyrise Level 3
            else
            if(vexRT[Btn8L] == 1)
                LiftToPosition( 3335, 3447, 20 );
        
            //7Up - Skyrise Level 4
            else
            if(vexRT[Btn7U] == 1)
                LiftToPosition( 3056, 3199, 20 );
        
            //7Right - Skyrise Level 5
            else
            if(vexRT[Btn7R] == 1)
                LiftToPosition( 2704, 2846, 20 );
        
            //7Down - Skyrise Level 6
            else
            if(vexRT[Btn7D] == 1)
                LiftToPosition( 1280, 1160, 10 );
        
            //7Left - Skyrise Level 7
            else
            if(vexRT[Btn7L] == 1)
                LiftToPosition( 2270, 2410, 0 );
            // final else - no buttons pressed
            else
                LiftMotors( 0 );
            }
    }

    Nice, clean, compact code :)

    A few other comments.

    You are using almost all the range of the pot (0-4095), it doesn't work well over the full range, if you can reduce the range to something like 500 to 3500 it will be better.

    Skyrise positions 6&7 seem odd, so does the upper limit, these are the positions I see for the left pot in the code, any chance the pot exceeded it's range?

    0    4080
    1    3918
    2    3630
    3    3335
    4    3056
    5    2704
    6    1280  ???
    7    2270  ??? these seem odd

    Anyway, this code will be better but can be improved some more. That will be part 2 in another post.

  12. jpearman

    27 Jan 2015 Moderator, ROBOTC Tech Support, V5 Beta Moderator Rockstar, Los Angeles 8888

    Part2.

    I'm going to develop the code a little more, I will say upfront that the code I'm going to post still has problems, there's a lot involved in controlling the two sides and you really need to start considering velocity at some point, however, this will at least get you all thinking.

    First off, in the original code it looks like two of the preset positions may have been reversed, it didn't really make much sense to me yesterday and that's the only explanation I have.

    Secondly, the difference between the two potentiometers was fairly constant at about 140. The best way to deal with this small offset is to adjust one of them so when level the potentiometers are reading more or less the same. I assume the difference should be zero in the code below.

    Finally, you really need to get them away from the ends of the range. Try and adjust so they read no lower than about 500 and no higher than 3500. This, of course, depends very much on the mechanical design but that should have been taken into account when deciding to use pots. The code below assumes they are in this range.

    To start we change the code sending speeds to the lift motors so that left and right can be controlled independently. I also moved the hard limits into this function so that even using manual control you will not exceed the limits of the pot.

    #define LIFT_HARD_LIMIT_LEFT_P      3500
    #define LIFT_HARD_LIMIT_LEFT_N      1000
    #define LIFT_HARD_LIMIT_RIGHT_P     3500
    #define LIFT_HARD_LIMIT_RIGHT_N     1000
    
    // Send a control value to all four lift motors
    void
    LiftMotors( int speedleft, int speedright )
    {
        // Hard Limit left and right independantly
        // Limit upper end
        if( SensorValue[potleft] > LIFT_HARD_LIMIT_LEFT_P && speedleft < 0 )
            speedleft = 0;
        // Limit lower end
        if( SensorValue[potleft] < LIFT_HARD_LIMIT_LEFT_N && speedleft > 0 )
            speedleft = 0;
    
        // Limit upper end
        if( SensorValue[potright] > LIFT_HARD_LIMIT_RIGHT_P && speedright < 0 )
            speedright = 0;
        // Limit lower end
        if( SensorValue[potright] < LIFT_HARD_LIMIT_RIGHT_N && speedright > 0 )
            speedright = 0;
    
        // Now send to motors
        motor[liftleftbottom]  = speedleft;
        motor[liftleftup]      = speedleft;
    
        motor[liftrightbottom] = speedright;
        motor[liftrightup]     = speedright;
    }

    I'm now using 3500 and 1000 as the absolute limits for the lift.

    The general concept for the control is going to be using the left side pot as a target for position. The difference between right and left pots is calculated and either right or left motor slowed to keep that difference low. The logic gets a little tricky as motor slowing swaps depending on whether we are going up or down. You also have to consider the case where the left motor is on target and it's control value therefore set to 0, we handle this as a special case (and this is the area that fails if near it's target but stalled).

    // Send the lift to a preset position
    void
    LiftToPosition( int targetLeft, int positionDeadband )
    {
        int     liftSpeed;
        int     liftSpeedleft;
        int     liftSpeedright;
        int     error;
        int     leftrighterror;
    
        error = targetLeft - SensorValue[potleft];
    
        // Figure out which way to g0
        if( abs(error) < positionDeadband )
            liftSpeed = 0;
        else
        if( error > 100 )
            liftSpeed = -100;
        else
        if( error > 50 )
            liftSpeed = -30;
        else
        if( error < -100 )
            liftSpeed =  100; //
        else
        if( error < -50 )
            liftSpeed =  30; //
    
        // Figure out the error between left and right
        leftrighterror = SensorValue[ potleft ] - SensorValue[ potright ];
    
        // going up or down
        // if down then reverse error
        if( liftSpeed > 0 )
            leftrighterror = -leftrighterror;
    
        // Now try and correct the errpr
        // If we have stopped then only control the right motor.
        // else slow either left or right
        if( leftrighterror > 30 )
            {
            if(liftSpeed == 0)
                {
                liftSpeedleft   = liftSpeed;
                liftSpeedright  = -20;
                }
            else 
                {
                liftSpeedleft  = sgn(liftSpeed) * (abs(liftSpeed) * 8 / 10); // slow left
                liftSpeedright = liftSpeed;
                }
            }
        else
        if( leftrighterror < -30 )
            {
            liftSpeedleft  = liftSpeed;
            if(liftSpeed == 0)
                liftSpeedright  = 20;
            else
                liftSpeedright = sgn(liftSpeed) * (abs(liftSpeed) * 8 / 10); // slow right;
            }
        else
            {
            liftSpeedleft  = liftSpeed;
            liftSpeedright = liftSpeed;
            }
    
        // Finally send values to the motors
        LiftMotors( liftSpeedleft, liftSpeedright );
    
        // for debugging
        writeDebugStreamLine("%4d %4d %4d %4d %4d", error, leftrighterror, liftSpeed, liftSpeedleft, liftSpeedright );
    }

    There's no PID control here, all we do is use a lower control value when the motors are nearing the target position. The code is also full of "magic numbers", that is, the threshold values and speeds are just best guesses and would need tuning depending on the specific lift gearing and construction.

    The main control loop looks very similar to before, we only have one target value now. I made all the target values simple and also removed the intake control. Remember, this code is to explain the concept and will probably not work "as is" on anyones robot.

    task usercontrol()
    {
         while(true)
            {
            //Remote Control Commands
            // Manual lift control
            if(vexRT[Btn5U] == 1 && vexRT[Btn5D] != 1)
                LiftMotors( 127, 127 );
            else
            if(vexRT[Btn5D] == 1 && vexRT[Btn5U] != 1)
                LiftMotors( -127, -127 );
    
            // Skyrise preset
            // --------------
            else
            if(vexRT[Btn8U] == 1)
                LiftToPosition( 3500, 25 );
    
            //8Right - Skyrise Level 1
            else
            if(vexRT[Btn8R] == 1)
                LiftToPosition( 3300, 25 );
    
            //8Down - Skyrise Level 2
            else
            if(vexRT[Btn8D] == 1)
                LiftToPosition( 3000, 25 );
    
            //8Left - Skyrise Level 3
            else
            if(vexRT[Btn8L] == 1)
                LiftToPosition( 2600, 25 );
    
            //7Up - Skyrise Level 4
            else
            if(vexRT[Btn7U] == 1)
                LiftToPosition( 2200, 25 );
    
            //7Right - Skyrise Level 5
            else
            if(vexRT[Btn7R] == 1)
                LiftToPosition( 1800, 25 );
    
            //7Down - Skyrise Level 6
            else
            if(vexRT[Btn7D] == 1)
                LiftToPosition( 1400, 25 );
    
            //7Left - Skyrise Level 7
            else
            if(vexRT[Btn7L] == 1)
                LiftToPosition( 1000, 25 );
            // final else - no buttons pressed
            else
                LiftMotors( 0, 0 );
        
            wait1Msec(25);
            }
    }

    In a few days I will try and post some code that goes way beyond this and uses PID and motion profiles.

    Edit:
    A few more notes.

    The original code the OP posted had positive control values causing the pot to decrease. I left this alone in the code above but usually I would really try and arrange for the opposite. It is much easier if you can achieve the following.

    Send the motor positive control values, cause the lift to go up and potentiometer/encoder counts to increase.

    If the motors are not doing this then reverse in the motors&sensors setup.

    If the quad encoder counts down then swap the two wires (ie. swap the digital ports). (An IME will always count up when using positive control values and the nMotorEncoder function)

    If the pot is backwards and you cannot mount from the other side then use a small glue function, something like this

    int
    GetLiftPot()
    {
        return( 4095 - SensorValue[ pot ] );
    }
  13. Team80_Giraffes

    27 Jan 2015 Event Partner Downingtown, PA 80[A-Z]$|81[A-Z]$|90[A-Z]$|91[...

    This code assumes the pot values are exactly equal when level. If one pot is twisted a smidge, the level position values would be different and you are going to compensate for it.

    // Figure out the error between left and right
        leftrighterror = SensorValue[ potleft ] - SensorValue[ potright ];

    So you need to have a start value of each and see the difference in those values tuned to each other. Also make sure the pots are reading exactly the same and the variable resistance is exactly the same as you go up and the range is the exact same until the top.

    I need to think about this a bit more to get some good code examples. Adjust your sensor values so the scale of left and right is compensating against each other's scale.

  14. jpearman

    27 Jan 2015 Moderator, ROBOTC Tech Support, V5 Beta Moderator Rockstar, Los Angeles 8888

    @Team80_Giraffes This code assumes the pot values are exactly equal when level. If one pot is twisted a smidge, the level position values would be different and you are going to compensate for it.

    That's what I assumed, and in many cases that will be true. The code is never going to get the pot values exactly the same anyway so a small error between the two doesn't matter that much. The pot values are also noisy, without any movement they will be changing perhaps by +/- 10. The code can get a lot more complicated quite quickly and the end result may not be much better. Even using PID there can be problems deciding what to do if one side stalls.

  15. Mrlong2

    27 Jan 2015 Cloudsdale, south region. Movi...

    I think this is something like the problem my team was having. If the motors are constantly adjusting themselves, the burn out. I think our programer fixed it by having a automatic mode and a manual mode. So you just toggle to manual whenever you're not lifting so the motors don't move, unless you tell them to.

  16. Team80_Giraffes

    27 Jan 2015 Event Partner Downingtown, PA 80[A-Z]$|81[A-Z]$|90[A-Z]$|91[...

    @Mrlong2 I think this is something like the problem my team was having. If the motors are constantly adjusting themselves, the burn out. I think our programer fixed it by having a automatic mode and a manual mode. So you just toggle to manual whenever you're not lifting so the motors don't move, unless you tell them to.

    Very advisable to toggle between the two. Sometimes you just need to nudge it a bit from the preset.

    The length of time and amount of current running through the motors at a high rate lead most to motor burn out. jpearman has done a number of analysis you can search on the forums for the current and PTC tripping. It's pretty fascinating stuff.

    Using his slew rate code helps your motors a bit too so you don't slam the motor with sudden spikes which releases the hounds on it with a current spike. The PTC does not like this very much. So dampening your PID with the slew rate helps keep the motors from overheating. But by no means will it cure a motor geared for speed that can't operate well with the torque requirements to move. Physics still trumps code. Sorry.

  17. tabor473

    27 Jan 2015 V5 Beta Tester OYES, WPI

    @Team80_Giraffes This code assumes the pot values are exactly equal when level. If one pot is twisted a smidge, the level position values would be different and you are going to compensate for it.

    // Figure out the error between left and right
        leftrighterror = SensorValue[ potleft ] - SensorValue[ potright ];

    So you need to have a start value of each and see the difference in those values tuned to each other. Also make sure the pots are reading exactly the same and the variable resistance is exactly the same as you go up and the range is the exact same until the top.

    I need to think about this a bit more to get some good code examples. Adjust your sensor values so the scale of left and right is compensating against each other's scale.

    We did some testing and aligned 4 pots to each other perfectly(just bolted them together with nothing else in between) so they were all reading 50 at the same time. Then we turned the axle and the values changed at different rates. Even if the beginning and the end lined up perfectly all 4 of the pots would differ by as much as 100 between any 2 pots.

    It seemed that every pot changed at different non linear rates throughout its range. The solution that I thought of was making a LUT for each pot to get the angle each side was at and then use those angles for the P loop correction.

  18. jpearman

    27 Jan 2015 Moderator, ROBOTC Tech Support, V5 Beta Moderator Rockstar, Los Angeles 8888

    @tabor473 We did some testing and aligned 4 pots to each other perfectly(just bolted them together with nothing else in between) so they were all reading 50 at the same time. Then we turned the axle and the values changed at different rates. Even if the beginning and the end lined up perfectly all 4 of the pots would differ by as much as 100 between any 2 pots.

    It seemed that every pot changed at different non linear rates throughout its range. The solution that I thought of was making a LUT for each pot to get the angle each side was at and then use those angles for the P loop correction.

    Probably why I never actually use pots, always encoders. Perhaps the best solution is one pot for absolute position and then also encode each side with an IME (yes, I know, IMEs have issues but I still favor them over other methods).

  19. tabor473

    27 Jan 2015 V5 Beta Tester OYES, WPI

    @jpearman Probably why I never actually use pots, always encoders. Perhaps the best solution is one pot for absolute position and then also encode each side with an IME (yes, I know, IMEs have issues but I still favor them over other methods).

    I was able to get ime based stability control fairly simply but I was hoping to be able to know absolute position rather than relative position by using pots. I was just not able to get it working. Pots are great for set heights because only one is used.

    IME stability with pot set heights should work but I really wish pots were a lot more consistent.

    If anyone wants to repeat my test with a bunch of pots and publish the results I think everyone would appreciate it.

  20. Team80_Giraffes

    27 Jan 2015 Event Partner Downingtown, PA 80[A-Z]$|81[A-Z]$|90[A-Z]$|91[...

    @tabor473 I was able to get ime based stability control fairly simply but I was hoping to be able to know absolute position rather than relative position by using pots. I was just not able to get it working. Pots are great for set heights because only one is used.

    IME stability with pot set heights should work but I really wish pots were a lot more consistent.

    If anyone wants to repeat my test with a bunch of pots and publish the results I think everyone would appreciate it.

    Agree one pot helps a lot. I have a team rebuilding an 8 bar arm right now I could get a few pots all on the one shaft to read them together. they will need to do that for finding the broken pots anyway. We tend to throw them back in the box versus throw them out for some reason. :confused:

    Remember the end goal, we are trying to get left and right height the same. So use pot to get to height on each side and the difference in height makes your power requiring adjustment. The pot is the variable you have to get to the height.

    Treating each pot as an independent function gets you to figuring out the pot value to the height on that side. f(pot)=height for that side. So now you get into functions of pot to height on each side and what is the error in distance and adjust motor power there rather than the pot readings directly as they may be different non-linear functions for a given height.

    So how to do that:

    1. take readings of each pot at different heights
    2. write down readings in excel in columns at each height
    3. use a scatter plot (not a line one) in Excel to graph each pot vs height
    4. curve fit the line to get a pot to height function on each line. Line graphs won't make the linear regression function correctly
    5. compute height left and height right in robot C using this function
    6. make your error and error of height difference treating left or right as your reference. Adjust your motor power from there using proportional control! (adjust for cases where you are already at 127 power on one side)

    I've seen pots be weird and kind of non linear in their usage so that's why I try and steer folks to make the lift more mechanically tight first and that solves 80% of the issues.

    If both pots were exactly the same start point, same end point, and exactly the same increase over the height, then you could just use pot left and pot right difference to see if you need to move one side up more than the other. But these things tend to vary on a relatively cheap variable resistor in a plastic case.

    Twisting of the 4 or 6 or 8 bar left to right lift may or may not register on the pot on the lift tower. Not sure if you can correct for that from a pot reading. Mechanical stability helps on those typically. Tighter joints, another vertical bar to constrain motion, horizontal bars to keep it a tight rectangle plane all helps in physical construction assists. (But many add weight or may get in the way on a double reverse config)

  21. Newer ›
 

or Sign Up to reply!