Skip to content

Commit

Permalink
Added support for SIN/COS position sensors
Browse files Browse the repository at this point in the history 
Added option to reverse direction signals
No longer using velocity tracker in AB(Z) mode
Fixed "turns"
  • Loading branch information
@jsphuebner
jsphuebner committed Dec 5, 2018
1 parent 1db8c60 commit c3e7098
Show file tree
Hide file tree
Showing 6 changed files with 137 additions and 88 deletions.
4 changes: 2 additions & 2 deletions include/project/inc_encoder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,14 @@ class Encoder
public:
enum mode
{
SINGLE, AB, ABZ, SPI, RESOLVER, INVALID
SINGLE, AB, ABZ, SPI, RESOLVER, SINCOS, INVALID
};

static void Reset();
static void SetMode(enum mode encMode);
static bool SeenNorthSignal();
static void UpdateRotorAngle(int dir);
static void UpdateRotorFrequency();
static void UpdateRotorFrequency(int callingFrequency);
static void SetPwmFrequency(uint32_t frq);
static uint16_t GetRotorAngle();
static uint32_t GetSpeed();
Expand Down
18 changes: 12 additions & 6 deletions include/project/param_prj.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,12 @@
#define SNS_M "12=KTY83-110, 13=KTY84-130, 14=Leaf"
#define PWMFUNCS "0=tmpm, 1=tmphs, 2=speed, 3=speedfrq"
#define BTNSWITCH "0=Button, 1=Switch"
#define DIRMODES "0=Button, 1=Switch, 2=ButtonReversed, 3=SwitchReversed"
#define IDLEMODS "0=always, 1=nobrake, 2=cruise"
#define ONOFF "0=Off, 1=On, 2=na"
#define OKERR "0=Error, 1=Ok, 2=na"
#define CHARGEMODS "0=Off, 3=Boost, 4=Buck"
#define ENCMODES "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver"
#define ENCMODES "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver, 5=SinCos"
#define POTMODES "0=SingleRegen, 1=DualChannel, 2=CAN"
#define CANSPEEDS "0=250k, 1=500k, 2=800k, 3=1M"
#define CANIOS "1=Cruise, 2=Start, 4=Brake, 8=Fwd, 16=Rev, 32=Bms"
Expand All @@ -48,7 +49,7 @@
#define CAT_CHARGER "Charger"
#define CAT_COMM "Communication"

#define VER 4.78
#define VER 4.87
#define VERCEIL VER + 0.009

enum _modes
Expand All @@ -70,6 +71,13 @@ enum _tripmodes
TRIP_PRECHARGEON
};

enum _dirmodes
{
DIR_BUTTON = 0,
DIR_SWITCH = 1,
DIR_REVERSED = 2, //used as a flag
};

enum _canio
{
CAN_IO_CRUISE = 1,
Expand Down Expand Up @@ -113,14 +121,12 @@ enum _canio
PARAM_ENTRY(CAT_MOTOR, fslipmax, "Hz", 0, 10, 3, 33 ) \
PARAM_ENTRY(CAT_MOTOR, polepairs, "", 1, 16, 2, 32 ) \
PARAM_ENTRY(CAT_MOTOR, respolepairs,"", 1, 16, 1, 93 ) \
PARAM_ENTRY(CAT_MOTOR, enckp, "", 0, 40, 20, 6 ) \
PARAM_ENTRY(CAT_MOTOR, encki, "", 0, 16000, 400, 94 ) \
PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 4, 0, 75 ) \
PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 5, 0, 75 ) \
PARAM_ENTRY(CAT_MOTOR, fmin, "Hz", 0, 400, 1, 34 ) \
PARAM_ENTRY(CAT_MOTOR, fmax, "Hz", 0, 1000, 200, 9 ) \
PARAM_ENTRY(CAT_MOTOR, numimp, "ppr", 8, 8192, 60, 15 ) \
PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 2000, 100, 87 ) \
PARAM_ENTRY(CAT_MOTOR, dirmode, BTNSWITCH, 0, 1, 1, 95 ) \
PARAM_ENTRY(CAT_MOTOR, dirmode, DIRMODES, 0, 3, 1, 95 ) \
PARAM_ENTRY(CAT_MOTOR, syncofs, "dig", 0, 65535, 0, 70 ) \
PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 14, 12, 46 ) \
PARAM_ENTRY(CAT_INVERTER,pwmfrq, PWMFRQS, 0, 4, 1, 13 ) \
Expand Down
159 changes: 107 additions & 52 deletions src/project/inc_encoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,11 @@ static void InitTimerSingleChannelMode();
static void InitTimerABZMode();
static void InitSPIMode();
static void InitResolverMode();
static u32fp TrackFrequency(uint16_t angle);
static void DMASetup();
static uint16_t GetAngleSPI();
static uint16_t GetAngleResolver();
static uint16_t GetAngleSinCos();
static int GetPulseTimeFiltered();
static void GetMinMaxTime(uint32_t& min, uint32_t& max);

