View unanswered posts | View active topics It is currently Fri Oct 24, 2014 5:25 pm






Reply to topic  [ 133 posts ]  Go to page 1, 2, 3, 4, 5 ... 9  Next
Endcoders better? 
Author Message
Rookie

Joined: Tue Oct 19, 2010 1:16 am
Posts: 9
Post Endcoders better?
Ok, Right now, my teams robot is running off a time sequence for autonmous. We have gone to two different regionals, and the foam pads have been different both times. One was new pads, the other had pads from years before (wear and tear). With the difference in pads, the autonomous is completely different. It worked on the first regional, but the second regional, we were having trouble.

My Question though, Is it better to program the autonomous with encoders, or will it do the same thing as a time sequence does. I am not able to get a gyro, as much as I would like to. My Team is on a VERY tight budget. We are going to be headed to nationals, and we want the autonomous to be the best it can be.


Sat Mar 26, 2011 11:22 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
If you are talking about using the encoders to measure the distance you have traveled, then yes it is definitely better than using time. When using time to measure distance, you are assuming the speed of your robot is fixed. But in reality, the speed of your robot is never the same in each match. It could be because of the pad friction is different in different field like you said. It could even vary on different spots on the same field. The condition of the battery plays a major part too. If your battery is fully charged, your robot may run faster than when the battery has been through several matches. Even if you change your battery before each match, the age of different batteries can also affect the voltage/current of the battery during a match. The motor performance can be affected by temperature too. If the robot is runing back to back matches, the motors may get warmer and thus affect its speed. If you change out your fried motors in between matches, the performance of the motors are not identical. So you can see there are a lot of factors affecting the speed of your robot. That makes using timed autonomous not very repeatable and very inaccurate.
However, if you are using encoders for measuring distance, the encoders used on the Tetrix motors have 1440 pulse per revolution. Depending on your wheel size, gear ratio and whether you mount the encoders on the wheel side or the motor side, the resolution of the encoder can be quite high. For our robot, we used 24:16 gear ratio, 4" wheels and encoders mounted on the motor side, the encoders can give me a resolution of 0.01354 inch per encoder click. So unless the wheel is slipping, it can be that accurate. In our autonomous, we routinely commanded the robot to go across the entire field and can stop at the target within an inch of the inteneded distance. But of course, encoders are good at measuring linear distance. If you want to turn accurately, then you need to use a gyro. Using the right sensors is very important for an accurate autonomous routine. On our robot, we used encoders for linear distance, gyro for turn, accelerometer for balancing on the bridge and IR seeker for locating the dispenser.


Sat Mar 26, 2011 2:35 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
With some help, we have gotten the Gyro. Is there anyone here that would grasiously put at least a code that would turn the robot 90 degrees using the gryo? Maybe explain why the line is being added...? From there I can learn the rest.

Thanks

Team 2844


Sat Apr 09, 2011 1:09 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Gyro gives you the turn rate. In order to use it to measure your robot's heading, you need to integrate its value against time. Once you have the robot's heading, you can compare it against your target heading. Here is a link to the gyro module in our library. Note that the GyroTask is called periodically in your robot loop. This will integrate the turn rate into the robot's heading. Since analog sensors have bias and noise, the GyroCal function is called during robot initialization to calibrate the gyro by determining the bias offset and the noise level which will be eliminated before the integration. To use our gyro module, you need to do something like this in your main code:
http://proj.titanrobotics.net/hg/Ftc/20 ... lib/gyro.h
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     Magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Documents and Settings\Mom\My Documents\Robotics\Code\gyro.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_tolerance = 0.5;  //needs to be tuned
float Kp = 0.5;           //needs to be tuned

void SetTurnTarget(float angle)
{
    g_turnTarget = GyroGetHeading(g_Gyro) + angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (abs(error) > g_tolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100.
            //
            int turnPower = BOUND((int)(Kp*error), -100, 100);
            motor[leftMotor] = turnPower;
            motor[rightMotor] = -turnPower;
        }
        else
        {
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            g_turnEnabled = false;
        }
    }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
  initializeRobot();
  waitForStart(); // Wait for the beginning of autonomous phase.
    SetTurnTarget(90.0);
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        wait1Msec(10);
    }
}


Last edited by MHTS on Mon Oct 10, 2011 8:22 pm, edited 2 times in total.



