velocity_demo.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, ,sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor,  port2, motorA, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    Module:     velocity_demo.c                                              */
/*    Author:     James Pearman                                                */
/*    Created:    17 Jan 2016                                                  */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    The author is supplying this software for use with the VEX cortex        */
/*    control system. This file can be freely distributed and teams are        */
/*    authorized to freely use this program , however, it is requested that    */
/*    improvements or additions be shared with the Vex community via the vex   */
/*    forum.  Please acknowledge the work of the author when appropriate.      */
/*    Thanks.                                                                  */
/*                                                                             */
/*    Licensed under the Apache License, Version 2.0 (the "License");          */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*      http://www.apache.org/licenses/LICENSE-2.0                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    distributed under the License is distributed on an "AS IS" BASIS,        */
/*    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/*    See the License for the specific language governing permissions and      */
/*    limitations under the License.                                           */
/*                                                                             */
/*    The author can be contacted on the vex forums as jpearman                */
/*    or electronic mail using jbpearman_at_mac_dot_com                        */
/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate velocity                                             */
/** @param[in]  port the motor port                                            */
/** @param[in]  gear_ratio external gear ratio                                 */
/** @returns    The motor velocity (optionally) multiplied by constant         */
/*-----------------------------------------------------------------------------*/
/*
 * @details
 *  The absolute, simplest way to get motor velocity with ROBOTC V4.52 or
 *  later.  As this functionality was only introduced with the latest version
 *  it would not be available if you have not upgraded yet.
 *
 *  The calculated motor speed is multiplied by any external gear ratio to
 *  obtain the angular velocity of, for example, a flywheel.
 *
 */

float
calculateVelocity( tMotor port, float gear_ratio = 1.0 )
{
    return( gear_ratio * getMotorVelocity( port ) );
}

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate velocity                                             */
/** @param[in]  port the motor port                                            */
/** @param[in]  gear_ratio external gear ratio                                 */
/** @returns    The motor velocity (optionally) multiplied by constant         */
/*-----------------------------------------------------------------------------*/
/*
 * @details
 *  A Different way to calculate velocity.  This method needs to be called at 
 *  a constant rate.  The difference in encoder count over a fixed time interval
 *  is used to calculate velocity.  This function does not filter velocity and
 *  there will be some jitter if multiple IMEs are connected and configured
 *  in ROBOTC.  It works best when there is only one IME.  The function has 
 *  hardcoded motor information (count per revolution).
 *
 *  The calculated motor speed is multiplied by any external gear ratio to
 *  obtain the angular velocity of, for example, a flywheel.
 *
 */

#define SMLIB_TPR_393Turbo     261.333
#define SMLIB_TPR_393Speed     392.0
#define SMLIB_TPR_393Torque    627.2
#define SMLIB_TPR_393Quad      360.0

float
calculateVelocityBetter( tMotor port, float gear_ratio = 1.0 )
{
    static  long  nSysTime_last;
    static  long  encoder_counts_last;
    static  float ticks_per_rev = SMLIB_TPR_393Speed; // fixed, known motor

    int     delta_ms;
    int     delta_enc;
    long    encoder_counts;
    float   motor_velocity;

    // Get current encoder value
    encoder_counts = nMotorEncoder[ port ];

    // This is just used so we don't need to know how often we are called
    // how many mS since we were last here
    delta_ms = nSysTime - nSysTime_last;
    nSysTime_last = nSysTime;

    // Change in encoder count
    delta_enc = (encoder_counts - encoder_counts_last);

    // save last position
    encoder_counts_last = encoder_counts;

    // Calculate velocity in rpm
    motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
    
    // multiply by any gear ratio's being used
    return( motor_velocity * gear_ratio );
}

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate velocity                                             */
/** @param[in]  port the motor port                                            */
/** @param[in]  gear_ratio external gear ratio                                 */
/** @returns    The motor velocity (optionally) multiplied by constant         */
/*-----------------------------------------------------------------------------*/
/*
 * @details
 *  The best way to calculate velocity without the addition of complex filtering.
 *  This function understands which type of motor is configured, uses the 
 *  timestamp that ROBOTC adds to each encoder reading and filters the final
 *  velocity to remove some of the noise.
 *
 *  The calculated motor speed is multiplied by any external gear ratio to
 *  obtain the angular velocity of, for example, a flywheel.
 *
 */
