View unanswered posts | View active topics It is currently Tue Sep 01, 2015 2:08 pm






Reply to topic  [ 37 posts ]  Go to page 1, 2, 3  Next
Autonomous Turning using HiTechnic Gyro 
Author Message
Rookie

Joined: Mon Dec 15, 2014 12:46 am
Posts: 19
Post Autonomous Turning using HiTechnic Gyro
Hello all,
I am a new programmer and have been working on a program to turn 90 degrees using a HiTechnic Gyro Sensor. This is for the Autonomous section of the FTC Cascade Effect competition. I based my program off of MHTS's Gyro program I found on the form, but am having a problem I don't understand. On line 74 this error is displayed:

**Error**:Expression does not fit parameter. Call to 'GyroInit'. Parameter: 'short sensorID' is 'GYRO' of type 'GYRO'.

This error only displayed after I changed the pragmas to match my teams robot. The other programs such as gyro.h and the Xander drive are fine. The entire program is displayed below. I'll also include the gyro.h and driver code.

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IRSeekerLeft,   sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     IRSeekerRight,  sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     reaperMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     launcherMotor, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLeft,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorRight,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     skyRiseMotor,  tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorL,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    grapplerServo,        tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    reaperEjection,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\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 (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[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

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()
{
    int step = 0;

    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        switch (step)
        {
           // case 0:
                //step 0: go forward 48 inches.//
                //SetTurnTarget(48.0);//
                //step++;//
               // break;//
                     case 0:
                     //step 0: turn 90 degrees
                     SetTurnTarget(90.0);
                     step++;
                     break;


        }

        TurnTask();
        //DriveTask();//
        wait1Msec(10);
    }
}


Code:
#if 0

#endif

#ifndef _GYRO_H
#define _GYRO_H

#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-gyro.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


Code:
/*!@addtogroup HiTechnic
 * @{
 * @defgroup htgyro Gyroscopic Sensor
 * HiTechnic Gyroscopic Sensor
 * @{
 */

/*
 * $Id: hitechnic-gyro.h 133 2013-03-10 15:15:38Z xander $
 */

#ifndef __HTGYRO_H__
#define __HTGYRO_H__
/** \file hitechnic-gyro.h
 * \brief HiTechnic Gyroscopic Sensor driver
 *
 * hitechnic-gyro.h provides an API for the HiTechnic Gyroscopic Sensor.
 *
 * Changelog:
 * - 0.1: Initial release
 * - 0.2: Renamed HTGYROgetCalibration to HTGYROreadCal<br>
 *        Renamed HTGYROsetCalibration to HTGYROsetCal<br>
 *        Renamed HTGYROcalibrate to HTGYROstartCal<br>
 *        Added SMUX functions
 * - 0.3: Removed some of the functions requiring SPORT/MPORT macros
 * - 0.4: Removed "NW - No Wait" functions\n
 *        Replaced array structs with typedefs\n
 *
 * Credits:
 * - Big thanks to HiTechnic for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where its due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER.
 * \author Xander Soldaat (xander_at_botbench.com)
 * \date 20 February 2011
 * \version 0.4
 * \example hitechnic-gyro-test1.c
 * \example hitechnic-gyro-test2.c
 * \example hitechnic-gyro-SMUX-test1.c
 */

#pragma systemFile

#ifndef __COMMON_H__
#include "common.h"
#endif

float HTGYROreadRot(tSensors link);
float HTGYROstartCal(tSensors link);
float HTGYROreadCal(tSensors link);
// void HTGYROsetCal(tSensors link, int offset);

#ifdef __HTSMUX_SUPPORT__
float HTGYROreadRot(tMUXSensor muxsensor);
float HTGYROstartCal(tMUXSensor muxsensor);
float HTGYROreadCal(tMUXSensor muxsensor);
void HTGYROsetCal(tMUXSensor muxsensor, int offset);
#endif // __HTSMUX_SUPPORT__

float HTGYRO_offsets[][] = {{620.0, 620.0, 620.0, 620.0}, /*!< Array for offset values.  Default is 620 */
                          {620.0, 620.0, 620.0, 620.0},
                          {620.0, 620.0, 620.0, 620.0},
                          {620.0, 620.0, 620.0, 620.0}};

/**
 * Read the value of the gyro
 * @param link the HTGYRO port number
 * @return the value of the gyro
 */
float HTGYROreadRot(tSensors link) {
  // Make sure the sensor is configured as type sensorRawValue
  if (SensorType[link] != sensorAnalogInactive) {
    SetSensorType(link, sensorAnalogInactive);
    wait1Msec(100);
  }

  return (SensorValue[link] - HTGYRO_offsets[link][0]);
}


/**
 * Read the value of the gyro
 * @param muxsensor the SMUX sensor port number
 * @return the value of the gyro
 */
#ifdef __HTSMUX_SUPPORT__
float HTGYROreadRot(tMUXSensor muxsensor) {
  return HTSMUXreadAnalogue(muxsensor) - HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)];
}
#endif // __HTSMUX_SUPPORT__


