ROBOTC.net forums
http://www.robotc.net/forums/

Continuous Servo Braking
http://www.robotc.net/forums/viewtopic.php?f=52&t=3774
Page 2 of 2

Author:  MHTS [ Sat Oct 05, 2013 2:13 pm ]
Post subject:  Re: Continuous Servo Braking

You can find the latest library here.
http://proj.titanrobotics.net/hg/Ftc/20 ... e7c/ftclib

What did you change in gyro.h? There are a lot of reasons why you may turn the wrong way. Depending on the reason, there are also a lot of ways to fix it. For example, if you mount your gyro up side down, you may want to add the GYROO_INVERSE flag when calling GyroInit.

If you believe you have fixed a bug in gyro.h, I would like to see the fix and integrate it.

Author:  SergeantFTC [ Sat Oct 05, 2013 3:06 pm ]
Post subject:  Re: Continuous Servo Braking

We haven't touched it since we implemented it, so I unfortunately couldn't tell you what specifically we changed.

I downloaded the latest version, I'm going to get it working from that, and forget the old code. So far the only thing I've changed is to remove the TFuncName, TLevel, TEnter, and TExit function calls, since it wouldn't compile with them and you said they were just for debugging here: http://www.robotc.net/forums/viewtopic. ... 4&start=30
I haven't gotten to do any testing yet. Hopefully I'll get to start that on Tuesday. It compiles though.

Author:  MHTS [ Sat Oct 05, 2013 3:16 pm ]
Post subject:  Re: Continuous Servo Braking

If you send me the changed code, I can do a diff to figure out the changes.

Author:  SergeantFTC [ Sat Oct 05, 2013 3:22 pm ]
Post subject:  Re: Continuous Servo Braking

If I send you our old modified code you mean?

Author:  SergeantFTC [ Sat Oct 05, 2013 3:25 pm ]
Post subject:  Re: Continuous Servo Braking

Here is the version of gyro.h we've been using for the last 2(?) seasons.

Attachments:
gyro.h [5.42 KiB]
Downloaded 712 times

Author:  SergeantFTC [ Tue Oct 08, 2013 10:18 pm ]
Post subject:  Re: Continuous Servo Braking

Wait a minute. All of the stuff about actually turning, like SetTurnTarget() and TurnFunction() isn't in the newer version. And I can't even find it in older versions. Was that ever in your code or did our mentor write that?

Author:  MHTS [ Wed Oct 09, 2013 12:26 am ]
Post subject:  Re: Continuous Servo Braking

That's your code. I was about to comment that you should not add code to gyro.h. That defeats modularity. You should put that code in something like drivetarget.h.

Author:  MHTS [ Wed Oct 09, 2013 2:14 am ]
Post subject:  Re: Continuous Servo Braking

To show you how to turn your code into reusable object oriented module, here is an example. Save the following code into a file named "pidturn.h"
Code:
#ifndef _PIDTURN_H
#define _PIDTURN_H

#pragma systemFile

typedef struct
{
    tMotor leftMotor;
    tMotor rightMotor;
    int    minTurnPower;// Minimum turn power
    GYRO  *gyro;        // The gyro to be used for turning
    float  Kp;          // Proportion constant for turning
    float  tolerance;   // Turn tolernace
    float  angle;       // Absolute turn angle
    bool   enabled;     // PID turn is enabled or not
} PIDTURN;

void PidTurnInit(
    PIDTURN &pidTurn,
    tMotor leftMotor,
    tMotor rightMotor,
    int minTurnPower,
    GYRO &gyro,
    float Kp,
    float tolerance)
{
    pidTurn.leftMotor = leftMotor;
    pidTurn.rightMotor = rightMotor;
    pidTurn.minTurnPower = abs(minTurnPower);
    pidTurn.gyro = &gyro;
    pidTurn.Kp = Kp;
    pidTurn.tolerance = tolerance;
    pidTurn.angle = 0;
    pidTurn.enabled = false;
}

void PidTurnSetTarget(PIDTURN &pidTurn, float angle)
{
    pidTurn.angle = GyroGetHeading(*pidTurn.gyro) + angle;
    pidTurn.enabled = true;
}

bool PidTurnTask(PIDTURN &pidTurn)
{
    if (pidTurn.enabled)
    {
        float error = pidTurn.angle - GyroGetHeading(*pidTurn.gyro);
        if (abs(error) > pidTurn.tolerance)
        {
            int turnPower = BOUND((int)(pidTurn.Kp*error), -100, 100);
            if (abs(turnPower) < pidTurn.minTurnPower)
            {
                turnPower = (turnPower < 0)?
                                -pidTurn.minTurnPower:
                                 pidTurn.minTurnPower;
            }
            nxtDisplayCenteredTextLine(4, "%d", turnPower);
            motor[pidTurn.leftMotor] = turnPower;
            motor[pidTurn.rightMotor] = -turnPower;
        }
        else
        {
            motor[pidTurn.leftMotor] = 0;
            motor[pidTurn.rightMotor] = 0;
            pidTurn.enabled = false;
        }
    }

    return pidTurn.enabled;
}

#endif  //ifndef _PIDTURN_H

The following is the main code that shows how to use the modules:
Code:
#pragma config(Sensor, S1,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,          rightMotor,    tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "gyro.h"
#include "pidturn.h"