Sat Apr 09, 2011 5:41 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, when you say that "gyro.h" goes into my main code, Do you mean into my teleop code? I'm a little confused here. Dont I just save it to the Includes, of RobotC's files as an .h file. <-- I have done that, but it still wont work. Im sorry, but I'm just confused here..


Sat Apr 09, 2011 10:42 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
You are trying to do this in autonomous, aren't you? First, copy the following code and save it to a file named "gyro.h":
Code:
#if 0
/// Copyright (c) Titan Robotics Club. All rights reserved.
///
/// <module name="gyro.h" />
///
/// <summary>
///     This module contains the library functions for the gyro sensor.
/// </summary>
///
/// <remarks>
///     Environment: RobotC for Lego Mindstorms NXT.
/// </remarks>
#endif

#ifndef _GYRO_H
#define _GYRO_H

#include "..\HTDriversV1.6\drivers\HTGYRO-driver.h"

#pragma systemFile

//
// Constants.
//
#define GYROF_USER_MASK         0x00ff
#define GYROF_INVERSE           0x0001
#ifdef HTSMUX_STATUS
  #define GYROF_HTSMUX          0x0080
#endif

#define GYRO_NUM_CAL_SAMPLES    50
#define GYRO_CAL_INTERVAL       10

//
// Macros
//
#define __in
#define __out
#define __inout
#define DEADBAND(n,t)           ((abs(n) > (t))? (n): 0)
#define GyroGetTurnRate(p)      (p.turnRate)
#define GyroGetHeading(p)       (p.heading)

//
// Type definitions.
//
typedef struct
{
    int   sensorID;
    int   gyroFlags;
    int   zeroOffset;
    int   deadBand;
    long  timestamp;
    int   turnRate;
    float heading;
} GYRO;

/**
 *  This function calibrates the gyro for zero offset and deadband.
 *
 *  @param gyro Points to the GYRO structure to be initialized.
 *  @param numSamples Specifies the number of calibration samples.
 *  @param calInterval Specifies the calibration interval in msec.
 */
void
GyroCal(
    __out GYRO &gyro,
    __in  int numSamples,
    __in  int calInterval
    )
{
    int i;
    int turnRate;
    int min, max;

    gyro.zeroOffset = 0;
    gyro.deadBand = 0;
    min = 1023;
    max = 0;

    for (i = 0; i < numSamples; i++)
    {
#ifdef HTSMUX_STATUS
        turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
        turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
        gyro.zeroOffset += turnRate;

        if (turnRate < min)
        {
            min = turnRate;
        }
        else if (turnRate > max)
        {
            max = turnRate;
        }

        wait1Msec(calInterval);
    }

    gyro.zeroOffset /= numSamples;
    gyro.deadBand = max - min;

    return;
}   //GyroCal

/**
 *  This function performs the gyro task where it integrates the turn rate
 *  into a heading value.
 *
 *  @param gyro Points to the GYRO structure.
 */
void
GyroTask(
    __inout GYRO &gyro
    )
{
    long currTime;

    currTime = nPgmTime;
#ifdef HTSMUX_STATUS
    gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
    gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
    gyro.turnRate -= gyro.zeroOffset;
    gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
    if (gyro.gyroFlags & GYROF_INVERSE)
    {
        gyro.turnRate = -gyro.turnRate;
    }
    gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
    gyro.timestamp = currTime;

    return;
}   //GyroTask

/**
 *  This function resets the gyro heading.
 *
 *  @param gyro Points to the GYRO structure to be reset.
 */
void
GyroReset(
    __out GYRO &gyro
    )
{
    GyroTask(gyro);
    gyro.heading = 0;
    return;
}   //GyroReset

/**
 *  This function initializes the gyro sensor.
 *
 *  @param gyro Points to the GYRO structure to be initialized.
 *  @param sensorID Specifies the ID of the gyro sensor.
 *  @param gyroFlags Specifies the gyro flags.
 */
void
GyroInit(
    __out GYRO &gyro,
    __in  int sensorID,
    __in  int gyroFlags
    )
{
    gyro.sensorID = sensorID;
    gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
#ifdef HTSMUX_STATUS
    if (gyro.gyroFlags & GYROF_HTSMUX)
    {
        HTGYROstartCal((tMUXSensor)sensorID);
    }
    else
    {
        HTGYROstartCal((tSensors)sensorID);
    }
#else
    HTGYROstartCal((tSensors)sensorID);
#endif
    GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
    gyro.timestamp = nPgmTime;
    GyroReset(gyro);

    return;
}   //GyroInit
#endif  //ifndef _GYRO_H