/**
 * Calibrate the gyro by calculating the average offset of 5 raw readings.
 * @param link the HTGYRO port number
 * @return the new offset value for the gyro
 */
float HTGYROstartCal(tSensors link) {
  long _avgdata = 0;

  // Make sure the sensor is configured as type sensorRawValue
  if (SensorType[link] != sensorAnalogInactive) {
    SetSensorType(link, sensorAnalogInactive);
    wait1Msec(100);
  }

  // Take 50 readings and average them out
  for (int i = 0; i < 50; i++) {
    _avgdata += SensorValue[link];
    wait1Msec(5);
  }

  // Store new offset
  HTGYRO_offsets[link][0] = (_avgdata / 50.0);

  // Return new offset value
  return HTGYRO_offsets[link][0];
}


/**
 * Calibrate the gyro by calculating the average offset of 50 raw readings.
 * @param muxsensor the SMUX sensor port number
 * @return the new offset value for the gyro
 */
#ifdef __HTSMUX_SUPPORT__
float HTGYROstartCal(tMUXSensor muxsensor) {
  long _avgdata = 0;

  // Take 5 readings and average them out
  for (int i = 0; i < 50; i++) {
    _avgdata += HTSMUXreadAnalogue(muxsensor);
    wait1Msec(50);
  }

  // Store new offset
  HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)] = (_avgdata / 50.0);

  // Return new offset value
  return HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)];
}
#endif // __HTSMUX_SUPPORT__


/**
 * Override the current offset for the gyro manually
 * @param link the HTGYRO port number
 * @param offset the new offset to be used
 */
//#define HTGYROsetCal(link, offset) HTGYRO_offsets[link][0] = offset
void HTGYROsetCal(tSensors link, int offset) {
  HTGYRO_offsets[link][0] = offset;
}


/**
 * Override the current offset for the gyro manually
 * @param muxsensor the SMUX sensor port number
 * @param offset the new offset to be used
 */
#ifdef __HTSMUX_SUPPORT__
//#define HTGYROsetCal(muxsensor, offset) HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)] = offset
void HTGYROsetCal(tMUXSensor muxsensor, int offset) {
  HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)] = offset;
}
#endif // __HTSMUX_SUPPORT__


/**
 * Retrieve the current offset for the gyro
 * @param link the HTGYRO port number
 * @return the offset value for the gyro
 */
float HTGYROreadCal(tSensors link) {
  return HTGYRO_offsets[link][0];
}


/**
 * Retrieve the current offset for the gyro
 * @param muxsensor the SMUX sensor port number
 * @return the offset value for the gyro
 */
#ifdef __HTSMUX_SUPPORT__
float HTGYROreadCal(tMUXSensor muxsensor) {
  return HTGYRO_offsets[SPORT(muxsensor)][MPORT(muxsensor)];
}
#endif // __HTSMUX_SUPPORT__

#endif // __HTGYRO_H__

/*
 * $Id: hitechnic-gyro.h 133 2013-03-10 15:15:38Z xander $
 */
/* @} */
/* @} */

_________________
FTC 7104 - The Synergists


Mon Jan 05, 2015 2:07 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
You have a few bugs in your code. The main one is of course in the GyroInit call. The second parameter is GyroInit is your sensor ID which is gyroSensor, not the structure type GYRO.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IRSeekerLeft,   sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     IRSeekerRight,  sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     reaperMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     launcherMotor, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLeft,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorRight,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     skyRiseMotor,  tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorL,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    grapplerServo,        tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    reaperEjection,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\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 = 1.0;  //MHTS: 1 degree tolerance should be good enough
float Kp = 5.0;           //MHTS: 0.5 is way too low (need 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)   //MHTS: You need to compare to absolute value of error
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int turnPower = BOUND((int)(Kp*error), -50, 50);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

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); //MHTS: The initialization here is wrong, use the line below instead.
  GyroInit(g_Gyro, gyroSensor);

  return;
}

