127 lines
2.7 KiB
C
127 lines
2.7 KiB
C
/**** Includes ****/
|
|
#include "coil.h"
|
|
|
|
/**** Private definitions ****/
|
|
|
|
/**** Private constants ****/
|
|
|
|
/**** Private variables ****/
|
|
static uint16_t lock_current = 4500; //mA
|
|
static uint16_t min_current = 100; //mA
|
|
static uint16_t target_current = 0; //mA
|
|
static uint8_t new_target = 1;
|
|
|
|
static uint16_t adj_target_current = 0; //mA
|
|
static uint16_t target_voltage = 0; //mV
|
|
|
|
static uint8_t act_force = 0;
|
|
|
|
static uint16_t nominal_resitance = 1500; //mR
|
|
|
|
static int32_t sum_current = 0;
|
|
static uint8_t sum_cnt = 0;
|
|
|
|
|
|
/**** Private function declarations ****/
|
|
static uint16_t UpdateVoltage(uint16_t cur, uint16_t res);
|
|
|
|
/**** Public function definitions ****/
|
|
void Coil_SetLockCurrent(uint16_t lock_i)
|
|
{
|
|
lock_current = lock_i;
|
|
}
|
|
|
|
void Coil_SetTarget_Force(uint8_t force)
|
|
{
|
|
// Check if worth doing
|
|
if(force == act_force) return;
|
|
|
|
// Calculate new target current
|
|
act_force = force;
|
|
|
|
// Check simple answer
|
|
uint16_t new_current = 0;
|
|
if(force==0) new_current = 0;
|
|
else if(force >= 100) new_current = lock_current;
|
|
else
|
|
{
|
|
uint32_t temp = (uint32_t)force * lock_current;
|
|
temp /= 100;
|
|
|
|
if(temp > 0x0000FFFF) new_current = lock_current;
|
|
else new_current = (uint16_t) temp;
|
|
}
|
|
|
|
// Update new target
|
|
Coil_SetTarget_Current(new_current);
|
|
}
|
|
|
|
void Coil_SetTarget_Current(uint16_t current)
|
|
{
|
|
if(current >= lock_current) current = lock_current;
|
|
else if((current > 0)&&(current <= min_current)) current = min_current;
|
|
|
|
if(current != target_current) new_target = 1;
|
|
target_current = current;
|
|
}
|
|
|
|
uint16_t Coil_GetTargetVolatge(void)
|
|
{
|
|
return target_voltage;
|
|
}
|
|
|
|
uint16_t Coil_Update(analog_t* meas)
|
|
{
|
|
// Collect average current
|
|
sum_current += meas->hb_currnet;
|
|
sum_cnt++;
|
|
|
|
// Update measurement
|
|
if(sum_cnt >= 10)
|
|
{
|
|
// Calculate average
|
|
sum_current /= sum_cnt;
|
|
|
|
// Calculate error
|
|
int32_t error = sum_current - (int32_t)target_current;
|
|
int32_t temp = (int32_t)adj_target_current - error;
|
|
|
|
// Limit to 16bits
|
|
if(temp<0) adj_target_current = 0;
|
|
else if(temp > 0x0000FFFF) adj_target_current = 0xFFFF;
|
|
else adj_target_current = (uint16_t)temp;
|
|
sum_cnt = 0;
|
|
sum_current = 0;
|
|
};
|
|
|
|
// Closed loop or open loop target set
|
|
if(new_target)
|
|
{
|
|
// Set open-loop HB output
|
|
target_voltage = UpdateVoltage(target_current, nominal_resitance);
|
|
adj_target_current = target_current;
|
|
new_target = 0;
|
|
sum_cnt = 0;
|
|
sum_current = 0;
|
|
}
|
|
else
|
|
{
|
|
target_voltage = UpdateVoltage(adj_target_current, nominal_resitance);
|
|
}
|
|
|
|
return target_voltage;
|
|
}
|
|
|
|
/**** Private function definitions ****/
|
|
static uint16_t UpdateVoltage(uint16_t cur, uint16_t res)
|
|
{
|
|
// Update settable voltage
|
|
if(cur==0) return 0;
|
|
else
|
|
{
|
|
uint32_t volt = (uint32_t)cur * res;
|
|
volt /= 1000;
|
|
if(volt > 0x0000FFFF) return 0xFFFF;
|
|
else return (uint16_t)volt;
|
|
}
|
|
} |