Secondly, I assume you already have Xander's drivers because the gyro code above includes Xander's gyro driver. You need to change the include path to point to where you save Xander's drivers. Next, since I don't know what your autonomous code looks like, I could only imagine how you would tie in the gyro code and use it in your autonmous. As I mentioned in my previous post, create the SetTurnTarget and TurnTask functions. Then in your autonomous task main, before going into the robot while loop, call GyroInit to initialize the gyro. Then at whichever point in your code that you need to turn 90 degrees, call SetTurnTarget(90.0) or whatever angle you wish (90.0 for turning right 90-degree, and -90.0 to turn left 90-degree). Most importantly, somewhere in your robot loop, you need to periodically call GyroTask and TurnTask so that the gyro task can do the integration and the turn task can do PID control on the turning.


Last edited by MHTS on Sat Apr 09, 2011 7:49 pm, edited 1 time in total.



Sat Apr 09, 2011 2:40 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I have saved the code above as "gyro.h", but it still wont do anything to my autonomous code. My auto code right now is only trying to get the robot to turn 90 degrees so i can figure it out. The code is listed below, with what is showing up as "wrong" in RobotC underneath.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     Magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "gyro.h"
#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_tolerance = 0.5;  //needs to be tuned
float Kp = 0.5;           //needs to be tuned

void SetTurnTarget(float angle)
{
    g_turnTarget = GyroGetHeading(g_yro) + angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_tolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100.
            //
            int turnPower = BOUND((int)(Kp*error), -100, 100);
            motor[motorD] = turnPower;
            motor[motorE] = -turnPower;
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_turnEnabled = false;
        }
    }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}

task main()
{
  initializeRobot();
  waitForStart(); // Wait for the beginning of autonomous phase.
//times are not correct, need to be tested
  GyroInit(g_Gyro, gyro, 0);
    SetTurnTarget(90.0);
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        wait1Msec(10);
    }
  wait10Msec(999999);
}


Code:
File "RSWA.c" compiled on Apr 09 2011 13:32:06
Line 18 **Severe*:Couldn't open '#include' file 'gyro.h'
Line 21*Warning*:Substituting similar variable 'gyro' for 'GYRO'. Check spelling and letter case.
           **Error**:Expected->';'. Found 'g_Gyro'
           **Error**:Executable statements not valid in 'main' declaration block
           **Error**:Undefined variable 'g_Gyro'. 'short' assumed.
           *Warning*:Meaningless statement -- no code generated
Line 29 **Error**:Undefined procedure 'GyroGetHeading'.
           **Error**:Undefined variable 'g_yro'. 'short' assumed.
          *Warning*:Invalid '+' operation for types 'void' and 'float'
Line 37 **Error**:Undefined procedure 'GyroGetHeading'.
          *Warning*:Invalid '-' operation for types 'float' and 'void'
Line 76 **Error**:Undefined procedure 'GyroInit'.
Line 80 **Error**:Undefined procedure 'GyroTask'.


Sat Apr 09, 2011 4:42 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I have been playing around with just the #include Lines, and I have them working. BUT I am using the latest of Xander's drivers. With it work, I have run into problems with the "gyro.h" code you gave me. All I have done is changed the #include line path. The Code you gave me, with Errors.. (There is // LINE:____ in the codes that I added so you could see what line i am having trouble on.)
Code:
#if 0
/// Copyright (c) Titan Robotics Club. All rights reserved.
///
/// <module name="gyro.h" />
///
/// <summary>
///     This module contains the library functions for the gyro sensor.
/// </summary>
///
/// <remarks>
///     Environment: RobotC for Lego Mindstorms NXT.
/// </remarks>
#endif

#ifndef _GYRO_H
#define _GYRO_H

#include "C:\Projects\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"

#pragma systemFile

//
// Constants.
//
#define GYROF_USER_MASK         0x00ff
#define GYROF_INVERSE           0x0001
#ifdef HTSMUX_STATUS
  #define GYROF_HTSMUX          0x0080
#endif

#define GYRO_NUM_CAL_SAMPLES    50
#define GYRO_CAL_INTERVAL       10