task main()
{
    int step = 0;

    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        switch (step)
        {
           // case 0:
                //step 0: go forward 48 inches.//
                //SetTurnTarget(48.0);//
                //step++;//
               // break;//
                     case 0:
                     //step 0: turn 90 degrees
                     SetTurnTarget(90.0);
                     step++;
                     break;
             // MHTS: You need a case 1 to wait for the completion of the turn
            case 1:
                if (!g_turnEnabled)
                {
                    // MHTS: turn is complete, move to the next step.
                    step++;
                }
                break;

            default:
                // MHTS: We are done, do nothing.
                break;
        }

        TurnTask();
        //DriveTask();//
        wait1Msec(10);
    }
}


Mon Jan 05, 2015 5:18 pm
Profile
Rookie

Joined: Mon Dec 15, 2014 12:46 am
Posts: 19
Post Re: Autonomous Turning using HiTechnic Gyro
Thank you MHTS for your quick reply. We will integrate this at the next meeting.

Another question, earlier today another team member created the following gyro function to do exactly the same end goal as the previous code above which is integrating drivers including yours. Here is the code my teammate wrote:

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IRSeekerLeft,   sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     IRSeekerRight,  sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     reaperMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     launcherMotor, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLeft,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorRight,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     skyRiseMotor,  tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorL,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    grapplerServo,        tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    reaperEjection,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//turning
task main()
{
   // take initial reading from gyro
   int initialReading = SensorValue[gyroSensor];
   int degreesNeeded = 90; //positive = right, Negative = Left
   float degreesSoFar = 0;
   // turn

   //Motors A and B turn in different directions
   motor[motorLeft] = 75*sgn(degreesNeeded);
   motor[motorRight] = -75*sgn(degreesNeeded);
   //check if turned enough
   while (abs(degreesSoFar) < abs(degreesNeeded)) //change if wanting to turn different way
   {
      wait1Msec(10);
      int currentGyroReading = SensorValue[gyroSensor] - initialReading; //tells speed of turning
      //dinstance = rate x time
      degreesSoFar = degreesSoFar + currentGyroReading*.01;
   }
   //stop
   motor[motorRight] = 0;
   motor[motorLeft] = 0;
}


This worked as expected, turning the robot 90 degrees and subsequent tests with different values worked as expected. My question is why should we use all of the complicated code associated with the drivers when something simple works?

We have not tested integrating straight movement and turns along with sub-routines so I do not know if that will change the turn behavior of the simple code. However, my guess as to why the complicated code is better is that the drivers calibrate the sensor, reset the heading, integrate the turn rate and heading and create a dead zone. With your experience, do you see any problems in the future if we stick with the simple code above? Thanks.

_________________
FTC 7104 - The Synergists


Tue Jan 06, 2015 1:36 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
I am glad you asked the question. Instead of taking the code as is, you are curious on why the code is designed that way and what benefits does the code offer?
If your teammate's code does the job, there is no reason why you need to use more complicated code. Having said that, since you asked about the differences, I will list some here.
First, the simpler code you have assumes a fixed integration loop time of 10 msec. It is approximately correct in your case just because you have minimal code in your integration loop. If you are going to put more code in the integration loop, it may take much longer than 10 msec to execute. That will affect the accuracy of the integration. It will be even worse if the code you will add to the integration loop has indeterministic execution time (i.e. the loop time varies depending on different situations). The integration task in the gyro.h module is agnostic to loop time. It uses timestamps to determine the sampling interval for integration. Having said that, if you are not going to add any more code in your integration loop, it's probably fine.
Secondly, the simpler code is turning the robot at 75% power whereas the more complicated code is doing PID controlled turn. PID controlled turn means it will slow down when it gets closer to the target heading whereas the simpler code will always overshoot.
Finally, one of the main strengths of the more complicated code is the ability to multi-task using a state machine. This allows your code to do much more sophisticated autonomous. Take an example of this year's game, our team's robot can raise the elevator while running down the ramp and capturing the rolling goal. Then it dumps the balls into the goal. After that it lowers the elevator while at the same time turning toward the parking zone and dragging the rolling goal there. Then it will turn towards the kickstand and try to knock it down. All these operations must be done in 30 seconds. If the robot is doing all these sequentially, it will never be able to finish within 30 seconds. Therefore, we will overlap operations whenever we can. This means the robot loop must do several things at the same time. Having said that if you are not going to worry about multi-tasking, then you don't need to worry about using a state machine. Here is a simplified code that still takes advantage of the gyro module and PID controlled turn but not doing any multi-tasking:
Code:
#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_tolerance = 1.0;  //MHTS: 1 degree tolerance should be good enough
float Kp = 5.0;           //MHTS: 0.5 is way too low (need to be tuned)

