89 lines
2.1 KiB
C
89 lines
2.1 KiB
C
#include "PowerControl.h"
|
|
#include "stdint.h"
|
|
#include "math.h"
|
|
|
|
|
|
void power_calu(power_model_t* param,float* in_array,float* rpm_array)
|
|
{
|
|
float InitialGivePower[param->motor_num];
|
|
float totalpower=0;
|
|
float in[4];
|
|
float ve[4];
|
|
for (uint8_t i = 0; i < param->motor_num; i++) // first get all the initial motor power and total motor power
|
|
{
|
|
in[i] = in_array[i];
|
|
ve[i] = rpm_array[i];
|
|
InitialGivePower[i] = in[i] * param->toque_coefficient * ve[i] +
|
|
param->k2 * ve[i] * ve[i]+ param->k1 * in[i]*in[i] + param->constant;
|
|
|
|
param->power[i] = InitialGivePower[i];
|
|
if (InitialGivePower[i] < 0) // negative power not included (transitory)
|
|
continue;
|
|
totalpower += InitialGivePower[i];
|
|
}
|
|
param->total_power = totalpower;
|
|
}
|
|
float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power)
|
|
{
|
|
float total_power=0;
|
|
for(uint8_t i=0;i<num;i++)
|
|
{
|
|
total_power+=param_array[i]->total_power;
|
|
}
|
|
if(total_power>max_power)
|
|
{return max_power/total_power;}
|
|
else
|
|
{return 2;}
|
|
}
|
|
|
|
void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array)
|
|
{
|
|
float in[4];
|
|
float ve[4];
|
|
if(scale<1)
|
|
{
|
|
|
|
float ScaledGivePower[4];
|
|
for (uint8_t i = 0; i < param->motor_num; i++)
|
|
{
|
|
in[i] = in_array[i];
|
|
ve[i] = rpm_array[i];
|
|
ScaledGivePower[i] = param->power[i] * scale; // get scaled power
|
|
if (ScaledGivePower[i] < 0)
|
|
{
|
|
continue;
|
|
}
|
|
float b = param->toque_coefficient * ve[i];
|
|
float c = param->k2 * ve[i]*ve[i] - ScaledGivePower[i] + param->constant;
|
|
float inside = b * b - 4 * param->k1 * c;
|
|
if (inside < 0)
|
|
{
|
|
continue;
|
|
}
|
|
else if (in[i] > 0) // Selection of the calculation formula according to the direction of the original moment
|
|
{
|
|
float temp = (-b + sqrt(inside)) / (2 * param->k1);
|
|
if (temp > 16000)
|
|
{
|
|
out_array[i] = 16000;
|
|
}
|
|
else
|
|
out_array[i] = temp;
|
|
}
|
|
else
|
|
{
|
|
float temp = (-b - sqrt(inside)) / (2 * param->k1);
|
|
if (temp < -16000)
|
|
{
|
|
out_array[i] = -16000;
|
|
}
|
|
else
|
|
out_array[i] = temp;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|