Diff: brushless_motor.cpp

Revision:

11:bfb73f083009

Parent:

10:e40d8724268a

Child:

12:d1d21a2941ef

--- a/brushless_motor.cpp	Tue Jan 15 09:03:57 2019 +0000
+++ b/brushless_motor.cpp	Sat Jan 19 11:45:01 2019 +0000
@@ -7,44 +7,77 @@
 #include "FastPWM.h"
 #include "Servo.h"
 #include "brushless_motor.h"
-brushless_motor::brushless_motor    (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall)        //  Constructor
+
+
+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
-    maxV = _maxV_;
-    maxI = _maxI_;
+    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;
-//    if  (lutptr != A_tabl && lutptr != B_tabl)
-//        pc.printf   ("Fatal in 'motor' constructor, Invalid lut address\r\n");
     Hall_tab_ptr = 0;
-    Motor_Port = P;
-//    pc.printf   ("In motor constructor, Motor port = %lx\r\n", P);
-    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);
+    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;
-    Hindex[0] = Hindex[1]  = read_Halls    ();
+    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
-    Hall1 = Hall[0];
-    Hall2 = Hall[1];
-    Hall3 = Hall[2];
     PPS     = 0;
     RPM     = 0;
     last_V = last_I = 0.0;
-    int x = read_Halls  ();
-    if  (x == 7)
-        dc_motor = true;
+    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
-        dc_motor = false;
+        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   ()
@@ -71,24 +104,27 @@
 int     brushless_motor::read_Halls  ()
 {
     int x = 0;
-    if  (*Hall1 != 0)    x |= 1;
-    if  (*Hall2 != 0)    x |= 2;
-    if  (*Hall3 != 0)    x |= 4;
+    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 = *Motor_Port;
+    uint16_t    p = OP;
     p &= lut[32];   //  KEEP_L_MASK_A or B
-    *Motor_Port = p;
+//    *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];
+//    *Motor_Port = lut[31];
+    OP = lut[31];
 }
 
 void    brushless_motor::current_calc ()
@@ -113,7 +149,7 @@
 {
     if  (v < 1) v = 1;
     if  (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
-    maxV->pulsewidth_ticks  (v);
+    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
@@ -125,7 +161,7 @@
     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
+    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
@@ -141,7 +177,7 @@
         a = MAX_PWM_TICKS;
     if  (a < 0)
         a = 0;
-    maxI->pulsewidth_ticks  (a);  //  PWM
+    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
@@ -194,30 +230,10 @@
     return  true;
 }
 
-void    brushless_motor::Hall_change  ()
-{
-    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
-    }  ;
-    int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
-    if  (delta_theta == 0)
-        encoder_error_cnt++;
-    else
-        angle_cnt += delta_theta;
-    *Motor_Port = lut[inner_mode | Hindex[0]];  //  changed mode to inner_mode 27/04/18
-    Hall_total++;
-    Hindex[1] = Hindex[0];
-}
 
 void    brushless_motor::motor_set  ()
 {
-    Hindex[0]  = read_Halls    ();
-    *Motor_Port = lut[inner_mode | Hindex[0]];
+    Hall_index[0]  = read_Halls    ();
+//    *Motor_Port = lut[inner_mode | Hindex[0]];
+    OP = lut[inner_mode | Hall_index[0]];
 }