//
// Macros
//
#define __in
#define __out
#define DEADBAND(n,t)           ((abs(n) > (t))? (n): 0)
#define GyroGetTurnRate(p)      (p.turnRate)
#define GyroGetHeading(p)       (p.heading)

//
// Type definitions.
//
typedef struct
{
    int   sensorID;
    int   gyroFlags;
    int   zeroOffset;
    int   deadBand;
    long  timestamp;
    int   turnRate;
    float heading;
} GYRO;

/**
*  This function calibrates the gyro for zero offset and deadband.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param numSamples Specifies the number of calibration samples.
*  @param calInterval Specifies the calibration interval in msec.
*/
void
GyroCal(
    __out GYRO &gyro,
    __in  int numSamples,
    __in  int calInterval
    )
{
    int i;
    int turnRate;
    int min, max;

    gyro.zeroOffset = 0;
    gyro.deadBand = 0;
    min = 1023;
    max = 0;

    for (i = 0; i < numSamples; i++)
    {
#ifdef HTSMUX_STATUS
        turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
        turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
        gyro.zeroOffset += turnRate;

        if (turnRate < min)
        {
            min = turnRate;
        }
        else if (turnRate > max)
        {
            max = turnRate;
        }

        wait1Msec(calInterval);
    }

    gyro.zeroOffset /= numSamples;
    gyro.deadBand = max - min;

    return;
}   //GyroCal

/**
*  This function performs the gyro task where it integrates the turn rate
*  into a heading value.
*
*  @param gyro Points to the GYRO structure.
*/
void
GyroTask(
    __inout GYRO &gyro                   //LINE 117
    )                                            //LINE 118                                                           
{
    long currTime;

    currTime = nPgmTime;
#ifdef HTSMUX_STATUS
    gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
    gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
    gyro.turnRate -= gyro.zeroOffset;
    gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
    if (gyro.gyroFlags & GYROF_INVERSE)
    {
        gyro.turnRate = -gyro.turnRate;
    }
    gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
    gyro.timestamp = currTime;

    return;
}   //GyroTask

/**
*  This function resets the gyro heading.
*
*  @param gyro Points to the GYRO structure to be reset.
*/
void
GyroReset(
    __out GYRO &gyro
    )
{
    GyroTask(gyro);                                       //LINE 152
    gyro.heading = 0;
    return;
}   //GyroReset

/**
*  This function initializes the gyro sensor.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param sensorID Specifies the ID of the gyro sensor.
*  @param gyroFlags Specifies the gyro flags.
*/
void
GyroInit(
    __out GYRO &gyro,
    __in  int sensorID,
    __in  int gyroFlags
    )
{
    gyro.sensorID = sensorID;
    gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
#ifdef HTSMUX_STATUS
    if (gyro.gyroFlags & GYROF_HTSMUX)
    {
        HTGYROstartCal((tMUXSensor)sensorID);
    }
    else
    {
        HTGYROstartCal((tSensors)sensorID);
    }
#else
    HTGYROstartCal((tSensors)sensorID);
#endif
    GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
    gyro.timestamp = nPgmTime;
    GyroReset(gyro);

    return;
}   //GyroInit
#endif  //ifndef _GYRO_H


File "RSWA .c" compiled on Apr 09 2011 14:57:06
*Warning*:Variable '__inout' declaration must be qualified with type. Type 'short' used.
Line 117: **Error**:Ummatched left parenthesis '('
**Error**:Expected->')'. Found 'GYRO'
**Error**:No body declared for procedure
**Error**:Expected->'}'. Found 'GYRO'
Line 118: **Error**:Type mismatch with previous 'const' declaration for variable with same name
**Error**:Missing ';' before ')'
**Error**:Unexpected scanner token-> ')'

Line 152: *Warning*:Invalid '=' operation for types 'short' and 'GYRO &'

Then mine:

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     Magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Documents and Settings\Mom\My Documents\Robotics\Code\gyro.h"
#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_tolerance = 0.5;  //needs to be tuned
float Kp = 0.5;           //needs to be tuned

void SetTurnTarget(float angle)
{
    g_turnTarget = GyroGetHeading(g_yro) + angle;             //Line 29
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_tolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100.
            //
            int turnPower = BOUND((int)(Kp*error), -100, 100);
            motor[motorD] = turnPower;
            motor[motorE] = -turnPower;
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_turnEnabled = false;
        }
    }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}