Expand All @@ -56,6 +58,7 @@ static u32fp lastFrequency = 0;
static bool ignore = true;
static enum Encoder::mode encMode = Encoder::INVALID;
static bool seenNorthSignal = false;
static uint32_t turnsSinceLastSample = 0;

void Encoder::Reset()
{
Expand Down Expand Up @@ -92,6 +95,7 @@ void Encoder::SetMode(Encoder::mode mode)
InitSPIMode();
break;
case RESOLVER:
case SINCOS:
InitResolverMode();
break;
default:
Expand Down Expand Up @@ -133,6 +137,7 @@ void Encoder::UpdateRotorAngle(int dir)
int16_t numPulses;
uint32_t timeSinceLastPulse;
uint16_t interpolatedAngle;
uint16_t angleDiff;

switch (encMode)
{
Expand All @@ -142,6 +147,9 @@ void Encoder::UpdateRotorAngle(int dir)
cntVal *= TWO_PI;
cntVal /= pulsesPerTurn * 4;
angle = (uint16_t)cntVal;
angleDiff = ((TIM_CR1(REV_CNT_TIMER) & TIM_CR1_DIR_DOWN) ?
(lastAngle - angle) : (angle - lastAngle)) & 0xFFFF;
turnsSinceLastSample += angleDiff;
break;
case SINGLE:
numPulses = GetPulseTimeFiltered();
Expand All @@ -151,44 +159,48 @@ void Encoder::UpdateRotorAngle(int dir)
angle = accumulatedAngle + dir * interpolatedAngle;
break;
case SPI:
angle = GetAngleSPI(); //Gets 12-bit
angle <<= 4;
angle = GetAngleSPI();
lastFrequency = TrackFrequency(angle);
break;
case RESOLVER:
angle = GetAngleResolver();
lastFrequency = TrackFrequency(angle);
break;
case SINCOS:
angle = GetAngleSinCos();
lastFrequency = TrackFrequency(angle);
break;
default:
break;
}

if (lastAngle <= (TWO_PI / 2) && angle > (TWO_PI / 2))
{
poleCounter--;
if (poleCounter == 0)
{
fullTurns++;
poleCounter = Param::GetInt(Param::respolepairs);
}
else
{
poleCounter--;
}
}

lastAngle = angle;
}

/** Update rotor frequency.
* @ref https://www.embeddedrelated.com/showarticle/530.php
* @param callingFrequency Frequency at which this function is called in Hz
*/
void Encoder::UpdateRotorFrequency()
void Encoder::UpdateRotorFrequency(int callingFrequency)
{
static int velest = 0, velint = 0;
static uint16_t posest = 0;
const int kp = Param::GetInt(Param::enckp);
const int ki = Param::GetInt(Param::encki);

posest += velest / (int)pwmFrq;
int16_t poserr = angle - posest;
velint += (poserr * ki) / (int)pwmFrq;
velest = poserr * kp + velint;
lastFrequency = ABS(velint) / FP_TOINT(TWO_PI);
if ((encMode == AB) || (encMode == ABZ))
{
//65536 is one turn
lastFrequency = (callingFrequency * turnsSinceLastSample) / FP_TOINT(TWO_PI);
turnsSinceLastSample = 0;
}
}

