velocity_demo2.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           motorL,        tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           motorR,        tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    Module:     velocity_demo2.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.                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

// These are the same constants we use in the smartmotor library
// System parameters - don't change
#define SMLIB_R_SYS             0.3
#define SMLIB_PWM_FREQ          1150
#define SMLIB_V_DIODE           0.75

// parameters for vex 393 motor
#define SMLIB_I_FREE_393        0.2
#define SMLIB_I_STALL_393       4.8
#define SMLIB_RPM_FREE_393      110
#define SMLIB_R_393             (7.2/SMLIB_I_STALL_393)
#define SMLIB_L_393             0.000650
#define SMLIB_Ke_393            (7.2*(1-SMLIB_I_FREE_393/SMLIB_I_STALL_393)/SMLIB_RPM_FREE_393)
#define SMLIB_I_SAFE393         0.90

// counts per rev for known motor configurations
#define SMLIB_TPR_393Turbo     261.333
#define SMLIB_TPR_393Speed     392.0
#define SMLIB_TPR_393Torque    627.2
#define SMLIB_TPR_393Quad      360.0

// Structure to gather all the motor related data
typedef struct _motor_controller {
    tMotor          motor;                  ///< The motor port

    float           ticks_per_rev;          ///< IME ticks per rev
    tSensors        encoder_id;             ///< motor mush have an encoder

    // variables related to velocity measurement
    long            e_count;                ///< new encoder count
    long            e_count_last;           ///< previous encoder count
    long            e_timestamp;            ///< new encoder timestamp
    long            e_timestamp_last;       ///< last encoder timestamp
    long            v_current;              ///< last calculated velocity

    // variables used for current calculation
    float           i_free;                 ///< free current for motor
    float           i_stall;                ///< stall current for motor
    float           r_motor;                ///< resistance of motor
    float           l_motor;                ///< inductance of motor
    float           ke_motor;               ///< back emf constant
    float           rpm_free;               ///< free speed of motor with no load
    float           v_bemf_max;             ///< maximum back emf motor will generate
    float           current;                ///< instantaneous calculated current
    float           filtered_current;       ///< average current
    float           peak_current;           ///< peak current
    } motor_controller;