task main()                                           //Line 71
{
  initializeRobot();
  waitForStart(); // Wait for the beginning of autonomous phase.
//times are not correct, need to be tested
  GyroInit(g_Gyro, gyro, 0);                          //Line 76
    SetTurnTarget(90.0);
    while (true)
    {
        GyroTask(g_Gyro);[/color]                   //Line 80
        TurnTask();
        wait1Msec(10);
    }
  wait10Msec(999999);
}


Line 29:**Error**:Undefined variable 'g_yro'. 'short' assumed.
**Error**:'g_yro' is not a 'struct' variable
Line 71: **Error**:Duplicate definition for task 'main'.
Line 76: *Warning*:Invalid '=' operation for types 'short' and 'GYRO &'
Line 80: *Warning*:Invalid '=' operation for types 'short' and 'GYRO'


Sat Apr 09, 2011 6:11 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Sorry, I missed one line in gyro.h. Please add the define for __inout like below:
Code:
//
// Macros
//
#define __in
#define __out
#define __inout

That should take care all the errors in gyro.h.
BTW, you should either put the GyroInit() line inside initializeRobot or right below it. In other words, it should be performed before waitForStart() because GyroInit involves gyro calibration which takes a long time. You don't want it stop dead doing calibration when the competition starts.
Also, I have edited my previous posts to correct the errors in the code. Please refer to the code in the previous posts.


Sat Apr 09, 2011 7:45 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, so its now working but I have a few questions from some problems I have seen. Since I am working on this at my house, without the pads, I am running this on my hard floor mat. It runs the 90 degrees perfectly on the floor mat time after time. But when I moved it to the carpet, to test different frictions, it turns about 20-30 degrees then runs forward. Is there something I need to put into the code to fix the errors of different frictions that might happen on the pads at World?

Also... In the code, you said that the "float g_tolerance = 0.5" and "float KP = 0.5;" needed to be tuned. Could you explain what they are and how to tune those if I need to have them?

I am just starting to program the encoders, but the sample program code that is given, doesnt work. I run it, and it goes past what it is told to do. I have not changed anything in it, so the code looks just like it is if you open it. Is there something wrong with that code, or a better code to run the encoders off of.

Thanks for putting up for all these questions... just trying to learn all of this programming.


Sun Apr 10, 2011 2:23 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Team2844 wrote:
But when I moved it to the carpet, to test different frictions, it turns about 20-30 degrees then runs forward. Is there something I need to put into the code to fix the errors of different frictions that might happen on the pads at World?

Do you have more code than in the previous posts? The code in the previous post does not run forward. If you have more code than in the previous post, I need to see it in order to tell you what's wrong.
Team2844 wrote:
Also... In the code, you said that the "float g_tolerance = 0.5" and "float KP = 0.5;" needed to be tuned. Could you explain what they are and how to tune those if I need to have them?

The theory behind PID control is that it is a feedback control system. The output of the system is determined by how far you are away from the target. If you are far away, the output will be large. If you are close to your target, the output will be lower. Basically, you run as hard as you can if the target is far away but you slow down when you are close to the target. However, if you are very close to the target, it is possible that the output force is so low that it cannot even overcome its friction. This is called steady state error. Steady state error will prevent a PID control system from ever getting to its target. If your code is waiting for the PID control system to stop after it reaches its target, it will wait forever. Tolerance is a way to tell the PID control system that "you are close enough", so consider it reaching target if you are within the tolerance. Another way to do it is to increase Kp, the proportional constant. Since Kp is multiplied to the error, it scales the output higher or lower. By increasing Kp, the output could be made bigger even though it is close to the target so that it can overcome friction. However, if Kp is too larger, you will have oscillation (ringing). The robot will pass the target because it is too fast, then it will back up and may back up pass the target again. So this cycle repeats back and forth a few times before it settles, or it may never settle and go back and forth forever. So you should tune Kp as large as possible until it starts oscillating and then you should decrease it until the oscillation stops. You may still need give it a tolerance because it will not be on target exactly. For example, in the code above, the tolerance is 0.5 degree.
Team2844 wrote:
I am just starting to program the encoders, but the sample program code that is given, doesnt work. I run it, and it goes past what it is told to do. I have not changed anything in it, so the code looks just like it is if you open it. Is there something wrong with that code, or a better code to run the encoders off of.

What sample code did you use?


Sun Apr 10, 2011 6:03 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
MHTS wrote:
Do you have more code than in the previous posts? The code in the previous post does not run forward. If you have more code than in the previous post, I need to see it in order to tell you what's wrong.