void Turn(float degrees)
{
    // Calculate target heading.
    float target = GyroGetHeading(g_Gyro) + degrees;
    while (true)
    {
        // Integrate for gyro heading.
        GyroTask(g_Gyro);
        float error = target - GyroGetHeading(g_Gyro);
        if (abs(error) > g_tolerance)
        {
            // Not reaching target yet, calculate the motor turn power.
            int turnPower = BOUND((int)(Kp*error), -50, 50);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            // Reached target, stop motors and quit.
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            break;
        }
        wait1Msec(10);
    }
}

task main()
{
    GyroInit(g_Gyro, gyroSensor);
    Turn(90.0);
    Turn(-90.0);
}


Tue Jan 06, 2015 5:54 am
Profile
Rookie

Joined: Mon Dec 15, 2014 12:46 am
Posts: 19
Post Re: Autonomous Turning using HiTechnic Gyro
Ok. That makes sense. I would like to multi-task as well, but I'll see what the team thinks. Thanks again!

_________________
FTC 7104 - The Synergists


Tue Jan 06, 2015 1:01 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
BTW, this is what multi-tasking can do for our autonomous routines.
https://www.youtube.com/watch?v=nBSTOM7rEDg
https://www.youtube.com/watch?v=9YlX7rq9Gxg
https://www.youtube.com/watch?v=ystuHF5e85s
https://www.youtube.com/watch?v=R3-2054IrQs


Thu Jan 08, 2015 3:30 am
Profile
Rookie

Joined: Mon Dec 15, 2014 12:46 am
Posts: 19
Post Re: Autonomous Turning using HiTechnic Gyro
It worked! Thanks MHTS! One of our new programmers was working on the turn so he was very excited once it did the 90* turn! Also, thank you for making your drivers available, that helped save the beginner programmer some time and gave me some code to learn from an expert. Thanks again!

_________________
FTC 7104 - The Synergists


Sat Jan 10, 2015 12:42 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
Synergists wrote:
It worked! Thanks MHTS! One of our new programmers was working on the turn so he was very excited once it did the 90* turn! Also, thank you for making your drivers available, that helped save the beginner programmer some time and gave me some code to learn from an expert. Thanks again!

What worked? I thought you already said that the simpler code your teammate wrote was working.


Sun Jan 11, 2015 6:03 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Autonomous Turning using HiTechnic Gyro
Sorry for branching a little off topic, but MHTS what do you use to be so accurate when you deposit the balls into the center goal?

_________________
Thanks!


Sun Jan 11, 2015 1:06 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
akash329dsouza wrote:
Sorry for branching a little off topic, but MHTS what do you use to be so accurate when you deposit the balls into the center goal?

Did you read this thread?
viewtopic.php?f=52&t=2780&p=16146&hilit=IR+seeker+inaccuracy#p16146


Sun Jan 11, 2015 2:56 pm
Profile
Rookie

Joined: Mon Dec 15, 2014 12:46 am
Posts: 19
Post Re: Autonomous Turning using HiTechnic Gyro
Sorry, for not being clear... Yes, the simple code worked, but your code also worked with the changes! (I am the head programmer overseeing the other two, the beginner programmer and other programmer who wrote the simple code)...I wanted to use your code since it was already integrating the turn rate, initializing the sensor and doing all of that higher level code stuff. So anyways, thanks!

_________________
FTC 7104 - The Synergists