/*-----------------------------------------------------------------------------*/
/** @brief      Initialize motor controller structure                          */
/** @param[in]  m pointer to the motor controller structure                    */
/** @param[in]  port the port we are initializing                              */
/*-----------------------------------------------------------------------------*/
void
motorControllerInitialize( motor_controller *m, tMotor port )
{
    // bounds check port
    if( (port < port1) || (port > port10) )
      port = port1;

    // save motor port
    m->motor = port;

    // clear out variables
    m->e_count          = 0;
    m->e_count_last     = 0;
    m->e_timestamp      = 0;
    m->e_timestamp_last = 0;

    // Setup everything for current measurement
    m->i_free   = SMLIB_I_FREE_393;
    m->i_stall  = SMLIB_I_STALL_393;
    m->r_motor  = SMLIB_R_393;
    m->l_motor  = SMLIB_L_393;

    // Figure out what type of encoder we have
    m->encoder_id = getEncoderForMotor( port );
    if( m->encoder_id == 0 )
      return;

    // IME is attached
    // setup motor configuration specific constants
    switch( motorType[ m->motor ] )
      {
      // 393 set for high torque
      case    tmotorVex393_HBridge:
      case    tmotorVex393_MC29:
        m->ke_motor = SMLIB_Ke_393;
        m->rpm_free = SMLIB_RPM_FREE_393;

        if( m->encoder_id >= I2C_1 )
          m->ticks_per_rev = SMLIB_TPR_393Torque;
        else
          m->ticks_per_rev = SMLIB_TPR_393Quad;
        break;

      // 393 set for high speed
      case    tmotorVex393HighSpeed_HBridge:
      case    tmotorVex393HighSpeed_MC29:
        m->ke_motor = SMLIB_Ke_393/1.6;
        m->rpm_free = SMLIB_RPM_FREE_393 * 1.6;

        if( m->encoder_id >= I2C_1 )
          m->ticks_per_rev = SMLIB_TPR_393Speed;
        else
          m->ticks_per_rev = SMLIB_TPR_393Quad;
        break;

      // 393 set for Turbo
      case    tmotorVex393TurboSpeed_HBridge:
      case    tmotorVex393TurboSpeed_MC29:
        m->ke_motor = SMLIB_Ke_393/2.4;
        m->rpm_free = SMLIB_RPM_FREE_393 * 2.4;

        if( m->encoder_id >= I2C_1 )
          m->ticks_per_rev = SMLIB_TPR_393Turbo;
        else
          m->ticks_per_rev = SMLIB_TPR_393Quad;
        break;

      default:
        // Set unknown
        m->rpm_free      = 0;
        m->ticks_per_rev = SMLIB_TPR_393Quad;
        break;
      }

    // maximum theoretical v_bemf
    m->v_bemf_max = m->ke_motor * m->rpm_free;
}

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate velocity                                             */
/** @param[in]  m pointer to the motor controller structure                    */
/** @returns    The motor velocity                                             */
/*-----------------------------------------------------------------------------*/
float
calculateVelocity( motor_controller *m  )
{
    int     delta_ms;
    int     delta_enc;
    float   motor_velocity_t;

    // Must have an encoder
    if( m->encoder_id == (tSensors)(-1) )
      return(0);

    // First time is undefined
    // save last values and exit
    if( m->e_timestamp_last == 0 ) {
      // first read to get initial values
      getEncoderAndTimeStamp( m->motor, m->e_count_last, m->e_timestamp_last );
      // no calculation this time
      return(0.0);
    }

    // Get current encoder value
    getEncoderAndTimeStamp( m->motor, m->e_count, m->e_timestamp );

    // calculate the change in time since the last encoder poll
    delta_ms = m->e_timestamp - m->e_timestamp_last;
    m->e_timestamp_last = m->e_timestamp;

    // no change in time, then return result from last time
    // we don't want a divide by zero
    if(delta_ms == 0)
      return( m->v_current );

    // calculate the change in encoder count since the last poll
    delta_enc = (m->e_count - m->e_count_last);
    m->e_count_last = m->e_count;

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

    // filter if we are running quickly (< 100mS loop speed)
    if( delta_ms < 100 )
      m->v_current = (m->v_current * 0.7) + (motor_velocity_t * 0.3);
    else
      m->v_current = motor_velocity_t;

    // IIR filter means velocity will just keep getting smaller, clip if really low.
    if(fabs(m->v_current) < 0.01)
      m->v_current = 0;

    // return velocity
    return( m->v_current );
}

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate motor current                                        */
/** @param[in]  m pointer to the motor controller structure                    */
/** @param[in]  v_battery the battery voltage                                  */
/** @returns    instantaneous current for the motor                            */
/*-----------------------------------------------------------------------------*/
float
calculateCurrent( motor_controller *m, float v_battery  )
{
    float   v_bemf;
    float   c1, c2;
    float   lamda;

    float   duty_on, duty_off;

    float   i_max, i_bar, i_0;
    float   i_ss_on, i_ss_off;

    int     dir;

    // get motor cmd
    int     cmd = motor[ m->motor ];

    // no current measurement if free rpm not set
    if( m->rpm_free == 0 )
      return(0);

    // rescale control value
    // ports 2 through 9 behave a little differently
    if( m->motor > port1 && m->motor < port10 )
        cmd = (cmd * 128) / 90;

    // clip control value to +/- 127 (not really necessary with this version)
    if( abs(cmd) > 127 )
        cmd = sgn(cmd) * 127;

    // which way are we turning ?
    // modified to use rpm near command value of 0 to reduce transients
    if( abs(cmd) > 10 )
        dir = sgn(cmd);
    else
        dir = sgn(m->v_current);


    duty_on = abs(cmd)/127.0;

    // constants for this pwm cycle
    lamda = m->r_motor/((float)SMLIB_PWM_FREQ * m->l_motor);
    c1    = exp( -lamda *    duty_on  );
    c2    = exp( -lamda * (1-duty_on) );

    // Calculate back emf voltage
    v_bemf  = m->ke_motor * m->v_current;

    // new - clip v_bemf, stops issues if motor runs faster than rpm_free
    if( fabs(v_bemf) > m->v_bemf_max )
        v_bemf = sgn(v_bemf) * m->v_bemf_max;

    // Calculate steady state current for on and off pwm phases
    i_ss_on  =  ( v_battery * dir - v_bemf ) / (m->r_motor + SMLIB_R_SYS);
    i_ss_off = -( SMLIB_V_DIODE   * dir + v_bemf ) /  m->r_motor;

    // compute trial i_0
    i_0 = (i_ss_on*(1-c1)*c2 + i_ss_off*(1-c2))/(1-c1*c2);

    //check to see if i_0 crosses 0 during off phase if diode were not in circuit
    if(i_0*dir < 0)
        {
        // waveform reaches zero during off phase of pwm cycle hence
        // ON phase will start at 0
        // once waveform reaches 0, diode clamps the current to zero.
        i_0 = 0;

        // peak current
        i_max = i_ss_on*(1-c1);

        //where does the zero crossing occur
        duty_off = -log(-i_ss_off/(i_max-i_ss_off))/lamda ;
        }
    else
        {
        // peak current
        i_max = i_0*c1 + i_ss_on*(1-c1);

        // i_0 is non zero so final value of waveform must occur at end of cycle
        duty_off = 1 - duty_on;
        }

    // Average current for cycle
    i_bar = i_ss_on*duty_on + i_ss_off*duty_off;

    // Save current
    m->current = i_bar;

    // simple iir filter to remove transients
    m->filtered_current = (m->filtered_current * 0.8) + (i_bar * 0.2);

    // peak current - probably not useful
    if( fabs(m->current) > m->peak_current )
        m->peak_current = fabs(m->current);

    return i_bar;
}