#define MIN_TURN_POWER      85
#define TURN_KP             0.9
#define TURN_TOLERANCE      0.5

GYRO    g_Gyro;
PIDTURN g_PidTurn;

task main()
{
    GyroInit(g_Gyro, gyroSensor);
    PidTurnInit(g_PidTurn, leftMotor, rightMotor, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
    PidTurnSetTarget(g_PidTurn, 90.0);
    while (true)
    {
        GyroTask(g_Gyro);
        PidTurnTask(g_PidTurn);
        wait1Msec(20);
    }
}

By creating the pidturn.h module, you now have a generic module that can handle PID controlled turn given the left and right motors, a gyro and the parameters of the PID control (Kp and tolerance).
Note that unlike your original code, I don't have "direction" in the PidTurnTask code. This is because if the robot is turning the wrong way probably because your gyro is mounted up-side-down, you can set the GYROO_INVERSE option when calling GyroInit. Therefore, "direction" is really redundant.
With pidturn.h, you can now use gyro.h as is without modification (except for removing TFunc, TLevel, TEnter and TExit and possibly adding the DEADBAND and BOUND macros).
Alternatively, if you really don't want to change gyro.h, you can also include trcdefs.h and dbgtrace.h from our library. This contains the definitions of all the missing things in gyro.h.

Author:  SergeantFTC [ Fri Oct 11, 2013 10:26 pm ]
Post subject:  Re: Continuous Servo Braking

wow, thank you! :) So, to specify direction, do we use negative values for the turn target? (sorry, I haven't had time to run through the code to find out for myself)

Author:  MHTS [ Sat Oct 12, 2013 4:28 am ]
Post subject:  Re: Continuous Servo Braking

Since I do not have the rest of your code, I can only guess how gyro.h was used. If I understand your original code correctly, the TurnFunction is actually a periodic task. I don't know why you need the "direction" parameter in TurnFunction. The only explanation I came up with is that you may have the gyro mounted up-side-down so it will cause the code to turn the wrong way. So direction could have been used for correcting that. But for the code I posted, you can achieve that with the GYROO_INVERSE option when calling GyroInit. In any case, for the code I posted, it turns right 90 degrees. If you want to turn left, you call PidTurnSetTarget with a negative angle. For example:
Code:
#pragma config(Sensor, S1,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,          rightMotor,    tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "gyro.h"
#include "pidturn.h"

#define MIN_TURN_POWER      85
#define TURN_KP             0.9
#define TURN_TOLERANCE      0.5

GYRO    g_Gyro;
PIDTURN g_PidTurn;

task main()
{
    GyroInit(g_Gyro, gyroSensor);
    PidTurnInit(g_PidTurn, leftMotor, rightMotor, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
    //
    // Turn right 90 degrees.
    //
    PidTurnSetTarget(g_PidTurn, 90.0);
    while (g_PidTurn.enabled)
    {
        GyroTask(g_Gyro);
        PidTurnTask(g_PidTurn);
        wait1Msec(20);
    }
    //
    // Turn left 90 degrees.
    //
    PidTurnSetTarget(g_PidTurn, -90.0);
    while (g_PidTurn.enabled)
    {
        GyroTask(g_Gyro);
        PidTurnTask(g_PidTurn);
        wait1Msec(20);
    }
}

Author:  hpolack [ Thu Feb 12, 2015 11:07 am ]
Post subject:  Re: Continuous Servo Braking

My kids are in science olympiad and have the exact problem you described with Gravity Brake. They have a heavy arm 393 motors and we bought the integrated encoders. We basically need to tell the arm to stop going down when we stop pressing the down button. Today, it just coasts and goes an extra 2-3 inches which does not work when you have a bucket on the front and trying to position to pick up items and to dump them in a bucket. We have an arm to go up and down and a bucket to tip forward and back. We would like to have both setup to brake stop, but the arm up and down suffers the worst from the gravity pull. If you could give us a leg up in what to do using RobotC configuration or programming, we would be very grateful. This is our first robot build, but I have programmed mainframes and PCs a couple of decades ago, so I have the basics of code breakdown.

Thanks in advance for your help.

Author:  MHTS [ Thu Feb 12, 2015 11:39 pm ]
Post subject:  Re: Continuous Servo Braking

hpolack wrote:
My kids are in science olympiad and have the exact problem you described with Gravity Brake. They have a heavy arm 393 motors and we bought the integrated encoders. We basically need to tell the arm to stop going down when we stop pressing the down button. Today, it just coasts and goes an extra 2-3 inches which does not work when you have a bucket on the front and trying to position to pick up items and to dump them in a bucket. We have an arm to go up and down and a bucket to tip forward and back. We would like to have both setup to brake stop, but the arm up and down suffers the worst from the gravity pull. If you could give us a leg up in what to do using RobotC configuration or programming, we would be very grateful. This is our first robot build, but I have programmed mainframes and PCs a couple of decades ago, so I have the basics of code breakdown.

Thanks in advance for your help.

I am not familiar with VEX systems. If you have an encoder on the arm, you can turn on position PID control in RobotC to set an encoder target and hold it there. Again, I am not familiar with RobotC's built-in PID control so I can't tell you how well it will do that. But in theory, PID control should be able to hold the motor at a certain position.

Page 2 of 2 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/