Skip to content

Instantly share code, notes, and snippets.

@jpearman

jpearman/velocity_demo2.c

Last active January 21, 2016 04:27
Show Gist options
  • Save jpearman/b500523c4cda4c500f52 to your computer and use it in GitHub Desktop.
Save jpearman/b500523c4cda4c500f52 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
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);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment