Here’s the code:
#pragma config(Sensor, dgtl1, Encoder, sensorQuadEncoder)
#pragma config(Motor, port1, testMotor, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*----------------------------------------------------------------------------------------------------*\
|* - Measure Torque and Speed with Encoders - *|
|* ROBOTC on VEX 2.0 CORTEX *|
|* *|
|* MOTORS & SENSORS: *|
|* * [Name] [Type] [Description] *|
|* Motor - Port 1 testMotor VEX 3-wire module Motor under test *|
|* Digital - Port 1,2 Encoder VEX Shaft Encoder Angle of weight *|
\*----------------------------------------------------------------------------------------------------*/
int a[200];
task main()
{
wait1Msec(200); // Delay
clearDebugStream();
SensorValue[Encoder] = 0; //Clear Encoders
int bb = nImmediateBatteryLevel;
motor[testMotor] = 127;
for (int i=0; i<160; i++)
{
a* = SensorValue[Encoder]; // Sample Encoder
wait1Msec(100); // at 10 Hz
}
motor[testMotor] = 0;
float mom = 0.390; // Nm
float sumSp=0, sumS2=0, sumTq=0, sumST=0, num=0;
writeDebugStream("sec deg RPM Nm\n");
for (int i=8; i<158; i++)
{
float sp = (a*-a*)*2.083; // RPM
float tq = sinDegrees(a*)*mom; // Nm
writeDebugStream("%3.1f %d %1.0f %5.3f\n", i*0.1, a*, sp, tq);
if (tq>=0) // Linear regression only on positive torques
{
sumSp += sp;
sumS2 += sp*sp;
sumTq += tq;
sumST += sp*tq;
num++;
}
}
writeDebugStream("Batt: %4.2f - %4.2f\n", bb*0.001, nImmediateBatteryLevel*0.001);
if (sumS2==0)
writeDebugStream("No motion!\n");
else
{
float m = (num*sumST-sumSp*sumTq)/(num*sumS2-sumSp*sumSp); // Slope
float b = (sumTq-m*sumSp)/num; // Intercept
writeDebugStream("Free speed: %2.0f RPM\n", -b/m);
writeDebugStream("Load speed: %2.0f RPM at %4.2f Nm\n", (mom-b)/m, mom);
writeDebugStream("Stall torque: %4.2f Nm\n", b);
}
}
First it clears the encoder. It is important to make sure the pendulum is hanging straight down before running, so as to get a good zero angle reference. Then it records the battery voltage and turns on the motor at full power. For 16 seconds it records the angle every tenth of a second, then turns off the motor.
Next it initializes the linear regression sums. It skips the startup readings and analyzes 150 data points. Speed is found at each time step by subtracting the angle at 0.2 seconds in the past from the angle at 0.2 seconds in the future, like a moving average over a 0.4 second period. This gives a speed resolution of about 2 RPM. Torque at each time step is the sine of the angle times the moment. For each step it writes time, angle, speed, and torque to the DebugStream window, and adds torque and speed to the regression sums if torque is positive.
Once all data are in, it calculates slope and y-intercept of the best fit line, y = mx + b . It then writes out the battery voltages before and after the run, the free (no-load) speed, the speed at maximum dyno load, and the extrapolated stall torque. The DebugStream window in RobotC allows you to save all this to a text file.
Please post any questions if something is unclear.
