File content as of revision 11:bfb73f083009:

//  Cloned from 'DualBLS2018_06' on 23 November 2018
#include "mbed.h"
//#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
#include    "stm32f401xe.h"
#include "DualBLS.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"
#include "brushless_motor.h"


brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
    const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask) 
    : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask)      //  Constructor
{
    //  Constructor
    OP = 0;
    H1.mode (PullUp);
    H2.mode (PullUp);
    H3.mode (PullUp);
    H1.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H1.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    H2.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H2.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    H3.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
    H3.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
    Hall_total = 0;  //  mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
    latest_pulses_per_sec = 0;
    for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
        edge_count_table[i] = 0;
    Hall_tab_ptr = 0;
    maxV.period_ticks      (MAX_PWM_TICKS + 1);  //  around 18 kHz
    maxI.period_ticks      (MAX_PWM_TICKS + 1);
    maxV.pulsewidth_ticks  (MAX_PWM_TICKS / 20);
    maxI.pulsewidth_ticks  (MAX_PWM_TICKS / 30);
    visible_mode    = REGENBRAKE;
    inner_mode      = REGENBRAKE;
    lut = lutptr;
    Hall_index[0] = Hall_index[1]  = read_Halls    ();
    ppstmp  = 0;
    cs_ptr = 0;
    tickleon    = 0;
    direction   = 0;
    angle_cnt   = 0;        //  Incremented or decremented on each Hall event according to actual measured direction of travel
    encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
    PPS     = 0;
    RPM     = 0;
    last_V = last_I = 0.0;
    dc_motor = (Hall_index[0] == 7) ? true : false    ;
}

void    brushless_motor::sniff_current   ()  {
    current_samples[cs_ptr++] = Motor_I.read_u16    (); //
    if  (cs_ptr >= CURRENT_SAMPLES_AVERAGED)
        cs_ptr = 0;
}

void    brushless_motor::Hall_change   ()  {
    int32_t     delta_theta;
    const   int32_t delta_theta_lut[] = {   //  Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 0
        0, 0, 0,-1, 0, 1, 0, 0,     //  Previous Hindex was 1
        0, 0, 0, 1, 0, 0,-1, 0,     //  Previous Hindex was 2
        0, 1,-1, 0, 0, 0, 0, 0,     //  Previous Hindex was 3
        0, 0, 0, 0, 0,-1, 1, 0,     //  Previous Hindex was 4
        0,-1, 0, 0, 1, 0, 0, 0,     //  Previous Hindex was 5
        0, 0, 1, 0,-1, 0, 0, 0,     //  Previous Hindex was 6
        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 7
    }  ;

    Hall_index[0]   = read_Halls    ();
    delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];

    if  (delta_theta == 0)
        encoder_error_cnt++;
    else
        angle_cnt += delta_theta;
    OP = lut[inner_mode | Hall_index[0]];  //  changed mode to inner_mode 27/04/18
    Hall_total++;
    Hall_index[1] = Hall_index[0];
}

bool    brushless_motor::motor_is_brushless   ()
{
    /*    int x = read_Halls  ();
        if  (x < 1 || x > 6)
            return  false;
        return  true;
        */
    return  !dc_motor;
}

/**
void    brushless_motor::direction_set   (int dir)  {
Used to set direction according to mode data from eeprom
*/
void    brushless_motor::direction_set   (int dir)
{
    if  (dir != 0)
        dir = FORWARD | REVERSE;  //  bits used in eor
    direction = dir;
}

int     brushless_motor::read_Halls  ()
{
    int x = 0;
    if  (H1 != 0)    x |= 1;
    if  (H2 != 0)    x |= 2;
    if  (H3 != 0)    x |= 4;
    return  x;
}

void    brushless_motor::high_side_off   ()
{
//    uint16_t    p = *Motor_Port;
    uint16_t    p = OP;
    p &= lut[32];   //  KEEP_L_MASK_A or B
//    *Motor_Port = p;
    OP = p;
}

void    brushless_motor::low_side_on   ()
{
//    uint16_t    p = *Motor_Port;
//    p &= lut[31];   //  KEEP_L_MASK_A or B
//    *Motor_Port = lut[31];
    OP = lut[31];
}

void    brushless_motor::current_calc ()
{
    I.min = 0x0fffffff; //  samples are 16 bit
    I.max = 0;
    I.ave = 0;
    uint16_t    sample;
    for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++)  {
        sample  = current_samples[i];
        I.ave += sample;
        if  (I.min > sample)
            I.min   = sample;
        if  (I.max < sample)
            I.max   = sample;
    }
    I.ave /= CURRENT_SAMPLES_AVERAGED;
}


void    brushless_motor::raw_V_pwm    (int    v)
{
    if  (v < 1) v = 1;
    if  (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
    maxV.pulsewidth_ticks  (v);
}

void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
{
    if  (p < 0.0)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_V = p;     //  for read by diagnostics
    p *= 0.95;   //  need limit, ffi see MCP1630 data
    p   = 1.0 - p;  //  because pwm is wrong way up
    maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
}

void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
{
    int a;
    if  (p < 0.0)
        p = 0.0;
    if  (p > 1.0)
        p = 1.0;
    last_I = p;
    a = (int)(p * MAX_PWM_TICKS);
    if  (a > MAX_PWM_TICKS)
        a = MAX_PWM_TICKS;
    if  (a < 0)
        a = 0;
    maxI.pulsewidth_ticks  (a);  //  PWM
}

uint32_t    brushless_motor::pulses_per_sec   ()       //  call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
{
    //  Can also test for motor running or not here
    if  (dc_motor)
        return  0;
    if  (ppstmp == Hall_total)  {
//    if  (dc_motor || ppstmp == Hall_total)  {
        moving_flag  = false;       //  Zero Hall transitions since previous call - motor not moving
        tickleon    = TICKLE_TIMES;
    } else    {
        moving_flag  = true;
        ppstmp = Hall_total;
    }
    latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
    edge_count_table[Hall_tab_ptr] = ppstmp;
    Hall_tab_ptr++;
    if  (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
        Hall_tab_ptr = 0;
    PPS = latest_pulses_per_sec;
    RPM = (latest_pulses_per_sec * 60) / 24;
    return  latest_pulses_per_sec;
}

bool    brushless_motor::is_moving ()
{
    return  moving_flag;
}

/**
bool    brushless_motor::set_mode (int m)
Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
If this causes change of mode, also sets V and I to zero.
*/
bool    brushless_motor::set_mode (int m)
{
    if  ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE))  {
//        pc.printf   ("Error in set_mode, invalid mode %d\r\n", m);
        return  false;
    }
    if  (visible_mode != m) {   //  Mode change, kill volts and amps to be safe
        set_V_limit (0.0);
        set_I_limit (0.0);
        visible_mode = m;
    }
    if  (m == FORWARD || m == REVERSE)
        m ^= direction;
    inner_mode = m;     //  idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
    return  true;
}


void    brushless_motor::motor_set  ()
{
    Hall_index[0]  = read_Halls    ();
//    *Motor_Port = lut[inner_mode | Hindex[0]];
    OP = lut[inner_mode | Hall_index[0]];
}