There has been some recent testing that has been looking at both the speed-torque curves for the 269 and 393 motors and also the control value to actual motor speeds when the motor is free running. It occurred to me that there is enough information contained in this data to estimate the current a motor is using. Having estimated the motor average current, decisions could be made as to when the motor and/or cortex PTCs may trip and evasive actions taken.
The code I’m going to post is a work in progress, it gives a pretty good value but is not 100% accurate and needs more tweaking. I though it would still be worth posting as perhaps someone else could also work on this and I’m sure improve on it.
The basic idea is as follows.
We know the control value that is sent to the motor, it creates a pwm drive signal that is proportional and therefore allows calculation of the average voltage sent to the motor.
We can measure the speed of the motor using an IME (or other encoder directly coupled to it). We know what the free running speed of the motor should be based on the control value sent so can therefore calculate the percentage of the free run speed we are at.
The motor will generate a back emf based on it’s speed, the back emf is an opposing voltage to the drive voltage and tends to cancel it out, this why as the motor is slowed down the motor current increases and along with that the torque.
The code estimates current by first estimating the back emf based on the measured motor speed and subtracting that from the drive voltage to create a net voltage. This is divided by a theoretical motor resistance to estimate current.
This code has been tested with a 269 motor and is accurate to within perhaps 20%, not great but if the idea is to know that you are over the 1.5A limit then it’s probably good enough. As I said above, it’s a work in progress and I’m sure could be improved.
This code below is for a 269 motor on a 3-wire port (port9) driven by a motor controller 29. I have only tested one motor and have not had time to test on a 2 wire port. This is a ROBOTC example but the same principle could be applied in EasyC.
#pragma config(UART_Usage, UART1, VEX_2x16_LCD, baudRate19200, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, motorA_enc, sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port9, motorA, tmotorVex269, openLoop, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*-----------------------------------------------------------------------------*/
/* */
/* LUT for control value to expected rpm for a 269 motor in a 2 wire port */
/* */
/*-----------------------------------------------------------------------------*/
const unsigned char SpeedLut[128] =
{
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 5, 9, 13, 17, 20, 24,
27, 30, 33, 36, 39, 42, 44, 47, 49, 52, 54, 56, 59, 61, 63, 64,
66, 68, 70, 71, 73, 74, 76, 77, 79, 80, 81, 82, 83, 84, 85, 86,
87, 88, 89, 90, 91, 92, 92, 93, 94, 94, 95, 95, 96, 96, 97, 97,
98, 98, 99, 99, 99,100,100,100,101,101,101,102,102,102,103,103,
103,103,104,104,104,104,105,105,105,105,105,106,106,106,106,107,
107,107,107,108,108,108,108,108,109,109,109,109,110,110,110,110,
111,111,111,111,111,112,112,112,112,112,113,113,113,113,113,114
};
/*-----------------------------------------------------------------------------*/
// being lazy and using globals
float motor_rpm;
float motor_current;
int requested_speed;
/*-----------------------------------------------------------------------------*/
/* */
/* Try and estimate the motor current based on the current control value, */
/* the expected motor speed with no load and the actual motor speed. */
/* */
/* This is a work in progress and gives a value for current that is in the */
/* ballpark but is not exactly correct. The idea here is to get an idea of */
/* when a PTC may trip. */
/* */
/*-----------------------------------------------------------------------------*/
float
CalcCurrent269( tMotor m, int speed, int rpm )
{
int control_speed;
int expected_rpm;
float battery_voltage;
float pwm_time;
float drive_voltage;
float bemf_voltage;
float net_voltage;
float motor_current;
// Get battery voltage in volts
battery_voltage = nAvgBatteryLevel/1000.0;
// calculate the time that the motor is being driven by the pwm signal
// This varies from 0 to 1.0, for example, 0.5 would mean 50% duty cycle
// and would occur when the speed was 64.
pwm_time = (float)abs(speed) / 127.0;
// calculate the average voltage driving the motor
drive_voltage = pwm_time * battery_voltage;
// rescale control value
// ports 2 through 9 behave a little differently
if( m > port1 && m < port10 )
control_speed = abs(speed * 128 / 100);
else
control_speed = abs(speed);
// limit to 127
if( control_speed > 127)
control_speed = 127;
// get the expected motor speed based on the control_value
expected_rpm = SpeedLut control_speed ];
// calculate the back emf (possible)
bemf_voltage = (float)abs(rpm) / (float)expected_rpm * drive_voltage;
// bemf voltage cannot be less than 0
if( bemf_voltage < 0 )
bemf_voltage = 0;
// calculate a possible net voltage accross the motor
net_voltage = drive_voltage - bemf_voltage;
// calculate the current assuming motor resistance is 3 ohms
// this gives about 2.5A at 7.5v using the max control value
// with the motor stalled
motor_current = (net_voltage / 3);
return( motor_current );
}
/*-----------------------------------------------------------------------------*/
/* */
/* A task to monitor the motor IME, calculate the current motor speed and */
/* the motor current. */
/* */
/*-----------------------------------------------------------------------------*/
task calcSpeed()
{
static int delayTimeMs = 500;
long enc;
long oldenc;
float delta;
while(1)
{
// Get encoder value
enc = nMotorEncoder[motorA];
// calculate encoder delta
delta = enc-oldenc;
oldenc = enc;
// calculate the rpm for a 269 motor
motor_rpm = (1000.0/delayTimeMs) * delta * 60.0 / 240.448;
// calculate current for a 269 motor
motor_current = CalcCurrent269( motorA, requested_speed, motor_rpm );
// wait
wait1Msec(delayTimeMs);
}
}
/*-----------------------------------------------------------------------------*/
/* */
/* Main task, control one motor with the joystick and display program */
/* variables. */
/* */
/*-----------------------------------------------------------------------------*/
task main()
{
string str;
// I like the LCD backlight on
bLCDBacklight = true;
// start the encoder/current calculation task
StartTask( calcSpeed );
// clear LCD
clearLCDLine(0);
clearLCDLine(1);
// run forever
while(1)
{
// get channel 1 joystick value
requested_speed = vexRT[Ch1];
// Set out motor
motor[motorA] = requested_speed;
// show control value and measured rpm
sprintf(str, "%4d - %6.2f ", requested_speed, motor_rpm);
displayLCDString(0, 0, str);
// display estimated current
sprintf(str, "I = %6.2fA", motor_current );
displayLCDString(1, 0, str);
// don't hog cpu
wait1Msec(50);
}
}
