Smart software monitor keeps PTC fuses from tripping

VEX V5 General Discussion Technical Discussion
1

https://vexforum.com/attachment.php?attachmentid=6676&stc=1&d=1345278398

This blog post contains a video link to my latest testing with two 393 motors in my Trophy robot and V1.0 RobotC smart PTC monitor software.

So far so good!!

Smart software monitor keeps PTC fuses from tripping

No need to look at the jpg…it is the same as the embedded image.

PTC intro slide.JPG
PTC intro slide.JPG800×600 44.6 KB

2

smart_PTC_monitor beta code release
Download: https://dl.dropbox.com/u/17455717/smart_PTC_monitor_template.beta.8.27.12.c
[edit…please redownload if you grabbed the code before 1:45pm 8/31, I had some critical stuff commented out before then]

Ok, I’ve got the RobotC code used in my smart_PTC_monitor developed to a point where I would let others play with it. I originally used a single timed iterative loop that ran in the same task as autonomous and user modes. I decided to create a separate task that simplifies the user interface so normal programming can occur without any of my constraints. When done, the user functions are easily inserted into my template to seamlessly add the PTC monitor protection capability. The program is self documented with liberal commenting but I am sure people will have a lot of questions. I am putting it out without fully testing all the possible motor configurations to get early user feedback and if it proves to have utility, I will put out a more mature version that includes user feedback.

Please post your comments to the Vex forum thread where the program is introduced. Thanks

Below is a copy of the introduction comments in the program:

//*************************************************************************************************************** // smart_PTC_monitor_template.beta.8.27.2012.c Rev 0

// This program is an open source beta version of a competition template for Sack Attack. It adds a task // called smart_PTC_monitor which uses current and PTC temperature models to manage a current_limiter function // which can keep hardware PTC fuses in the motors,cortex and power expander from ever tripping.

// Program Author: Chris Siegert aka Vamfun on the Vex forums. // see my blog: vamfun.wordpress.com for model details and vex forum threads on subject // email: [email protected] // Mentor for team 599 Robodox and team 1508 Lancer Bots

// 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 my work when appropriate. Thanks

// Scope and limitations: // Program assumes that servo currents are zero. If servo position is known, then currents can be modeled. // Testing has been limited to 393 motors and 3wire motors, however, the program handles 269 motors. // Periodic updates to the models will be made and posted on the Vex forum as more testing is done. // Program handles the Power Expander PTC when the ports are entered with provided #defines // All other motor ports are automatically configured to calculate the correct currents for each PTC

// Basic Instructions: // Write your program as you normally would. When done, put your autonomous code in place of my_autonomous() // and your user code in place of my_user(). Then do a find and replace to change all the “motor” functions to // “motor_input” i.e. motor[Left_motor] –> motor_input[Left_motor].

// Put all your encoder speed calculations into void compute_speeds() function and assign speeds to M[port].speed variable. // You can read these speeds from the global array whenever you need them. This loop runs at about 100ms update rate.

// Use a #define or switch to create a // PTC_monitor_engage boolean and set true to engage the monitor.

// Tell the program which ports are used by the Power Expander as shown in the example below: // #define PE_CH1 UNUSED // if used put port number …. i.e. PE_CH1 port1 else put the #define UNUSED

// #define PE_CH2 Right_drive_motor //use setup names

// #define PE_CH3 port3 //use port name

// #define PE_CH4 1 // use integer = to port number -1. This is motor port2

// Initialize the PTC temperatures using these #defines: // #define T_0_DEG_F (72.) //Set robot ambient temperature here. If you are in a hot gym…this might be 85 deg

// #define T_M (100.) //nominally 100 deg C =Setpoint temperature for the PTC current monitor. This can be lowered if your hardware PTCs are tripping before the monitor current limit is engaged. Or it can be increased to delay current limit engagement if it is occuring too soon.

// Motor currents and PTC temperature data are held in a global state matrix M] which is a structured array that contains // the motor PTC/current data in the first 10 elements (index 0 to 9) and the non motor PTC’s data in the next three elements. // M[10] , M[11] and M[12] pertain to the cortex1_5,cortex6_10 and power expander PTC states respectively.

// Program uses two update rates: The current_limiter updates at PTC_TASK_DELAY_MS which is 15ms // and the temperature calculations are set to run at about 6 times {PTC_TASK_DELAY_MS + subtasks delays) ~= 100ms. //****************************************************************************************************************

3

Great work Chris, I will try it over the long weekend.

One thing on my to do list is to try and gain access to the on board temperature monitor that the cortex has, it may help determine ambient temperature, then again it may not. This was always possible using EasyC and now should be possible with the upcoming release of ROBOTC V3.5 that supports pointers. In both cases it requires some rather special coding but it should not be too difficult.

