314 lines
9.7 KiB
C
314 lines
9.7 KiB
C
//
|
||
// Created by cfif on 05.05.23.
|
||
//
|
||
#include "MainModesArbiter_Private.h"
|
||
|
||
// Функция расчета D для Rwa
|
||
int32_t calc_D_for_Rwa(float Rwa_desired, float Rab, float Rw) {
|
||
float D_float = 256.0f - ((Rwa_desired - Rw) / Rab) * 256.0f;
|
||
int32_t D = (int32_t) (D_float + 0.5f); // Округление
|
||
|
||
if (D > 255) D = 255;
|
||
if (D < 0) D = 0;
|
||
|
||
return D;
|
||
}
|
||
|
||
// Параметры
|
||
|
||
const float R_const = 163.4f;
|
||
const float Rw_typ = 00.0f; //60.0f // Типовое сопротивление движка
|
||
const float Rw_max = 120.0f; // Максимальное (для консервативного расчета)
|
||
|
||
/**
|
||
* Расчет шагов для двухкаскадного цифрового потенциометра
|
||
* Резисторы: 100 кОм (грубый) и 10 кОм (точный)
|
||
* Постоянное сопротивление: 171 Ом
|
||
*
|
||
* @param Rout_required Требуемое сопротивление в Омах
|
||
* @param CountStep100 Указатель для сохранения шагов грубого резистора (0-255)
|
||
* @param CountStep10 Указатель для сохранения шагов точного резистора (0-255)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int32_t CalculateSteps(float Rout_required, int32_t *CountStep100, int32_t *CountStep10) {
|
||
const float R_constL = 163.4f;
|
||
const float R_ab_100k = 100000.0f;
|
||
const float R_ab_10k = 10000.0f;
|
||
|
||
if (CountStep100 == NULL || CountStep10 == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
const float Step100 = R_ab_100k / 256.0f;
|
||
const float Step10 = R_ab_10k / 256.0f;
|
||
|
||
float Rout = Rout_required - R_constL;
|
||
if (Rout < 0) Rout = 0;
|
||
|
||
*CountStep100 = (int32_t) (Rout / Step100);
|
||
if (*CountStep100 > 255) *CountStep100 = 255;
|
||
if (*CountStep100 < 0) *CountStep100 = 0;
|
||
|
||
float R_tail = Rout - (float) (*CountStep100) * Step100;
|
||
if (R_tail < 0) R_tail = 0;
|
||
|
||
*CountStep10 = (int32_t) (R_tail / Step10 + 0.5f);
|
||
if (*CountStep10 > 255) *CountStep10 = 255;
|
||
if (*CountStep10 < 0) *CountStep10 = 0;
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/**
|
||
* Расчет шагов для двухкаскадного цифрового потенциометра
|
||
* Приоритет заполнения точного резистора (10 кОм)
|
||
* Резисторы: 100 кОм (грубый) и 10 кОм (точный)
|
||
* Постоянное сопротивление: 163.4 Ом
|
||
*
|
||
* @param Rout_required Требуемое сопротивление в Омах
|
||
* @param CountStep100 Указатель для сохранения шагов грубого резистора (0-255)
|
||
* @param CountStep10 Указатель для сохранения шагов точного резистора (0-255)
|
||
* @return 0 - успех, -1 - ошибка
|
||
*/
|
||
int32_t CalculateSteps2(float Rout_required, float R_constS,float R_constL, int32_t *CountStep100, int32_t *CountStep10) {
|
||
const float R_ab_100k = 100000.0f;
|
||
const float R_ab_10k = 10000.0f;
|
||
|
||
if (CountStep100 == NULL || CountStep10 == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
const float Step100 = R_ab_100k / 256.0f;
|
||
const float Step10 = R_ab_10k / 256.0f;
|
||
|
||
float Rout = Rout_required - R_constS + R_constL;
|
||
if (Rout < 0) Rout = 0;
|
||
|
||
*CountStep100 = (int32_t) (Rout / Step100);
|
||
if (*CountStep100 > 255) *CountStep100 = 255;
|
||
if (*CountStep100 < 0) *CountStep100 = 0;
|
||
|
||
float R_tail = Rout - (float) (*CountStep100) * Step100;
|
||
if (R_tail < 0) R_tail = 0;
|
||
|
||
*CountStep10 = (int32_t) (R_tail / Step10 + 0.5f);
|
||
if (*CountStep10 > 255) *CountStep10 = 255;
|
||
if (*CountStep10 < 0) *CountStep10 = 0;
|
||
|
||
return 0;
|
||
}
|
||
|
||
void Mma_Init(
|
||
tMma *env,
|
||
tGpios *gpios,
|
||
tSerialPorts *serialPorts,
|
||
tCanPorts *canPorts,
|
||
tSpiPorts *spiPorts,
|
||
tRtcs *rtcs
|
||
) {
|
||
env->gpios = gpios;
|
||
env->serialPorts = serialPorts;
|
||
env->canPorts = canPorts;
|
||
env->spiPorts = spiPorts;
|
||
env->rtcs = rtcs;
|
||
|
||
InitThreadAtrStatic(&env->thread.attr, "Mma", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
|
||
env->thread.id = 0;
|
||
}
|
||
|
||
static _Noreturn void Mma_Thread(tMma *env) {
|
||
|
||
// Запуск устройства
|
||
Mma_InitStage(env);
|
||
|
||
bool result = false;
|
||
|
||
// Rwa — желаемое сопротивление между выводами W и A.
|
||
// Rab — полное номинальное сопротивление потенциометра (5, 10, 50 или 100 кОм).
|
||
// Rw — сопротивление движка (Wiper resistance). Типовое значение — 60 Ом (может варьироваться от 50 до 120 Ом).
|
||
// D — десятичное значение кода (от 0 до 255).
|
||
/*
|
||
float Rwa = 60.0f;
|
||
|
||
float Rwa100 = 21000.0f; // Это сколько сопротивление желаем
|
||
float R100ab = 100000.0f;
|
||
int32_t D100 = (int32_t) (256.0f - (((Rwa100 - Rwa) / R100ab) * 256.0f));
|
||
|
||
if (D100 > 255)
|
||
D100 = 255;
|
||
|
||
if (D100 < 0)
|
||
D100 = 0;
|
||
|
||
|
||
float Rwa10 = 3000.0f; // Это сколько сопротивление желаем
|
||
float R10ab = 10000.0f;
|
||
int32_t D10 = (int32_t) (256.0f - (((Rwa10 - Rwa) / R10ab) * 256.0f));
|
||
|
||
if (D10 > 255)
|
||
D10 = 255;
|
||
|
||
if (D10 < 0)
|
||
D10 = 0;
|
||
*/
|
||
|
||
int32_t step100, step10;
|
||
|
||
// Использование
|
||
float R_ab_100k = 100000.0f;
|
||
float R_ab_10k = 10000.0f;
|
||
float Step100 = R_ab_100k / 256.0f; // Шаг 100 кОм резистора
|
||
float Step10 = R_ab_10k / 256.0f; // Шаг 10 кОм резистора
|
||
|
||
float Rout = 9000.0f - R_const;
|
||
int32_t CountStep100 = (int32_t) (Rout / Step100); // Целых шагов на резисторе 100 кОм
|
||
float R_tail = Rout - (float) CountStep100 * Step100; // Хвост нужного сопротивления (Ом) для резистора 10 кОм
|
||
int32_t CountStep10 = (int32_t) (R_tail / Step10 + 0.5f); // Шагов на резисторе 10 КОм (+ 0.5f для округления)
|
||
|
||
|
||
float Rwa_desired_100k = (float)CountStep100 * Step100;
|
||
step100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);
|
||
|
||
float Rwa_desired_10k = (float)CountStep10 * Step10;
|
||
step10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);
|
||
|
||
/*
|
||
float Rwa_desired2_100k = 000.00f - R_const;
|
||
step100 = calc_D_for_Rwa(Rwa_desired2_100k, R_ab_100k, Rw_typ);
|
||
|
||
float Rwa_desired2_10k = 9000.0f - R_const;
|
||
step10 = calc_D_for_Rwa(Rwa_desired2_10k, R_ab_10k, Rw_typ);
|
||
*/
|
||
|
||
|
||
float required_resistance = 2000.00f;
|
||
// CalculateSteps2(required_resistance, 0, 0, &step100, &step10);
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
|
||
asm("nop");
|
||
|
||
|
||
// D100 = (int32_t)(255.0f - (R / Step100));
|
||
// D10 = (int32_t)(255.0f - (R / Step10));
|
||
|
||
|
||
// D100 = 255 - (R / 390.625);
|
||
// int32_t div
|
||
|
||
|
||
|
||
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, D100);
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, D10);
|
||
|
||
|
||
uint32_t step = 0;
|
||
|
||
for (;;) {
|
||
|
||
/*
|
||
Rwa_desired_100k += 1000.0f;
|
||
D100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);
|
||
|
||
if (Rwa_desired_100k > R_ab_100k) {
|
||
Rwa_desired_100k = 0;
|
||
}
|
||
|
||
|
||
Rwa_desired_10k += 1000.0f;
|
||
D10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);
|
||
|
||
if (Rwa_desired_10k > R_ab_10k) {
|
||
Rwa_desired_10k = 0;
|
||
}
|
||
*/
|
||
|
||
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
asm("nop");
|
||
// ++step;
|
||
|
||
// if (step> 255)
|
||
// step =0;
|
||
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
asm("nop");
|
||
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
/*
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, D100);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, D10);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, D100);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, D10);
|
||
asm("nop");
|
||
*/
|
||
/*
|
||
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 0;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
*/
|
||
/*
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
uint16_t data = 255;
|
||
bool result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 128;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 1;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 0;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
*/
|
||
}
|
||
}
|
||
|
||
|
||
void Mma_StartThread(tMma *env) {
|
||
if (!env->thread.id) {
|
||
env->thread.id = osThreadNew((osThreadFunc_t) (Mma_Thread), (void *) (env), &env->thread.attr);
|
||
} else {
|
||
osThreadResume(env->thread.id);
|
||
}
|
||
} |