No, it is the same exact code as above. Just when I put it on the carpet, it does this.
MHTS wrote:
What sample code did you use?

TETRIX nMotorTargetEncoders Example


Mon Apr 11, 2011 9:44 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
That seems impossible that the robot will go forward with the exact same code as above. If you look closely, the only place the above code programs the motors is in the TurnTask function:
Code:
            int turnPower = BOUND((int)(Kp*error), -100, 100);
            motor[motorD] = turnPower;
            motor[motorE] = -turnPower;

You will see that it always programs the two motors in opposit direction. So in theory, the robot will never go straight forward or backward no matter what value turnPower is. It would have required the motors to be programmed with values of the same sign which is impossible in this code. Please check your code carefully. Regardless, you can also try limiting the power to half by changing the turnPower calculation:
Code:
            int turnPower = BOUND((int)(Kp*error), -50, 50);

This will make the robot turned half the max speed (i.e. turn slower). This is in case that the motors turned too fast and saturated the gyro sensor. But as I said, even if this is the case, the robot won't go forward. It will overshoot your target and the integrator would have integrated smaller values than the actual values resulted in stopping at an angle beyond your set target. In other words, no matter what it is, I can't explain what you saw with the above code. Please add a line to display the gyro heading in the TurnTask() function as below. You may need to change the line number (i.e. the "3") to an empty line since nxt would print something else over it.
Code:
void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_tolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int turnPower = BOUND((int)(Kp*error), -50, 50);
            motor[leftMotor] = turnPower;
            motor[rightMotor] = -turnPower;
        }
        else
        {
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

After running the program, take a look at what your program thought the gyro heading was when it stopped.


Last edited by MHTS on Mon Apr 11, 2011 1:55 pm, edited 1 time in total.



Mon Apr 11, 2011 12:28 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Oh, I just noticed something. All your motors have PIDControl turned on in the pragma declaration. You should turn PIDControl off for the driving motors. The code I provided is doing its own PIDControl. So turning on built-in PID control will interfere my PID control code (i.e. the built-in PIDControl and my PIDControl will fight with each other). That may explain what you saw (i.e. the weird behavior was not from my PID code but from the built-in PID control). I am not sure about this though since I don't know how the built-in PID was written.

BTW, can you post the encoder sample code? I don't have it in front of me, so I can't tell you what's wrong when I can't see what it is.


Mon Apr 11, 2011 12:40 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Regarding the encoder code, alternatively, you can use the following instead:
Code:
GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 0.5;             //needs to be tuned
float g_driveTarget = 0.0;
bool g_driveEnable = false;
float g_driveTolerance = 0.5;
float g_driveKp = 1.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -50, 50);
            motor[leftMotor] = turnPower;
            motor[rightMotor] = -turnPower;
        }
        else
        {
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[leftMotor] + nMotorEncoder[rightMotor])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (driveErr > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn.
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = -GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -50, 50);
            motor[leftMotor] = BOUND(drivePower + turnPower, -100, 100);
            motor[rightMotor] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
    }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    SetDriveTarget(48.0);  //go forward 48 inches
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

See how similar the encoder code is comparing to the gyro code. Note, you have to determine the INCHES_PER_CLICK constant. It depends on your wheel size, gear ratio etc. You probably need to fine tune it. With the value fine tuned, you should be able to drive very precisely. The initial approximate value can be calculated by the following formula:
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (24.0/16.0)     //motor:wheel=24:16 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (WHEEL_CIRCUMFERENCE*GEAR_RATIO/CLICKS_PER_REVOLUTION)

Also note that the turn and drive code above is desgined to work by itself but not together. In other words, you can only do SetTurnTarget or SetDriveTarget alone and wait for the turn or drive to complete before setting another target. You may wish to use them together so that the gyro can make sure the drive go straight (or drive a set distance while maintaining the heading towards the IR beacon). But the above code is not sophisticated enough to work together. My library does allow turn and drive to work together, but the code is way more complicated than this. So for now, this should be good enough. If you want to learn how that could be done, I can explain it to you incrementally.


Last edited by MHTS on Tue Apr 12, 2011 8:33 pm, edited 3 times in total.



Mon Apr 11, 2011 1:51 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 133 posts ]  Go to page 1, 2, 3, 4, 5 ... 9  Next

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:  



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