float
calculateVelocityBest( tMotor port, float gear_ratio = 1.0 )
{
    static  long  encoder_time_last = 0;
    static  long  encoder_counts_last;
    static  float ticks_per_rev;

    int     delta_ms;
    int     delta_enc;
    long    encoder_time;
    long    encoder_counts;
    static  float   motor_velocity;
    float   motor_velocity_t;

    // First time is undefined
    // save last values and exit
    if( encoder_time_last == 0 ) {
      // first read to get initial values
      getEncoderAndTimeStamp( port, encoder_counts_last, encoder_time_last );

      // What sort of motor is connected
      // If the type of motor is set we assume it has an IME, otherwise we 
      // revert to a quad encoder.
      if( motorType[ port ] == tmotorVex393_HBridge || motorType[ port ] == tmotorVex393_MC29 )
        ticks_per_rev = SMLIB_TPR_393Torque;
      else
      if( motorType[ port ] == tmotorVex393HighSpeed_HBridge || motorType[ port ] == tmotorVex393HighSpeed_MC29 )
        ticks_per_rev = SMLIB_TPR_393Speed;
      if( motorType[ port ] == tmotorVex393TurboSpeed_HBridge || motorType[ port ] == tmotorVex393TurboSpeed_MC29 )
        ticks_per_rev = SMLIB_TPR_393Turbo;
      else
        ticks_per_rev = SMLIB_TPR_393Quad;
      
      return(0.0);
    }

    // Get current encoder value
    getEncoderAndTimeStamp( port, encoder_counts, encoder_time );

    // calculate the time since the last encoder poll
    delta_ms = encoder_time - encoder_time_last;
    encoder_time_last = encoder_time;

    // Change in encoder count
    delta_enc = (encoder_counts - encoder_counts_last);

    // save last position
    encoder_counts_last = encoder_counts;

    // Calculate velocity in rpm
    motor_velocity_t = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;

    // filter if we are running quickly (< 100mS loop speed)
    if( delta_ms < 100 )
      motor_velocity = (motor_velocity * 0.7) + (motor_velocity_t * 0.3);
    else
      motor_velocity = motor_velocity_t;
    
    // IIR filter means velocity will just keep getting smaller, clip if really low.
    if(motor_velocity < 0.01)
      motor_velocity = 0;

    // multiply by any gear ratio's being used
    return( motor_velocity * gear_ratio );
}

/*-----------------------------------------------------------------------------*/
/*  Test the above functions with a single motor                               */
/*-----------------------------------------------------------------------------*/

// Use globals so we can see them in the debugger
float   rpm_1;    // simple
float   rpm_2;    // better
float   rpm_3;    // best

/*-----------------------------------------------------------------------------*/
/*  A task to call the speed calculation function(s)  Obviously only one or    */
/*  the other would be used in production code, this is just a demo.           */
/*-----------------------------------------------------------------------------*/
task
velocityCalculate()
{
    while(1) {
      // calculate using the different methods
      rpm_1 = calculateVelocity( port2, 1 );
      rpm_2 = calculateVelocityBetter( port2, 1 );
      rpm_3 = calculateVelocityBest( port2, 1 );

      // must have a delay
      wait1Msec(25);
    }
}

task main()
{
    int motor_power;

    // start the velocity calculation task
    startTask( velocityCalculate, 10 );

    // this is effectively driver control
    while(1)
      {
      // run the motor
      // Different speeds set by buttons
      if( vexRT[ Btn8L ] )
          motor_power = 100;
      if( vexRT[ Btn8U ] )
          motor_power = 75;
      if( vexRT[ Btn8R ] )
          motor_power = 50;
      if( vexRT[ Btn8D ] )
          motor_power = 0;

      motor[ port2 ] = motor_power;

      // datalog the results - requires V4.52 or later
      datalogDataGroupStart();
      datalogAddValue( 0, rpm_1 );
      datalogAddValue( 1, rpm_2 );
      datalogAddValue( 2, rpm_3 );
      datalogDataGroupEnd();
      
      wait1Msec(50);
      }
}