/*-----------------------------------------------------------------------------*/
/*  Test the above functions with two motors                                   */
/*-----------------------------------------------------------------------------*/

// Use globals so we can see them in the debugger
float   rpm_L;    // final output
float   rpm_R;    // final output

// Left motor
motor_controller  fwmL;
// Right motor
motor_controller  fwmR;

/*-----------------------------------------------------------------------------*/
/*  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
motorMonitoringTask()
{
    while(1) {
      // calculate velocity
      rpm_L = calculateVelocity( &fwmL );
      rpm_R = calculateVelocity( &fwmR );

      // calculate motor current
      calculateCurrent( &fwmL, nImmediateBatteryLevel/1000.0 );
      calculateCurrent( &fwmR, nImmediateBatteryLevel/1000.0 );

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

task main()
{
    int motor_power = 0;
    char  str[32];

    // init the motor control structures
    motorControllerInitialize( &fwmL, motorL );
    motorControllerInitialize( &fwmR, motorR );

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

    // setup LCD
    bLCDBacklight = true;
    clearLCDLine(0);
    clearLCDLine(1);

    // 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[ motorL ] = motor_power;
      motor[ motorR ] = motor_power;

      // display on the LCD speed and motor current
      sprintf( str, "S:%4d I:%5.2f", fwmL.v_current, fwmL.filtered_current );
      displayLCDString(0, 0, str);

      sprintf( str, "S:%4d I:%5.2f", fwmR.v_current, fwmR.filtered_current );
      displayLCDString(1, 0, str);

      // datalog the results - requires V4.52 or later
      datalogDataGroupStart();
      datalogAddValue( 0, rpm_L );
      datalogAddValue( 1, rpm_R );
      datalogDataGroupEnd();

      wait1Msec(50);
      }
}