View unanswered posts | View active topics It is currently Sun Sep 14, 2014 10:01 pm






Reply to topic  [ 25 posts ]  Go to page Previous  1, 2
Continuous Servo Braking 
Author Message
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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.


Sat Oct 05, 2013 2:13 pm
Profile
Rookie

Joined: Tue Nov 20, 2012 8:09 pm
Posts: 35
Post 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.


Sat Oct 05, 2013 3:06 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Continuous Servo Braking
If you send me the changed code, I can do a diff to figure out the changes.


Sat Oct 05, 2013 3:16 pm
Profile
Rookie

Joined: Tue Nov 20, 2012 8:09 pm
Posts: 35
Post Re: Continuous Servo Braking
If I send you our old modified code you mean?


Sat Oct 05, 2013 3:22 pm
Profile
Rookie

Joined: Tue Nov 20, 2012 8:09 pm
Posts: 35
Post 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 250 times
Sat Oct 05, 2013 3:25 pm
Profile
Rookie

Joined: Tue Nov 20, 2012 8:09 pm
Posts: 35
Post 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?


Tue Oct 08, 2013 10:18 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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.


Wed Oct 09, 2013 12:26 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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.


Wed Oct 09, 2013 2:14 am
Profile
Rookie

Joined: Tue Nov 20, 2012 8:09 pm
Posts: 35
Post 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)


Fri Oct 11, 2013 10:26 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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);
    }
}


Sat Oct 12, 2013 4:28 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 25 posts ]  Go to page Previous  1, 2

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  
cron



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.