Sun Jan 11, 2015 6:18 pm
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Autonomous Turning using HiTechnic Gyro
So from what I get from the video and what you sent
Is that you have 2 ir seekers and you are comparing signal strength in between them until they are at the same signal strength each which, in theory would mean the robot is lined up? If I'm correct how exactly did you do this? Did you use a PID loop?

_________________
Thanks!


Mon Jan 12, 2015 12:27 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
Yes, we used PID everywhere but no, we don't directly compare the strength of the two IR seekers. We had them pointing to each other and add their acDir values together. Our IR seeker library module will interpolate the acDir values in between zones by reading the individual strengths so it is a float between 8.0 and 12.0. So 10.0 is directly ahead and we use it as our "target value" in our PID control. If the PID constant is tuned correctly, it could be quite accurate.


Mon Jan 12, 2015 12:52 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Autonomous Turning using HiTechnic Gyro
If your willing this is still pretty confusing. I'll comment the stuff I get in the code and can you fill in the blanks?
Code:
   
Code:
float IRGetACDir(tSensors link)
{
    static float prevDir = 0.0; //Create variables
    float currDir;
    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);  //Read acDir and store in idx
    currDir = (float)idx; //Store idx as currDir
    if (idx == 0)// if idx = 0 currDir is = to prevDir so robot does not veer
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4])) //Don't get all this part can you explain the logic?
    {
        idx = (idx - 1)/2;
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
        {
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
        nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
        nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
        nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
        nxtDisplayTextLine(4, "S5=%d", acS[4]);
    }
    prevDir = currDir;

    return currDir;
}

_________________
Thanks!


Tue Jan 13, 2015 12:25 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1521
Post Re: Autonomous Turning using HiTechnic Gyro
The function IRGetACDir is intended to be similar to the function HTIRS2readACDir except that instead of returning an integer between zone 1 and 9, it returns a float so you could have more precise number such as 4.3 and 5.9 etc (not just 4 or 5). The way it does it is to interpolate between two raw sensor strength values. Inside the IR seeker, there are 5 sensors (0 through 4) arranged in a fan orientation. The firmware in the IR seeker reads the signal strength of all 5 sensors and compare them. If there is strong signal in sensor 0, it returns zone 1. If there is strong signal in sensor 1, it returns zone 3. If there are signals from both sensor 0 and 1, it returns zone 2. In the IRGetACDir function, we called HTIRS2readACDir to get the zone value (idx). From the zone value, we can determine which of the 5 sensors have signals. For example, if we get zone 4, it means we get signal from sensor 1 and 2. Then we can read the raw strength of sensor 1 and 2. By the proportion of the signal strength of these two adjacent sensors, we can interpolate whether the source is closer to sensor 1 or sensor 2. If sensor 1 and sensor 2 have equal strength, we return zone 4 just like HTIRS2readACDir but if strength of sensor 1 is higher than sensor 2, we probably will return 3.x. If strength of sensor 2 is higher than sensor 1, we will return 4.x. The value .x is proportional to the difference of the signal strength between sensor 1 and 2.
akash329dsouza wrote:
If your willing this is still pretty confusing. I'll comment the stuff I get in the code and can you fill in the blanks?
Code:
   
Code:
float IRGetACDir(tSensors link)
{
    static float prevDir = 0.0; //Create variables
    float currDir;
    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);  //Read current zone and store in idx
    currDir = (float)idx; //Store current zone as a float instead of an integer
    if (idx == 0)// if HTIRS2readACDir returns 0, it means it lost the IR beacon. In that case we use the ACDir determined previously.
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4])) //Read the strength values of all 5 sensors.
    {
        idx = (idx - 1)/2; //translate the zone value into the sensor number 0 to 4. For example, zone 4 yielded idx 1.
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0)) //if sensor number and sensor number + 1 are still within 0 through 4 and if sensor number and sensor number + 1 both have non-zero strength
        {
            // calculate the .x proportion value from the relative strength of the two adjacent sensors. The result could be a positive or a negative number smaller than 1.0. Then add this .x number to the original zone number from HTIRS2readACDir.
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
        nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
        nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
        nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
        nxtDisplayTextLine(4, "S5=%d", acS[4]);
    }
    prevDir = currDir; // remember the current zone value in case the next time we lost the IR beacon.

    return currDir;
}


Tue Jan 13, 2015 2:03 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 37 posts ]  Go to page 1, 2, 3  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.