297 lines
8.3 KiB
C
297 lines
8.3 KiB
C
/* Copyright (C) 2025 Dennis Gunia - All Rights Reserved
|
|
* You may use, distribute and modify this code under the
|
|
* terms of the AGPL-3.0 license.
|
|
*
|
|
* https://www.dennisgunia.de
|
|
* https://github.com/dennis9819/splitflap_v1
|
|
*/
|
|
|
|
|
|
#include "mctrl.h"
|
|
|
|
// Motor driver steps definition. Reverse for direction change.
|
|
uint8_t motor_steps[4] = {
|
|
0b00000001,
|
|
0b00000010,
|
|
0b00000100,
|
|
0b00001000,
|
|
};
|
|
|
|
uint8_t step_index = 0; // current index in motor_steps
|
|
uint8_t target_flap = 0; // target flap
|
|
uint16_t absolute_pos = 0; // absolute position in steps
|
|
uint16_t steps_since_home = 0; // steps since last home signal
|
|
|
|
// homing util variables
|
|
uint8_t homing = 0; // current homing step
|
|
uint8_t lastSens = 0; // home sonsor signal from last tick
|
|
|
|
// counter for auto powersaving
|
|
uint8_t ticksSinceMove = 0;
|
|
|
|
// value to goto after the current is reached. 255 = NONE.
|
|
uint8_t afterRotation = STEPS_AFTERROT;
|
|
|
|
int16_t *delta_err;
|
|
|
|
// error and status flags
|
|
uint8_t sts_flag_errorTooBig = 0; // last home signal too early or too late
|
|
uint8_t sts_flag_noHome = 0; // no home signal detected. Wheel stuck
|
|
uint8_t sts_flag_fuse = 0; // blown fuse detcted
|
|
uint8_t sts_flag_pwrdwn = 0; // device is powered down by controller
|
|
uint8_t sts_flag_failsafe = 0; // device is powered down for safety reasons
|
|
uint8_t sts_flag_busy = 0; // device is busy
|
|
// voltage monitoring variables
|
|
uint16_t currentVoltage = 0; // current ADC reading
|
|
uint8_t currentFaultReadings = 0; // ticks with faulty readings (too many will
|
|
// trip pwrdwn and sts_flag_fuse)
|
|
|
|
// initialize motor controller
|
|
void mctrl_init()
|
|
{
|
|
DDRC = 0x0F; // set all pins as outputs
|
|
PORTC = 0x00; // set all to LOW
|
|
|
|
DDRD &= ~(1 << PD3); // PD3 is input
|
|
PORTD |= (1 << PD3); // PD3 pullup
|
|
|
|
// setup adc
|
|
ADMUX = 0x07; // Aref, ADC7
|
|
ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADPS1); // Enable ADC, Start first
|
|
// reading No freerunning, 8MHz
|
|
while ((ADCSRA & (1 << ADSC)) > 0)
|
|
{
|
|
};
|
|
// wait until first reading is complete,
|
|
// to avoid error flag on first tick!
|
|
|
|
// setup timer for ISR
|
|
TCCR1A = 0;
|
|
TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10); // CTC und Prescaler 64
|
|
OCR1A = MISR_OCR1A;
|
|
TIMSK = 1 << OCIE1A; // Timerinterrupts aktivieren
|
|
homing = 1;
|
|
delta_err = malloc(ERROR_DATASETS * sizeof(uint16_t));
|
|
_delay_ms(MDELAY_STARTUP);
|
|
sei();
|
|
}
|
|
|
|
// call when critical fail. Powers down motor and sets flags
|
|
void failSafe()
|
|
{
|
|
sts_flag_failsafe = 1;
|
|
PORTC = 0x00;
|
|
}
|
|
|
|
// read voltage non blocking (called every tick)
|
|
void readVoltage()
|
|
{
|
|
currentVoltage = ADC; // read last measurement
|
|
ADMUX = 0x07; // select ADC7
|
|
ADCSRA |= (1 << ADSC); // trigger next reading
|
|
if (currentVoltage < MVOLTAGE_LSTOP)
|
|
{ // if voltage is too low, fuse is probably broken
|
|
currentFaultReadings++;
|
|
if (currentFaultReadings > MVOLTAGE_FAULTRD)
|
|
{ // too many fault readings trigger failSafe
|
|
sts_flag_fuse = 1;
|
|
failSafe();
|
|
}
|
|
}
|
|
}
|
|
|
|
// MAIN service routine. Called by timer 1
|
|
ISR(TIMER1_COMPA_vect)
|
|
{
|
|
readVoltage(); // read and check voltage
|
|
if (sts_flag_pwrdwn == 1 || sts_flag_failsafe == 1)
|
|
{
|
|
return;
|
|
} // if sts_flag_pwrdwn, STOP!
|
|
if (steps_since_home > STEPS_PER_REV * MHOME_TOLERANCE)
|
|
{ // check if home is missing for too long
|
|
// home missing error. Wheel probably stuck or power fail
|
|
sts_flag_noHome = 1;
|
|
failSafe();
|
|
}
|
|
else if (homing == 1)
|
|
{ // Homing procedure 1. step: move out of home
|
|
if ((PIND & (1 << PD3)) > 0)
|
|
{
|
|
homing = 2;
|
|
}
|
|
else
|
|
{
|
|
mctrl_step();
|
|
}
|
|
}
|
|
else if (homing == 2)
|
|
{ // Homing procedure 2. step: find magnet
|
|
mctrl_step();
|
|
if ((PIND & (1 << PD3)) == 0)
|
|
{
|
|
homing = 3;
|
|
steps_since_home = 0;
|
|
absolute_pos = STEPS_OFFSET;
|
|
incrementCounter();
|
|
}
|
|
}
|
|
else if (homing == 3)
|
|
{ // Homing procedure 3. step: apply offset
|
|
if (absolute_pos <= 0)
|
|
{
|
|
homing = 0;
|
|
absolute_pos = STEPS_OFFSET; // set correct position again
|
|
}
|
|
mctrl_step();
|
|
absolute_pos--;
|
|
}
|
|
else
|
|
{ // when no failsafe is triggered and homing is done
|
|
// calculate target position
|
|
uint16_t target_pos = (target_flap * STEPS_PER_FLAP) + STEPS_OFFSET;
|
|
if (target_pos >= STEPS_PER_REV)
|
|
{
|
|
target_pos -= STEPS_PER_REV;
|
|
}
|
|
if (absolute_pos != target_pos)
|
|
{
|
|
// if target position is not reached, move motor
|
|
ticksSinceMove = 0;
|
|
mctrl_step();
|
|
absolute_pos++;
|
|
if (absolute_pos >= STEPS_PER_REV)
|
|
{
|
|
absolute_pos -= STEPS_PER_REV;
|
|
}
|
|
// detect home position
|
|
if ((PIND & (1 << PD3)) == 0)
|
|
{
|
|
if (lastSens == 0)
|
|
{
|
|
// new home transition
|
|
int16_t errorDelta =
|
|
(int16_t)(absolute_pos > (STEPS_PER_REV / 2) ? absolute_pos - STEPS_PER_REV : absolute_pos);
|
|
sts_flag_errorTooBig = (errorDelta > MHOME_ERRDELTA) || (errorDelta < -MHOME_ERRDELTA) ? 1 : 0;
|
|
// storeErr(errorDelta);
|
|
absolute_pos = 0;
|
|
steps_since_home = 0;
|
|
// increment rotations counter
|
|
incrementCounter();
|
|
}
|
|
lastSens = 1;
|
|
}
|
|
else
|
|
{
|
|
lastSens = 0;
|
|
}
|
|
}
|
|
else
|
|
{ // if target position is reached
|
|
if (afterRotation < (AMOUNTFLAPS + 5))
|
|
{ // if after rotation is set, apply it as new target
|
|
target_flap = afterRotation;
|
|
afterRotation = STEPS_AFTERROT;
|
|
}
|
|
else if (ticksSinceMove < 2)
|
|
{ // if motor has not been moved
|
|
sts_flag_busy = 0;
|
|
}
|
|
else if (ticksSinceMove < MPWRSVG_TICKSTOP)
|
|
{ // if motor has not been moved
|
|
ticksSinceMove++;
|
|
}
|
|
else
|
|
{ // power off after 50 ticks
|
|
// PORTC = 0x00; // turn off stepper
|
|
}
|
|
}
|
|
}
|
|
rc_tick(); // process counter tick, non-blocking
|
|
}
|
|
|
|
// TODO
|
|
void storeErr(int16_t error)
|
|
{
|
|
int16_t *delta_err_tmp = malloc(ERROR_DATASETS * sizeof(uint16_t));
|
|
memcpy(delta_err, delta_err_tmp + sizeof(uint16_t), ((ERROR_DATASETS - 2) * sizeof(uint16_t)));
|
|
memcpy(&error, delta_err_tmp, sizeof(uint16_t));
|
|
free(delta_err);
|
|
delta_err = delta_err_tmp;
|
|
}
|
|
// TODO
|
|
void getErr(int16_t *error)
|
|
{
|
|
memcpy(delta_err, error, (ERROR_DATASETS * sizeof(uint16_t)));
|
|
}
|
|
|
|
// return status flag
|
|
uint8_t getSts()
|
|
{
|
|
uint8_t status = sts_flag_errorTooBig; // bit 0: delta too big
|
|
status |= sts_flag_noHome << 1; // bit 1: no home found
|
|
status |= sts_flag_fuse << 2; // bit 2: fuse blown
|
|
status |= sts_flag_pwrdwn << 4; // bit 4: device powered down
|
|
status |= sts_flag_failsafe << 5; // bit 5: failsafe active
|
|
status |= sts_flag_busy << 6; // bit 6: failsafe active
|
|
if ((PIND & (1 << PD3)) == 0)
|
|
{
|
|
status |= (1 << 3);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
// return voltage
|
|
uint16_t getVoltage()
|
|
{
|
|
return currentVoltage;
|
|
}
|
|
|
|
// set target flap
|
|
void mctrl_set(uint8_t flap, uint8_t fullRotation)
|
|
{
|
|
sts_flag_busy = 1;
|
|
if (fullRotation == 0)
|
|
{
|
|
target_flap = flap;
|
|
}
|
|
else
|
|
{
|
|
target_flap = (target_flap + (STEPS_PER_FLAP - 1)) % STEPS_PER_FLAP;
|
|
afterRotation = flap;
|
|
}
|
|
}
|
|
|
|
// trigger home procedure
|
|
void mctrl_home()
|
|
{
|
|
homing = 1;
|
|
}
|
|
|
|
// change motor power state
|
|
void mctrl_power(uint8_t state)
|
|
{
|
|
if (state == 0)
|
|
{
|
|
sts_flag_pwrdwn = 1;
|
|
PORTC = 0x00;
|
|
}
|
|
else
|
|
{
|
|
sts_flag_pwrdwn = 0;
|
|
PORTC = motor_steps[step_index];
|
|
}
|
|
}
|
|
|
|
// do stepper step (I/O)
|
|
void mctrl_step()
|
|
{
|
|
step_index++;
|
|
steps_since_home++;
|
|
if (step_index > 3)
|
|
{
|
|
step_index = 0;
|
|
}
|
|
PORTC = motor_steps[step_index];
|
|
}
|