void power_tick(void)
{
static uint16_t c = 0;
if( c-- == 0 ){
c = 500;
power_update();
}
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if ENCODER_ENABLED
// Initialize software I2C to talk with encoder.
swi2c_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
{
int16_t position;
// Get start-up position
#if ENCODER_ENABLED
position=(int16_t) enc_get_position_value();
#else
while (!adc_position_value_is_ready());
position=(int16_t) adc_get_position_value();
#endif
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(position);
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, position);
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
}
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
uint8_t tick;
int16_t pwm;
int16_t position;
// Is ADC position value ready?
// NOTE: Even when the encoder is enabled, we still need the ADC potmeasurement as the
// clock because that is how the original firmware was written.
tick=adc_position_value_is_ready();
if(tick)
{
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
}
// Get the new position value.
if(tick)
{
position = (int16_t) adc_get_position_value(); // NOTE: For encoder builds, this is the clock: clear the flag
#if ENCODER_ENABLED
} // Always run the encoder (faster PID to PWM loop = better?)
position = (int16_t) enc_get_position_value();
if (position >= 0)
{
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position, tick);
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
#if ESTIMATOR_ENABLED
// Initialize the state estimator module.
estimator_init();
#endif
#if REGULATOR_MOTION_ENABLED
// Initialize the regulator algorithm module.
regulator_init();
#endif
#if PID_MOTION_ENABLED
// Initialize the PID algorithm module.
pid_init();
#endif
#if IPD_MOTION_ENABLED
// Initialize the IPD algorithm module.
ipd_init();
#endif
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
// Is position value ready?
if (adc_position_value_is_ready())
{
int16_t pwm;
int16_t position;
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// Get the new position value.
position = (int16_t) adc_get_position_value();
#if ESTIMATOR_ENABLED
// Estimate velocity.
estimate_velocity(position);
#endif
#if PID_MOTION_ENABLED
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#endif
#if IPD_MOTION_ENABLED
// Call the IPD algorithm module to get a new PWM value.
pwm = ipd_position_to_pwm(position);
#endif
#if REGULATOR_MOTION_ENABLED
// Call the state regulator algorithm module to get a new PWM value.
pwm = regulator_position_to_pwm(position);
#endif
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
void power_init(void)
{
power_update();
}
void power_init()
{
power_good_time = current_time;
power_update();
}
void main(void) {
uint16_t i;
char usb_char, err;
const char init_msg[] = {'I', 'N', 'I', 'T'};
//extern FILE *stdout = _H_USER; // redirect stdout to USB
init();
storage_init();
display_cnt = 0;
volume_tick = 0;
chan_tick = 0;
usb_tick = 0;
ir_tick = 0;
ir_speedup = 20;
dac_lock_tick = 0;
display_set_alt(0x00, 0x00, 0x00);
display_set(0x00, 0x00, 1);
ir_receiver_init();
err = relay_boards_init(); // as side-effect: determine board Type and Id
amp_state_init();
set_relays(0x00, 0x00, 0x00, 0x00);
display_oled_init();
if (has_oled_display) {
display_oled_chars( 0, 0, 5, "hello");
display_set_alt(DIGIT_D, 0x00, 3);
}
else if (err)
display_set_alt(DIGIT_E, 0x01, 3);
// Globally enable interrupts
#ifdef UseIPEN
INTCONbits.GIEH = 1;
INTCONbits.GIEL = 1;
#else
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
// (re-)launch USB activity
prev_usb_bus_sense = 0;
usb_write(init_msg, (uint8_t) 4);
// The above 'set_relays' enabled the power relay for the analog supply.
#ifdef __DEBUG
power_tick = 0;
#else
power_tick = 120;
#endif
// Set a timer to later undo the mute and activate last volume setting.
// wait some time for stabilization before enabling all other interrupts
while (power_tick > 0)
; // gets decreased on timer interrupts, 183Hz
// power==0 now, from amp_state_init().
// incr power now quickly to 1, and later to 2.
power_incr = 1;
INTCON3bits.INT1IF = 0;
INTCON3bits.INT1IE = 1;
INTCON3bits.INT2IF = 0;
INTCON3bits.INT2IE = 1;
INTCON3bits.INT3IF = 0;
INTCON3bits.INT3IE = 1;
// Check if a DAC is present in this Relaixed, if so initialize.
// This check was delayed to allow DAC power-up, otherwise its i2c interface stays in reset
dac_init();
while (1) {
if (volume_incr)
volume_update();
if (balance_incr > 1 || balance_incr < -1)
// suppress a single tick, might have been by accident
balance_update();
if (channel_incr)
channel_update();
if (power_incr) {
if (flash_tick != 0 && power_incr < 0) {
// quickly save recent volume/balance update
flash_tick = 0;
flash_volume_channel();
} else if (power_incr > 0 && power_state() == 0) {
// if we move power_state from 0 to 1, we surely want to go later to 2
// For RelaixedPassive: wait somewhat longer for its soft-switch main power
power_tick = (isRelaixedXLR) ? 500 : 700;
}
if (power_incr > 0)
dac_init(); // check (again) for presence of DAC: it needs time to get out of reset
power_update();
}
if (ir_received_ok) {
ir_received_ok = 0;
ir_handle_code();
if (volume_incr) {
vol_usb_msg[0] = 'V';
// when 'volume' keeps pressed, the volume-tick-speed goes up
if (ir_speedup <= 49)
ir_speedup += 4;
} else {
vol_usb_msg[0] = 'v';
ir_speedup = 20;
}
byte2hex(vol_usb_msg + 1, ir_tick);
byte2hex(vol_usb_msg + 3, ir_speedup);
if (power_incr)
ir_tick = 100; // increase the default 20 to 100 on power on/off
else
flash_tick = 400;
usb_write(vol_usb_msg, 5);
}
if (flash_tick == 1) {
flash_tick = 0;
flash_volume_channel();
}
if (dac_status() >= DAC_NOLOCK && dac_lock_tick == 0) {
dac_check_lock();
dac_lock_tick = 45; // check lock 4x per secnd
}
/* some I/O to check repeatedly, for absence of interrupt-on-change */
check_usb_power(err);
}
}
void main(void)
{
unsigned int i;
char usb_char, err;
const char init_msg[] = {'I', 'N', 'I', 'T'};
extern FILE *stdout = _H_USER; // redirect stdout to USB
init();
display_cnt = 0;
volume_tick = 0;
chan_tick = 0;
usb_tick = 0;
ir_tick = 0;
display_set( 0x00, 0x00);
display_set_alt( 0x00, 0x00, 0x00);
ir_receiver_init();
storage_init();
err = relay_boards_init();
set_relays(0x00, 0x00, 0x00, 0x00, 0x00);
amp_state_init();
if (err)
display_set_alt( DIGIT_E, 0x01, 3);
// Globally enable interrupts
#ifdef UseIPEN
INTCONbits.GIEH = 1;
INTCONbits.GIEL = 1;
#else
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
// (re-)launch USB activity
prev_usb_bus_sense = 0;
usb_write( init_msg, (byte)4);
// The above 'set_relays' enabled the power relay for the analog supply.
power_tick = 150;
// Set a timer to later undo the mute and activate last volume setting.
// wait some time for stabilization before enabling all other interrupts
while (power_tick > 0)
; // gets decreased on timer interrupts
// power==0 now, from amp_state_init().
// incr power now quickly to 1, and later to 2.
power_incr = 1;
INTCON3bits.INT1IF = 0;
INTCON3bits.INT1IE = 1;
INTCON3bits.INT2IF = 0;
INTCON3bits.INT2IE = 1;
INTCON3bits.INT3IF = 0;
INTCON3bits.INT3IE = 1;
while (1)
{
if (volume_incr)
volume_update();
if (balance_incr > 1 || balance_incr < -1)
// suppress a single tick, might have been by accident
balance_update();
if (channel_incr)
channel_update();
if (power_incr)
{
if (flash_tick && power_incr < 0)
{
// quickly save recent volume/balance update
flash_tick = 0;
flash_volume_channel();
} else if (power_incr > 0 && power_state() == 0)
{
// if we move power_state from 0 to 1, we surely want to go later to 2
power_tick = 500;
}
power_update();
}
if (ir_received_ok)
{
ir_received_ok = 0;
ir_handle_code();
if (power_incr)
ir_tick = 100;
else
flash_tick = 400;
}
if (flash_tick == 1)
{
flash_tick = 0;
flash_volume_channel();
}
/* some I/O to check repeatedly, for absence of interrupt-on-change */
check_usb_power(err);
}
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Initialize the PWM module.
pwm_init();
#endif
#if STEP_ENABLED
// Initialise the stepper motor
step_init();
#endif
// Initialize the ADC module.
adc_init();
// Initialise the Heartbeart
heartbeat_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if BACKEMF_ENABLED
// Initialise the back emf module
backemf_init();
#endif
#if ALERT_ENABLED
//initialise the alert registers
alert_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(banks_read_byte(POS_PID_BANK, REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Trigger the adc sampling hardware
adc_start(ADC_CHANNEL_POSITION);
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
pwm_enable();
#endif
#if STEP_ENABLED
step_enable();
#endif
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
static uint8_t emf_motor_is_coasting = 0;
// Is the system heartbeat ready?
if (heartbeat_is_ready())
{
static int16_t last_seek_position;
static int16_t wait_seek_position;
static int16_t new_seek_position;
// Clear the heartbeat flag
heartbeat_value_clear_ready();
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// General call support
// Check to see if we have the wait flag enabled. If so save the new position, and write in the
// old position until we get the move command
if (general_call_enabled())
{
//we need to wait for the go command before moving
if (general_call_wait())
{
// store the new position, but let the servo lock to the last seek position
wait_seek_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
if (wait_seek_position != last_seek_position) // do we have a new position?
{
new_seek_position = wait_seek_position;
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, last_seek_position);
}
}
last_seek_position = registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
//check to make sure that we can start the move.
if (general_call_start() ||
( registers_read_byte(REG_GENERAL_CALL_GROUP_START) == banks_read_byte(CONFIG_BANK, REG_GENERAL_CALL_GROUP)))
{
// write the new position with the previously saved position
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, new_seek_position);
general_call_start_wait_reset(); // reset the wait flag
general_call_start_reset(); // reset the start flag
}
}
#if BACKEMF_ENABLED
// Quick and dirty check to see if pwm is active. This is done to make sure the motor doesn't
// whine in the audible range while idling.
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
if (pwm_a || pwm_b)
{
// Disable PWM
backemf_coast_motor();
emf_motor_is_coasting = 1;
}
else
{
// reset the back EMF value to 0
banks_write_word(INFORMATION_BANK, REG_BACKEMF_HI, REG_BACKEMF_LO, 0);
emf_motor_is_coasting = 0;
}
#endif
#if ADC_ENABLED
// Trigger the adc sampling hardware. This triggers the position and temperature sample
adc_start(ADC_FIRST_CHANNEL);
#endif
}
// Wait for the samples to complete
#if TEMPERATURE_ENABLED
if (adc_temperature_value_is_ready())
{
// Save temperature value to registers
registers_write_word(REG_TEMPERATURE_HI, REG_TEMPERATURE_LO, (uint16_t)adc_get_temperature_value());
}
#endif
#if CURRENT_ENABLED
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
#endif
#if ADC_POSITION_ENABLED
if (adc_position_value_is_ready())
{
int16_t position;
// Get the new position value from the ADC module.
position = (int16_t) adc_get_position_value();
#else
if (position_value_is_ready())
{
int16_t position;
// Get the position value from an external module.
position = (int16_t) get_position_value();
#endif
int16_t pwm;
#if BACKEMF_ENABLED
if (emf_motor_is_coasting == 1)
{
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
// Quick and dirty check to see if pwm is active
if (pwm_a || pwm_b)
{
// Get the backemf sample.
backemf_get_sample();
// Turn the motor back on
backemf_restore_motor();
emf_motor_is_coasting = 0;
}
}
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#if ALERT_ENABLED
// Update the alert status registers and do any throttling
alert_check();
#endif
// Allow any alerts to modify the PWM value.
pwm = alert_pwm_throttle(pwm);
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
#endif
#if STEP_ENABLED
// Update the stepper motor as indicated by the PWM value.
// Sanity checks are performed against the position value.
step_update(position, pwm);
#endif
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
// Update the bank register operations
banks_update_registers();
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