/** Returns current angle of motor shaft to some arbitrary 0-axis
Expand Down Expand Up @@ -223,7 +235,7 @@ uint32_t Encoder::GetSpeed()
if (ignore) return 0;
return 60000000 / (lastPulseTimespan * pulsesPerTurn);
}
else if (encMode == RESOLVER)
else if (encMode == RESOLVER || encMode == SINCOS)
{
return FP_TOINT(60 * lastFrequency) / Param::GetInt(Param::respolepairs);
}
Expand All @@ -241,7 +253,24 @@ uint32_t Encoder::GetFullTurns()

bool Encoder::IsSyncMode()
{
return encMode == Encoder::ABZ || encMode == Encoder::SPI || encMode == Encoder::RESOLVER;
return encMode == ABZ || encMode == SPI || encMode == RESOLVER || encMode == SINCOS;
}

/** Track rotor frequency.
* @ref https://www.embeddedrelated.com/showarticle/530.php
*/
static u32fp TrackFrequency(uint16_t angle)
{
static int velest = 0, velint = 0;
static uint16_t posest = 0;
const int kp = 20;
const int ki = 400;

posest += velest / (int)pwmFrq;
int16_t poserr = angle - posest;
velint += (poserr * ki) / (int)pwmFrq;
velest = poserr * kp + velint;
return ABS(velint) / FP_TOINT(TWO_PI);
}

static void DMASetup()
Expand Down Expand Up @@ -289,7 +318,7 @@ static void InitTimerSingleChannelMode()
timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG);
timer_enable_counter(REV_CNT_TIMER);
gpio_set_mode(GPIOD, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO2);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO7);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6 | GPIO7);
DMASetup();
exti_disable_request(EXTI2);
}
Expand All @@ -300,17 +329,18 @@ static void InitSPIMode()
exti_disable_request(EXTI2);
gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2);
gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO7);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6);
seenNorthSignal = true;
}

static void InitTimerABZMode()
{
rcc_periph_reset_pulse(REV_CNT_TIMRST);
timer_set_period(REV_CNT_TIMER, 4 * pulsesPerTurn); //2 channels and 2 edges -> 4 times the number of base pulses
timer_set_period(REV_CNT_TIMER, 4 * pulsesPerTurn); //2 channels and 2 edges -> 4 times the number of base pulses

//In sync mode start out in reset mode and switch to encoder
//mode once the north marker has been detected
if (encMode == Encoder::ABZ)
//In sync mode start out in reset mode and switch to encoder
//mode once the north marker has been detected
if (encMode == Encoder::ABZ)
{
exti_select_source(EXTI2, GPIOD);
exti_set_trigger(EXTI2, EXTI_TRIGGER_RISING);
Expand All @@ -322,15 +352,15 @@ static void InitTimerABZMode()
}

timer_slave_set_mode(REV_CNT_TIMER, TIM_SMCR_SMS_EM3); // encoder mode
timer_ic_set_input(REV_CNT_TIMER, TIM_IC1, TIM_IC_IN_TI1);
timer_ic_set_input(REV_CNT_TIMER, TIM_IC2, TIM_IC_IN_TI2);
timer_ic_set_filter(REV_CNT_TIMER, TIM_IC1, TIM_IC_DTF_DIV_32_N_8);
timer_ic_set_filter(REV_CNT_TIMER, TIM_IC2, TIM_IC_DTF_DIV_32_N_8);
timer_ic_set_input(REV_CNT_TIMER, TIM_IC1, TIM_IC_IN_TI1);
timer_ic_set_input(REV_CNT_TIMER, TIM_IC2, TIM_IC_IN_TI2);
timer_ic_set_filter(REV_CNT_TIMER, TIM_IC1, TIM_IC_DTF_DIV_32_N_8);
timer_ic_set_filter(REV_CNT_TIMER, TIM_IC2, TIM_IC_DTF_DIV_32_N_8);
timer_ic_enable(REV_CNT_TIMER, TIM_IC1);
timer_ic_enable(REV_CNT_TIMER, TIM_IC2);
timer_enable_counter(REV_CNT_TIMER);
timer_enable_counter(REV_CNT_TIMER);
gpio_set_mode(GPIOD, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO2);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO7);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO6 | GPIO7);
seenNorthSignal = false;
}

Expand All @@ -342,33 +372,44 @@ static void InitResolverMode()
adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_JSWSTART);
adc_set_sample_time(ADC1, 6, ADC_SMPR_SMP_1DOT5CYC);
adc_set_sample_time(ADC1, 7, ADC_SMPR_SMP_1DOT5CYC);
rcc_periph_reset_pulse(REV_CNT_TIMRST);
timer_set_prescaler(REV_CNT_TIMER, 71); //run at 1MHz
timer_set_period(REV_CNT_TIMER, 65535);
timer_one_shot_mode(REV_CNT_TIMER);
timer_set_oc_mode(REV_CNT_TIMER, TIM_OC4, TIM_OCM_PWM2);
timer_enable_oc_output(REV_CNT_TIMER, TIM_OC4); //OC4 is connected to ADC trigger
timer_enable_preload(REV_CNT_TIMER);
timer_direction_up(REV_CNT_TIMER);
timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG);

gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO6);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO7);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, GPIO6 | GPIO7);
exti_disable_request(EXTI2);

adc_start_conversion_injected(ADC1);
if (encMode == Encoder::RESOLVER)
{
rcc_periph_reset_pulse(REV_CNT_TIMRST);
timer_set_prescaler(REV_CNT_TIMER, 71); //run at 1MHz
timer_set_period(REV_CNT_TIMER, 65535);
timer_one_shot_mode(REV_CNT_TIMER);
timer_set_oc_mode(REV_CNT_TIMER, TIM_OC4, TIM_OCM_PWM2);
timer_enable_oc_output(REV_CNT_TIMER, TIM_OC4); //OC4 is connected to ADC trigger
timer_enable_preload(REV_CNT_TIMER);
timer_direction_up(REV_CNT_TIMER);
timer_generate_event(REV_CNT_TIMER, TIM_EGR_UG);
gpio_set_mode(GPIOD, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO2);

while (!adc_eoc_injected(ADC1));
adc_start_conversion_injected(ADC1); //Determine offset

while (!adc_eoc_injected(ADC1));

int sin = adc_read_injected(ADC1, 2);
int cos = adc_read_injected(ADC1, 3);
adc_set_injected_offset(ADC1, 2, sin);
adc_set_injected_offset(ADC1, 3, cos);
adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_TIM3_CC4);
}
else //SINCOS
{
//Offset assumed 3.3V/2
adc_set_injected_offset(ADC1, 2, 2048);
adc_set_injected_offset(ADC1, 3, 2048);
}

int sin = adc_read_injected(ADC1, 2);
int cos = adc_read_injected(ADC1, 3);
adc_set_injected_offset(ADC1, 2, sin);
adc_set_injected_offset(ADC1, 3, cos);
adc_enable_external_trigger_injected(ADC1, ADC_CR2_JEXTSEL_TIM3_CC4);
seenNorthSignal = true;
}

/** Gets angle from an AD2S chip */
static uint16_t GetAngleSPI()
{
uint32_t d = 0;
Expand All @@ -385,10 +426,12 @@ static uint16_t GetAngleSPI()
GPIO_BSRR(GPIOD) |= GPIO2; //Read high
d >>= 10; //6 because of GPIO6, 4 because of encoder format
//Encoder format is ANGLE[11:0], RDVEL, Parity, DOS, LOT
return d;
return d << 4; //we want 16-bit representation
}

/** Calculates current angle and velocity from resolver feedback
* and generates square wave that is filtered
* into a sine wave for resolver excitation
*
* For different PWM frequencies you need to populate different
* resistor values on the 3-pole filter.
Expand All @@ -407,25 +450,37 @@ static uint16_t GetAngleResolver()
timer_set_oc_value(REV_CNT_TIMER, TIM_OC4, resolverSampleDelay);
timer_set_counter(REV_CNT_TIMER, 0);
timer_enable_counter(REV_CNT_TIMER);
state = false;
}
else
{
gpio_set(GPIOD, GPIO2);
int sin = adc_read_injected(ADC1, 2);
int cos = adc_read_injected(ADC1, 3);
angle = SineCore::Atan2(cos, sin);
state = true;
}

state = !state;

return angle;
}

/** Calculates angle from a Hall sin/cos encoder like MLX90380 */
static uint16_t GetAngleSinCos()
{
int sin = adc_read_injected(ADC1, 2);
int cos = adc_read_injected(ADC1, 3);
uint16_t calcAngle = SineCore::Atan2(cos, sin);

adc_start_conversion_injected(ADC1);

return calcAngle;
}

extern "C" void exti2_isr(void)
{
exti_reset_request(EXTI2);
exti_reset_request(EXTI2);
timer_set_counter(REV_CNT_TIMER, 0);
seenNorthSignal = true;
seenNorthSignal = true;
}

static int GetPulseTimeFiltered()
Expand Down

0 comments on commit c3e7098

Please sign in to comment.