4

That would be a nice feature.

Please note the edit I just made. I updated the code in the link so you might have to redownload. I know you will have some insightful comments on my attempted timing of the loops.

Thanks

5

Very very impressive. I’m very excited to try it out next week when school starts.

At first glance, it appears that your using the timers T1 and T2, are you using any more?

Also for encoders, I tend to very often reset them to zero. I assume that this would pose a problem for calculating RPM? Would using IME’s that don’t reset still work accurately?

Thanks for sharing your work!

6

Actually only T1 is essential. T2 is just used for a wall clock to see how long it takes for various events to take place. No other timers are used.

Resets can cause erroneous speed spikes which will essentially add noise on top of an already noisy signal. These spikes in turn will cause current spikes which in turn might trigger some PTC trip events , however, I would expect them to be transient in nature and probably not a problem. It all depends on what “very often” means. If you are resetting during autonomous at the start of move segments …no problem. If you are resetting to avoid overflow when an encoder value gets near max…no problem if done occasionally. Not sure why you would do it otherwise.

Although I requested that you put speed calculations in calculate_speed() function it is up to you how and where you compute speeds. It is just essential that you provide motor speeds to the M[motor port].speed variable at faster than around 10 per second. In some testing I did, when I ran the speed loop faster than 10 per second , the speeds seemed to noticibly increase eg…instead of 160 rpm it would bounce around 190 rpm but the debug sampling was too noisy to see what the average was . I didn’t investigate this fully and it remains a TODO. I got good stable speeds at dt_ms reading around 100ms.

IME should be fine.

Hope you have some good feedback for us after you try it out.

7

Oh alright, thanks!

Meaning after every “start of move segments”. Sorry for the confusion.

Oh alright. There seems to be enough example code that I can figure it out when I need to add in more motors/encoders.

8

Hmmm just tried the program with 3.5 beta and the compute speeds function wasn’t working… looks like the
encoder_count_last = encoder_count
statements should be moved to the end of the routine. It is strange, that the version I posted worked with 3.08.

Anyway… please download the program again if you downloaded it before 6 pm 9/1(sat) or just move

left_count_last = left_count;
right_count_last = right_count;
lift_count_last = lift_count;

to the end of compute_speeds() function.

Sorry.

9

Chris

That’s funny, I was just about to post about this, it is as a result of the change to using a stack for local variables. In 3.08 everything was allocated in global memory so, even though left_count etc. are local variables, they would retain their values from call to call. Now they are allocated each time the function is called and will be undefined until something assigned to them. Saving the last count at the end of the function is the right way to go.

James.

10

I played with the code a little today and have a couple of initial comments.

  1. Here is a more generic speed calc function that understands the IME’s as well as the quad encoders. In general, encoder values should be of type long to avoid overflow.
// encoder counts depending on motor
#define TICKS_PER_REV_269  240.448
#define TICKS_PER_REV_393S 392
#define TICKS_PER_REV_393T 627.2
#define TICKS_PER_REV_QUAD 360.0

#define MILLISECS_PER_MIN  60000.0

float
compute_motor_speed( tMotor m, bool ime, int encoderPort )
{
            long  enc;
    static  long  oldenc[kNumbOfTotalMotors];
            float rpm_k;
            float rpm;
            float delta;
    static  bool  init = true;
    
    // first time in init all the static variables
    if(init)
        {
        int  i;
        for(i=0;i<kNumbOfTotalMotors;i++)
            oldenc* = 0;
        init = false;
        }
    
    // are we using an ime ?
    if( ime )
        {
        switch( motorType[m] )
            {
            case    tmotorVex269:
                rpm_k = (MILLISECS_PER_MIN / TICKS_PER_REV_269);
                break;
            case    tmotorVex393:
                rpm_k = (MILLISECS_PER_MIN / TICKS_PER_REV_393T);
                break;
            case    tmotorVex393HighSpeed:
                rpm_k = (MILLISECS_PER_MIN / TICKS_PER_REV_393S);
                break;
            }
        enc = nMotorEncoder[m];
        }
    else
        {
        rpm_k = (MILLISECS_PER_MIN / TICKS_PER_REV_QUAD );
        enc = SensorValue encoderPort ];
        }    
        
    // calculate encoder delta
    delta  = enc-oldenc (short)m ];
    oldenc (short)m ] = enc;

    // calculate the rpm for the motor
    rpm = (delta * rpm_k) / dt_ms;
    
    return( rpm );
}


void compute_speeds()
{
    M[Left_motor].speed = compute_motor_speed( Left_motor, true, 0 );  
}
  1. We probably need a case for an un-encoded motor, currently if the speed is unknown and stays at 0 then temperature rises quickly as the code assumes the motor is stalled. Some motors can take the speed of a motor they may be coupled to, for example, if two motors are used to drive the left wheels then they will probably behave the same, however, some motors may never be encoded, I’m thinking intake motors and perhaps lift motors where a pot is used for position and an encoder not used.*
11

Here is some code that works in ROBOTC V3.45b2 that can access the temperature sensor. I find it rather useless, it measures the STM32 chip temperature and still needs calibration as it’s really only useful for temperature change measurement.

Call InitAdcTemperature() to setup the ADC registers.

Use GetAdcTemperature() to get temperature as a float.

If nothing else it’s a good demo for using pointers for accessing the STM32 registers.

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*        Module:     stm32_adc.c                                              */
/*        Author:     James Pearman                                            */
/*        Created:    2 Sept 2012                                              */
/*                                                                             */
/*        Revisions:  V0.1                                                     */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*        Description:                                                         */
/*                                                                             */
/*        Access the cortex temperature sensor                                 */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */

// change this to unsigned later when supported
typedef long    reg_t;

// ADC register definition
typedef struct
{
    reg_t SR;
    reg_t CR1;
    reg_t CR2;
    reg_t SMPR1;
    reg_t SMPR2;
    reg_t JOFR1;
    reg_t JOFR2;
    reg_t JOFR3;
    reg_t JOFR4;
    reg_t HTR;
    reg_t LTR;
    reg_t SQR1;
    reg_t SQR2;
    reg_t SQR3;
    reg_t JSQR;
    reg_t JDR1;
    reg_t JDR2;
    reg_t JDR3;
    reg_t JDR4;
    reg_t DR;
} ADC_TypeDef;

// memory mapped register space
#define PERIPH_BASE           (0x40000000) /*!< Peripheral base address in the alias region */
#define APB1PERIPH_BASE       PERIPH_BASE
#define APB2PERIPH_BASE       (PERIPH_BASE + 0x10000)
#define AHBPERIPH_BASE        (PERIPH_BASE + 0x20000)
#define ADC_R_BASE            (APB2PERIPH_BASE + 0x2400) /*!< Flash registers base address */
#define ADC1                 ((ADC_TypeDef *) ADC_R_BASE)

//
// Init the temperature sensor
// it's only available on ADC1 :(
// It's difficult to make into a regular channel due to background ROBOTC processing
// so make an injected channel.
//
// use variables due to bug in ROBOTC 3.45b2
// this may get fixed in 3.5
//
void
InitAdcTemperature()
{
    ADC_TypeDef   *p = ADC1;
    long    c1;

    // Set sample time for ADC ch16 & 17
    // temp sensor and Vrefint
    // Sample time is 28uS for V3.45b2, ADC clock is 9MHz, sample time is 239.5+12.5 clocks
    // this is the longest it can be, recommended is 17.1 uS but this is not possible
    c1 = 0x00FC0FFF;
    p->SMPR1 = c1;

    // enable temperature sensor
    p->CR2 |= 0x00800000;

    // sort of wierd - injected seq for temp sensor is backwards
    c1 = 0x00080000;
    p->JSQR = c1;

    // Enable auto injected channels
    p->CR1 |= 0x00000400;
}

// Get RAW ADC value from the temperature sensor

int
GetRawAdcTemperature()
{
    ADC_TypeDef   *p = ADC1;
    long          data;

    data = p->JDR1;

    return( (int)data );
}

// Get converted temperature
// See data sheet - Voltage at 25 deg C need calibration
//

// change these as needed for calibration
//
#define V25         1.40
#define AVG_SLOPE   0.0043

float
GetAdcTemperature()
{
    ADC_TypeDef   *p = ADC1;
    long          data;
    float         temp;
    float         Vsense;

    data = p->JDR1;

    if(data != 0)
        {
        Vsense = data * (3.3/4096.0);

        temp = ((V25 - Vsense) / AVG_SLOPE) + 25.0;
        }
    else
        temp = 0;

    return( temp);
}
12
13

IM trying to use the code but it continues blocking, and when I compile the program, this appears :
250Info*:‘lift’ is written but has no read references
1026Warning:Invalid ‘=’ operation for types ‘object_type’ and ‘word’
1185Warning:Invalid ‘=’ operation for types ‘bool’ and ‘char’
1190Warning:Invalid ‘=’ operation for types ‘bool’ and ‘char’
I dont know if this is the problem

14

The code in this thread was superseded by my smart motor library, see this thread.

https://vexforum.com/t/smart-motor-library/22282/1

Make sure you pickup the latest source (V1.05) from github rather than the (still linked) old versions.
https://github.com/jpearman/smartMotorLib