int I2C_init( void ) {
twi_options_t opt;
twi_slave_fct_t twi_slave_fct;
int status;
double total = 0;
// Initialize and enable interrupt
irq_initialize_vectors();
cpu_irq_enable();
// TWI gpio pins configuration
gpio_enable_module(TWI_GPIO_MAP, sizeof(TWI_GPIO_MAP) / sizeof(TWI_GPIO_MAP[0]));
// initialize the interrupt flag for alerting the Pi of new data (TWI = Three Wire Interface for us)
ioport_enable_pin(I2C_FLAG);
ioport_set_pin_dir(I2C_FLAG,IOPORT_DIR_OUTPUT);
ioport_set_pin_level(I2C_FLAG,false);
// options settings
opt.pba_hz = FOSC0;
opt.speed = TWI_SPEED;
opt.chip = EEPROM_ADDRESS;
// initialize TWI driver with options
twi_slave_fct.rx = &twi_slave_rx;
twi_slave_fct.tx = &twi_slave_tx;
twi_slave_fct.stop = &twi_slave_stop;
status = twi_slave_init(&AVR32_TWI, &opt, &twi_slave_fct );
return (&s_memory[0] );
}
int main()
{
DDRB = 0x20;
PORTC = 1 << 4 | 1 << 5;
stdout = &mystdout;
stdin = &mystdin;
uart_init();
puts("Slave Transmit!");
twi_slave_init(0x08);
printf("Enter char to send: ");
while (1)
{
char c = getchar();
twi_write(c);
printf("Status: %x\n", TW_STATUS);
if (TW_STATUS != TW_ST_DATA_ACK)
break;
PINB = 0x20;
}
puts("Disconnected");
return 0;
}
u8 init_slave(void) {
static twi_slave_fct_t twi_slave_fct;
opt.pba_hz = FPBA_HZ;
opt.speed = TWI_SPEED;
opt.chip = addr;
twi_slave_fct.rx = &slave_rx;
twi_slave_fct.tx = &slave_tx;
twi_slave_fct.stop = &slave_stop;
return twi_slave_init(&AVR32_TWI, &opt, &twi_slave_fct );
}
/**
* \brief Application entry point for TWI Slave example.
*
* \return Unused (ANSI-C compatibility).
*/
int main(void)
{
uint32_t i;
/* Initialize the SAM system */
sysclk_init();
#if (SAM4S || SAM4E)
/* Select PB4 and PB5 function, this will cause JTAG discconnect */
REG_CCFG_SYSIO |= CCFG_SYSIO_SYSIO4;
REG_CCFG_SYSIO |= CCFG_SYSIO_SYSIO5;
#endif
/* Initialize the board */
board_init();
/* Initialize the console UART */
configure_console();
/* Output example information */
puts(STRING_HEADER);
#if (SAMG55)
/* Enable the peripheral and set TWI mode. */
flexcom_enable(BOARD_FLEXCOM_TWI);
flexcom_set_opmode(BOARD_FLEXCOM_TWI, FLEXCOM_TWI);
#else
/* Enable the peripheral clock for TWI */
pmc_enable_periph_clk(BOARD_ID_TWI_SLAVE);
#endif
for (i = 0; i < MEMORY_SIZE; i++) {
emulate_driver.uc_memory[i] = 0;
}
emulate_driver.us_offset_memory = 0;
emulate_driver.uc_acquire_address = 0;
emulate_driver.us_page_address = 0;
/* Configure TWI as slave */
puts("-I- Configuring the TWI in slave mode\n\r");
twi_slave_init(BOARD_BASE_TWI_SLAVE, SLAVE_ADDRESS);
/* Clear receipt buffer */
twi_read_byte(BOARD_BASE_TWI_SLAVE);
/* Configure TWI interrupts */
NVIC_DisableIRQ(BOARD_TWI_IRQn);
NVIC_ClearPendingIRQ(BOARD_TWI_IRQn);
NVIC_SetPriority(BOARD_TWI_IRQn, 0);
NVIC_EnableIRQ(BOARD_TWI_IRQn);
twi_enable_interrupt(BOARD_BASE_TWI_SLAVE, TWI_SR_SVACC);
while (1) {
}
}
int main(void) {
// Set clock @ 8Mhz
CPU_PRESCALE(1);
sei();
twi_slave_init();
twi_slave_enable();
for (;;);
}
/*! \brief Main function.
*/
int main(void)
{
static const gpio_map_t TWI_GPIO_MAP =
{
{AVR32_TWI_SDA_0_0_PIN, AVR32_TWI_SDA_0_0_FUNCTION},
{AVR32_TWI_SCL_0_0_PIN, AVR32_TWI_SCL_0_0_FUNCTION}
};
twi_options_t opt;
twi_slave_fct_t twi_slave_fct;
int status;
// Switch to oscillator 0
pm_switch_to_osc0(&AVR32_PM, FOSC0, OSC0_STARTUP);
// Init debug serial line
init_dbg_rs232(FOSC0);
// Initialize and enable interrupt
irq_initialize_vectors();
cpu_irq_enable();
// Display a header to user
print_dbg("\x0C\r\nTWI Example\r\nSlave!\r\n");
// TWI gpio pins configuration
gpio_enable_module(TWI_GPIO_MAP, sizeof(TWI_GPIO_MAP) / sizeof(TWI_GPIO_MAP[0]));
// options settings
opt.pba_hz = FOSC0;
opt.speed = TWI_SPEED;
opt.chip = EEPROM_ADDRESS;
// initialize TWI driver with options
twi_slave_fct.rx = &twi_slave_rx;
twi_slave_fct.tx = &twi_slave_tx;
twi_slave_fct.stop = &twi_slave_stop;
status = twi_slave_init(&AVR32_TWI, &opt, &twi_slave_fct );
// check init result
if (status == TWI_SUCCESS)
{
// display test result to user
print_dbg("Slave start:\tPASS\r\n");
}
else
{
// display test result to user
print_dbg("slave start:\tFAIL\r\n");
}
while(1);
}
void TwoWire::begin(uint8_t address) {
if (onBeginCallback != nullptr)
{
onBeginCallback();
}
// Disable PDC channel
twi->TWI_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
twi_slave_init(twi, address);
status = SLAVE_IDLE;
twi_enable_interrupt(twi, TWI_IER_SVACC);
//| TWI_IER_RXRDY | TWI_IER_TXRDY | TWI_IER_TXCOMP);
}
void wm_init(unsigned char * id, unsigned char * button_data, unsigned char * cal_data, unsigned int cal_data_length/*, void (*function)(void)*/)
{
/*
// link user function
wm_sample_event = function;
*/
// start state
wm_newaction(button_data);
twi_reg[0xF0] = 0; // disable encryption
// set id
for(unsigned int i = 0, j = 0xFA; i < 6; i++, j++)
{
twi_reg[j] = id[i];
}
// set calibration data
for(unsigned int i = 0, j = 0x20; i < cal_data_length; i++, j++)
{
twi_reg[j] = cal_data[i];
}
/*
// initialize device detect pin
dev_detect_port &= 0xFF ^ _BV(dev_detect_pin);
dev_detect_ddr |= _BV(dev_detect_pin);
_delay_ms(500); // delay to simulate disconnect
*/
// ready twi bus, no pull-ups
twi_port &= 0xFF ^ _BV(twi_scl_pin);
twi_port &= 0xFF ^ _BV(twi_sda_pin);
// start twi slave, link events
twi_slave_init(0x52);
/*
// make the wiimote think something is connected
dev_detect_port |= _BV(dev_detect_pin);
*/
}
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;
}
// initialize application timer
extern void init_tc (void) {
volatile avr32_tc_t *tc = APP_TC;
// waveform options
static const tc_waveform_opt_t waveform_opt = {
.channel = APP_TC_CHANNEL, // channel
.bswtrg = TC_EVT_EFFECT_NOOP, // software trigger action on TIOB
.beevt = TC_EVT_EFFECT_NOOP, // external event action
.bcpc = TC_EVT_EFFECT_NOOP, // rc compare action
.bcpb = TC_EVT_EFFECT_NOOP, // rb compare
.aswtrg = TC_EVT_EFFECT_NOOP, // soft trig on TIOA
.aeevt = TC_EVT_EFFECT_NOOP, // etc
.acpc = TC_EVT_EFFECT_NOOP,
.acpa = TC_EVT_EFFECT_NOOP,
// Waveform selection: Up mode with automatic trigger(reset) on RC compare.
.wavsel = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,
.enetrg = false, // external event trig
.eevt = 0, // extern event select
.eevtedg = TC_SEL_NO_EDGE, // extern event edge
.cpcdis = false, // counter disable when rc compare
.cpcstop = false, // counter stopped when rc compare
.burst = false,
.clki = false,
// Internal source clock 5, connected to fPBA / 128.
.tcclks = TC_CLOCK_SOURCE_TC5
};
// Options for enabling TC interrupts
static const tc_interrupt_t tc_interrupt = {
.etrgs = 0,
.ldrbs = 0,
.ldras = 0,
.cpcs = 1, // Enable interrupt on RC compare alone
.cpbs = 0,
.cpas = 0,
.lovrs = 0,
.covfs = 0
};
// Initialize the timer/counter.
tc_init_waveform(tc, &waveform_opt);
// set timer compare trigger.
// we want it to overflow and generate an interrupt every 1 ms
// so (1 / fPBA / 128) * RC = 0.001
// so RC = fPBA / 128 / 1000
// tc_write_rc(tc, APP_TC_CHANNEL, (FPBA_HZ / 128000));
tc_write_rc(tc, APP_TC_CHANNEL, (FPBA_HZ / 128000));
// configure the timer interrupt
tc_configure_interrupts(tc, APP_TC_CHANNEL, &tc_interrupt);
// Start the timer/counter.
tc_start(tc, APP_TC_CHANNEL);
}
extern void init_spi (void) {
sysclk_enable_pba_module(SYSCLK_SPI);
static const gpio_map_t SPI_GPIO_MAP = {
{SPI_SCK_PIN, SPI_SCK_FUNCTION },
{SPI_MISO_PIN, SPI_MISO_FUNCTION},
{SPI_MOSI_PIN, SPI_MOSI_FUNCTION},
{SPI_NPCS0_PIN, SPI_NPCS0_FUNCTION },
{SPI_NPCS1_PIN, SPI_NPCS1_FUNCTION },
{SPI_NPCS2_PIN, SPI_NPCS2_FUNCTION },
};
// Assign GPIO to SPI.
gpio_enable_module(SPI_GPIO_MAP, sizeof(SPI_GPIO_MAP) / sizeof(SPI_GPIO_MAP[0]));
spi_options_t spiOptions = {
.reg = DAC_SPI,
.baudrate = 2000000,
.bits = 8,
.trans_delay = 0,
.spck_delay = 0,
.stay_act = 1,
.spi_mode = 1,
.modfdis = 1
};
// Initialize as master.
spi_initMaster(SPI, &spiOptions);
// Set SPI selection mode: variable_ps, pcs_decode, delay.
spi_selectionMode(SPI, 0, 0, 0);
// Enable SPI module.
spi_enable(SPI);
// spi_setupChipReg( SPI, &spiOptions, FPBA_HZ );
spi_setupChipReg(SPI, &spiOptions, sysclk_get_pba_hz() );
// add ADC chip register
spiOptions.reg = ADC_SPI;
spiOptions.baudrate = 20000000;
spiOptions.bits = 16;
spiOptions.spi_mode = 2;
spiOptions.spck_delay = 0;
spiOptions.trans_delay = 5;
spiOptions.stay_act = 0;
spiOptions.modfdis = 0;
spi_setupChipReg( SPI, &spiOptions, FPBA_HZ );
// add OLED chip register
spiOptions.reg = OLED_SPI;
spiOptions.baudrate = 40000000;
spiOptions.bits = 8;
spiOptions.spi_mode = 3;
spiOptions.spck_delay = 0;
spiOptions.trans_delay = 0;
spiOptions.stay_act = 1;
spiOptions.modfdis = 1;
spi_setupChipReg( SPI, &spiOptions, FPBA_HZ );
}
// initialize USB host stack
void init_usb_host (void) {
uhc_start();
}
// initialize i2c
void init_i2c_master(void) {
twi_options_t opt;
int status;
static const gpio_map_t TWI_GPIO_MAP = {
{AVR32_TWI_SDA_0_0_PIN, AVR32_TWI_SDA_0_0_FUNCTION},
{AVR32_TWI_SCL_0_0_PIN, AVR32_TWI_SCL_0_0_FUNCTION}
};
gpio_enable_module(TWI_GPIO_MAP, sizeof(TWI_GPIO_MAP) / sizeof(TWI_GPIO_MAP[0]));
// options settings
opt.pba_hz = FOSC0;
opt.speed = TWI_SPEED;
opt.chip = 0x50;
// initialize TWI driver with options
// status = twi_master_init(&AVR32_TWI, &opt);
status = twi_master_init(TWI, &opt);
/* // check init result
if (status == TWI_SUCCESS)
print_dbg("\r\ni2c init");
else
print_dbg("\r\ni2c init FAIL");
*/
}
void init_i2c_slave(void) {
twi_options_t opt;
twi_slave_fct_t twi_slave_fct;
int status;
static const gpio_map_t TWI_GPIO_MAP = {
{AVR32_TWI_SDA_0_0_PIN, AVR32_TWI_SDA_0_0_FUNCTION},
{AVR32_TWI_SCL_0_0_PIN, AVR32_TWI_SCL_0_0_FUNCTION}
};
gpio_enable_module(TWI_GPIO_MAP, sizeof(TWI_GPIO_MAP) / sizeof(TWI_GPIO_MAP[0]));
// options settings
opt.pba_hz = FOSC0;
opt.speed = TWI_SPEED;
opt.chip = 0x50;
// initialize TWI driver with options
twi_slave_fct.rx = &twi_slave_rx;
twi_slave_fct.tx = &twi_slave_tx;
twi_slave_fct.stop = &twi_slave_stop;
status = twi_slave_init(&AVR32_TWI, &opt, &twi_slave_fct );
/*
// check init result
if (status == TWI_SUCCESS)
print_dbg("\r\ni2c init");
else
print_dbg("\r\ni2c init FAIL");
*/
}
void main(void)
{
// Declare your local variables here
// Crystal Oscillator division factor: 1
#pragma optsize-
CLKPR=(1<<CLKPCE);
CLKPR=(0<<CLKPCE) | (0<<CLKPS3) | (0<<CLKPS2) | (0<<CLKPS1) | (0<<CLKPS0);
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif
// Input/Output Ports initialization
// Port B initialization
// Function: Bit7=In Bit6=In Bit5=In Bit4=In Bit3=In Bit2=In Bit1=In Bit0=In
DDRB=(0<<DDB7) | (0<<DDB6) | (0<<DDB5) | (0<<DDB4) | (0<<DDB3) | (0<<DDB2) | (0<<DDB1) | (0<<DDB0);
// State: Bit7=T Bit6=T Bit5=T Bit4=T Bit3=T Bit2=T Bit1=T Bit0=T
PORTB=(0<<PORTB7) | (0<<PORTB6) | (0<<PORTB5) | (0<<PORTB4) | (0<<PORTB3) | (0<<PORTB2) | (0<<PORTB1) | (0<<PORTB0);
// Port C initialization
// Function: Bit6=In Bit5=In Bit4=In Bit3=In Bit2=In Bit1=In Bit0=In
DDRC=(0<<DDC6) | (0<<DDC5) | (0<<DDC4) | (0<<DDC3) | (0<<DDC2) | (0<<DDC1) | (0<<DDC0);
// State: Bit6=T Bit5=T Bit4=T Bit3=T Bit2=P Bit1=P Bit0=P
PORTC=(0<<PORTC6) | (0<<PORTC5) | (0<<PORTC4) | (0<<PORTC3) | (1<<PORTC2) | (1<<PORTC1) | (1<<PORTC0);
// Port D initialization
// Function: Bit7=In Bit6=In Bit5=Out Bit4=Out Bit3=Out Bit2=Out Bit1=In Bit0=In
DDRD=(0<<DDD7) | (0<<DDD6) | (1<<DDD5) | (1<<DDD4) | (1<<DDD3) | (1<<DDD2) | (0<<DDD1) | (0<<DDD0);
// State: Bit7=P Bit6=T Bit5=0 Bit4=0 Bit3=0 Bit2=T Bit1=T Bit0=T
PORTD=(1<<PORTD7) | (0<<PORTD6) | (0<<PORTD5) | (0<<PORTD4) | (0<<PORTD3) | (0<<PORTD2) | (0<<PORTD1) | (0<<PORTD0);
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 16000,000 kHz
// Mode: Fast PWM top=0xFF
// OC0A output: Non-Inverted PWM
// OC0B output: Disconnected
// Timer Period: 0,016 ms
// Output Pulse(s):
// OC0A Period: 0,016 ms Width: 0 us
TCCR0A=(1<<COM0A1) | (0<<COM0A0) | (0<<COM0B1) | (0<<COM0B0) | (1<<WGM01) | (1<<WGM00);
TCCR0B=(0<<WGM02) | (0<<CS02) | (0<<CS01) | (1<<CS00);
TCNT0=0x00;
OCR0A=0x00;
OCR0B=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 16000,000 kHz
// Mode: Normal top=0xFFFF
// OC1A output: Disconnected
// OC1B output: Disconnected
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer Period: 4,096 ms
// Timer1 Overflow Interrupt: On
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=(0<<COM1A1) | (0<<COM1A0) | (0<<COM1B1) | (0<<COM1B0) | (0<<WGM11) | (0<<WGM10);
TCCR1B=(0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (0<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10);
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 16000,000 kHz
// Mode: Fast PWM top=0xFF
// OC2A output: Disconnected
// OC2B output: Non-Inverted PWM
// Timer Period: 0,016 ms
// Output Pulse(s):
// OC2B Period: 0,016 ms Width: 8,0314 us
ASSR=(0<<EXCLK) | (0<<AS2);
TCCR2A=(0<<COM2A1) | (0<<COM2A0) | (1<<COM2B1) | (0<<COM2B0) | (1<<WGM21) | (1<<WGM20);
TCCR2B=(0<<WGM22) | (0<<CS22) | (0<<CS21) | (1<<CS20);
TCNT2=0x00;
OCR2A=0x00;
OCR2B=0x00;
// Timer/Counter 0 Interrupt(s) initialization
TIMSK0=(0<<OCIE0B) | (0<<OCIE0A) | (1<<TOIE0);
// Timer/Counter 1 Interrupt(s) initialization
TIMSK1=(0<<ICIE1) | (0<<OCIE1B) | (0<<OCIE1A) | (1<<TOIE1);
// Timer/Counter 2 Interrupt(s) initialization
TIMSK2=(0<<OCIE2B) | (0<<OCIE2A) | (0<<TOIE2);
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// Interrupt on any change on pins PCINT0-7: Off
// Interrupt on any change on pins PCINT8-14: Off
// Interrupt on any change on pins PCINT16-23: Off
EICRA=(0<<ISC11) | (0<<ISC10) | (0<<ISC01) | (0<<ISC00);
EIMSK=(0<<INT1) | (0<<INT0);
PCICR=(0<<PCIE2) | (0<<PCIE1) | (0<<PCIE0);
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART0 Mode: Asynchronous
// USART Baud Rate: 9600
UCSR0A=(0<<RXC0) | (0<<TXC0) | (0<<UDRE0) | (0<<FE0) | (0<<DOR0) | (0<<UPE0) | (0<<U2X0) | (0<<MPCM0);
UCSR0B=(0<<RXCIE0) | (0<<TXCIE0) | (0<<UDRIE0) | (1<<RXEN0) | (1<<TXEN0) | (0<<UCSZ02) | (0<<RXB80) | (0<<TXB80);
UCSR0C=(0<<UMSEL01) | (0<<UMSEL00) | (0<<UPM01) | (0<<UPM00) | (0<<USBS0) | (1<<UCSZ01) | (1<<UCSZ00) | (0<<UCPOL0);
UBRR0H=0x00;
UBRR0L=0x67;
// Analog Comparator initialization
// Analog Comparator: On
// The Analog Comparator's positive input is
// connected to the AIN0 pin
// The Analog Comparator's negative input is
// connected to the AIN1 pin
// Interrupt on Rising Output Edge
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=(0<<ACD) | (0<<ACBG) | (0<<ACO) | (0<<ACI) | (1<<ACIE) | (0<<ACIC) | (1<<ACIS1) | (1<<ACIS0);
ADCSRB=(0<<ACME);
// Digital input buffer on AIN0: On
// Digital input buffer on AIN1: On
DIDR1=(0<<AIN0D) | (0<<AIN1D);
// ADC initialization
// ADC disabled
ADCSRA=(0<<ADEN) | (0<<ADSC) | (0<<ADATE) | (0<<ADIF) | (0<<ADIE) | (0<<ADPS2) | (0<<ADPS1) | (0<<ADPS0);
// SPI initialization
// SPI disabled
SPCR=(0<<SPIE) | (0<<SPE) | (0<<DORD) | (0<<MSTR) | (0<<CPOL) | (0<<CPHA) | (0<<SPR1) | (0<<SPR0);
// TWI initialization
// Mode: TWI Slave
// Match Any Slave Address: Off
// I2C Bus Slave Address: 0x21
twi_slave_init(false,0x21,twi_rx_buffer,sizeof(twi_rx_buffer),twi_tx_buffer,twi_rx_handler,twi_tx_handler);
// Alphanumeric LCD initialization
// Connections are specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTB Bit 0
// RD - PORTB Bit 7
// EN - PORTB Bit 1
// D4 - PORTB Bit 2
// D5 - PORTB Bit 3
// D6 - PORTB Bit 4
// D7 - PORTB Bit 5
// Characters/line: 16
lcd_init(16);
lcd_putsf(" WELCOME! ");
// Global enable interrupts
ACSR |= (1<<ACIE); // включить перывания от компарат
ACSR &= ~(1<<ACIE); // выкл прер компар
vyh_t0 = 0; // выкл таймер 0
TCCR0A &= ~(1<<CS00);
procfreq = 16000000 ;
ton1 = fdel(1080);
for (n = 0; n < 32; n++)
{
sost[n] = esost[n];
twi_tx_buffer[n] = sost[n];
}
//PORTD.4 = 1;
//delay_ms(100);
//PORTD.4 = 0;
//PORTD.2 = 1;
#asm("sei")
//vyh_plus = 1;
OCR2B=0x00; // выключение преобразователя
// time = 0;
// while (time < 1000) #asm("wdr"); // задержка 25 мСек
//
// OCR2B=0x50; // выключение преобразователя
//
// time = 0;
// while (time < 1000) #asm("wdr"); // задержка 25 мСек
//
//OCR2B=0x50; // выключение преобразователя
//
// time = 0;
// while (time < 1000) #asm("wdr"); // задержка 25 мСек
// vyh_plus = 0;
// vyh_minus = 1;
//
//OCR2B=0xB0; // выключение преобразователя
//
// time = 0;
// while (time < 1000) #asm("wdr"); // задержка 25 мСек
//
//
//OCR2B=0xFF; // выключение преобразователя
//
while (1)
{
for (i = 0 ; i < 128 ; i++)
{
if(kn1&kn3) nagh = 0; // сброс флага нажатия при отпущеной кнопке
if(!kn1)
{
if(!nagh) // если кнопка только-что нажата
{
nagh = 1;
lcd_gotoxy(3,0);
if (fix)
{
fix = 0; // если была включена фиксация, - выключить
lcd_putchar(' ');
}
else
{
fix = 1; // включить фиксацию
page = (i / 32); // на какой странице зафиксировано
if (page == 0) page = 4;
page--;
page *= 32;
pos = 0;
x_dysp = 5;
y_dysp = 0;
lcd_putchar('F');
}
}
}
if(fix)
{
if(p10++ == 0)
{
// отображение подч
lcd_gotoxy(x_dysp,y_dysp);
lcd_putchar('_');
}
if(p10 == 8)
{
// отображение текущего состояние точки n
sig_bayt = (pos+page) / 4; // текущий байт в слове состояния
sig_bit = ((pos + page) - (sig_bayt * 4)) * 2; // текущая двухбитовая пара в слове состояния
lcd_gotoxy(x_dysp,y_dysp);
if((sost[sig_bayt]&(1<<sig_bit)))
{
if((sost[sig_bayt]&(1<<(sig_bit+1)))) lcd_putchar(255);
else lcd_putchar('.');
} else lcd_putchar(' ');
}
if(p10 > 16)
{
p10 = 0;
if(!kn2)
{
if(++pos > 30)
{
pos = 0;
x_dysp = 3;
y_dysp = 0;
}
else if(pos == 15) pos++;
x_dysp++;
if(++x_dysp > 15)
{
y_dysp++;
x_dysp = 1;
}
}
if(!kn3)
{
if(!nagh) // если кнопка только-что нажата
{
nagh = 1;
// включение - выключение охраны
lcd_gotoxy(x_dysp,y_dysp);
sig_bayt = (pos+page) / 4; // текущий байт в слове состояния
sig_bit = ((pos + page) - (sig_bayt * 4)) * 2; // текущая двухбитовая пара в слове состояния
if((sost[sig_bayt]&(1<<sig_bit)))
{
sost[sig_bayt] &= ~(1<<sig_bit); // выкл. охрану
lcd_putchar(' ');
} else
{
sost[sig_bayt] |= (1<<sig_bit); // включаем охрану
lcd_putchar('.');
}
}
}
}
}
sig_bayt = i / 4; // текущий байт в слове состояния
sig_bit = (i - sig_bayt * 4) * 2; // текущая двухбитовая пара в слове состояния
OCR0A=0x00;
a = 49728;
TCCR0A |= (1<<CS00); // включить таймер 0;
TIMSK0 |= (1<<TOIE0); //
time = 0;
while (time < 6) #asm("wdr"); // задержка 25 мСек
zvuk = 0;
if(i == 0) while (time < 75) #asm("wdr"); // если первый шаг, задержка 300 мСек
TIMSK0 &= ~(1<<TOIE0); //
OCR0A=0x00;
a = 49728;
TCCR0A &= ~(1<<CS00); // выкл таймер 0
vyh_t0 = 0; // переключить а вход
ACSR |= (1<<ACIE); // включить перывания от компарат
time = 0;
while (time < 9) #asm("wdr"); // 25мСек + 12
ACSR &= ~(1<<ACIE); // выкл прер компар
vyh_t0 = 1; // переключить а вyход
if (flag435) // если был отлет 435 Гц
{
switch (i)
{
case 31:
// вывод первой половины на дисп
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,0);
lcd_putsf("1 ");
n = 1;
for (sig_bayt = 0; sig_bayt < 8 ; sig_bayt++)
{
for(sig_bit = 0; (sig_bayt == 3 || sig_bayt == 7)? sig_bit < 6: sig_bit < 8; sig_bit += 2)
{
if(++n>10) n = 1;
lcd_putchar(47+n);
if((sost[sig_bayt]&(1<<sig_bit)))
{
if((sost[sig_bayt]&(1<<(sig_bit+1)))) lcd_putchar(255);
else lcd_putchar('.');
} else lcd_putchar(' ');
}
}
}
pit_ok = 1;
while (time < 125) #asm("wdr"); // задержка до 0.5 сек
break;
case 15:
case 47:
case 63:
case 79:
case 95:
case 111:
case 127:
sinc_err = 1; // ошибка синхронизации
zvuk = 1;
break;
default:
if (sost[sig_bayt]&(1<<sig_bit)) // если под охраной
{
if ((esost[sig_bayt]&(1<<sig_bit) == 0))
{
//если не был под охр. поставить под охрану.
OCR2B=0x50; // выдача на шим
vyh_plus = 1;
vyh_t0 = 0; // переключить а вход
n = 0;
while(flag435)
{
flag435 = 0;
ACSR |= (1<<ACIE); // включить перывания от компарат
time = 0;
while (time < 9) #asm("wdr"); // 25мСек + 12
ACSR &= ~(1<<ACIE); // выкл прер компар
//Ждем прекращения сигнала
if (++n > 250) flag435 = 0;
}
vyh_t0 = 1; // переключить Hа вyход
if(n < 251)
{
esost[sig_bayt] |= (1<<sig_bit); // включаем охрану если сигнал прервался
twi_tx_buffer[sig_bayt] = esost[sig_bayt]; // передача состояния
}
else sost[sig_bayt] &= ~(3<<sig_bit); // если не взялась, - снять
}
else
{
// если объекм был по охраной тревога
sost[sig_bayt] |= (1<<(sig_bit+1));
esost[sig_bayt] |= (1<<(sig_bit+1));
twi_tx_buffer[sig_bayt] = esost[sig_bayt]; // передача состояния
trevoga = 1;
zvuk = 1;
}
while(time<375) #asm("wdr");
vyh_plus = 0;
OCR2B=0xFF;
}
else if (esost[sig_bayt]&(1<<sig_bit)) // не под охр, но был
{
esost[sig_bayt] &= ~(3<<sig_bit); // если был под охр. снять с охр.
twi_tx_buffer[sig_bayt] = esost[sig_bayt]; // передача состояния
}
}
}
else // если не было ответа
{
switch (i)
{
case 31:
// вкл питание
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,1);
lcd_putsf("Error Pitanie!!!");
}
pit_err = 1;
pit_ok = 0;
break;
case 63:
// вывод второй половины на дисп
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,0);
lcd_putsf("31 ");
n = 1;
for (sig_bayt = 8; sig_bayt < 16 ; sig_bayt++)
{
for(sig_bit = 0; (sig_bayt == 11 || sig_bayt == 15)? sig_bit < 6: sig_bit < 8; sig_bit += 2)
{
if(++n>10) n = 1;
lcd_putchar(47+n);
if((sost[sig_bayt]&(1<<sig_bit)))
{
if((sost[sig_bayt]&(1<<(sig_bit+1)))) lcd_putchar(255);
else lcd_putchar('.');
} else lcd_putchar(' ');
}
}
}
break;
case 95:
// вывод третьей половины на дисп
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,0);
lcd_putsf("61 ");
n = 1;
for (sig_bayt = 16; sig_bayt < 24 ; sig_bayt++)
{
for(sig_bit = 0; (sig_bayt == 19 || sig_bayt == 23)? sig_bit < 6: sig_bit < 8; sig_bit += 2)
{
if(++n>10) n = 1;
lcd_putchar(47+n);
if((sost[sig_bayt]&(1<<sig_bit)))
{
if((sost[sig_bayt]&(1<<(sig_bit+1)))) lcd_putchar(255);
else lcd_putchar('.');
} else lcd_putchar(' ');
}
}
}
break;
case 127:
// вывод четвертой половины на дисп
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,0);
lcd_putsf("91 ");
n = 1;
for (sig_bayt = 24; sig_bayt < 32; sig_bayt++)
{
for(sig_bit = 0; (sig_bayt == 27 || sig_bayt == 31)? sig_bit < 6: sig_bit < 8; sig_bit += 2)
{
if(++n>10) n = 1;
lcd_putchar(47+n);
if((sost[sig_bayt]&(1<<sig_bit)))
{
if((sost[sig_bayt]&(1<<(sig_bit+1)))) lcd_putchar(255);
else lcd_putchar('.');
} else lcd_putchar(' ');
}
}
}
break;
case 15:
case 47:
case 79:
case 111:
break;
default:
if ((sost[sig_bayt]&(1<<sig_bit)) == 0)
{
if (esost[sig_bayt]&(1<<sig_bit)) // не под охр, но был
{
//если был под охр. снять с охр.
esost[sig_bayt] &= ~(3<<sig_bit); // снять с охр.
twi_tx_buffer[sig_bayt] = esost[sig_bayt]; // передача состояния
}
}
else if ((esost[sig_bayt]&(1<<sig_bit)) == 0) //если под охраной, но не был
{
esost[sig_bayt] |= (1<<sig_bit); // включаем охрану
twi_tx_buffer[sig_bayt] = esost[sig_bayt]; // передача состояния
}
}
}
flag435 = 0;
if (pit_ok)
{
while (time < 125) #asm("wdr");
pit_ok = 0;
}
// if (trevoga) //while (time < 375);
// trevoga = 0;
if (sinc_err)
{
sinc_err = 0;
if (!fix)
{
clear_lcd();
lcd_gotoxy(0,1);
lcd_putsf(" Error Sync!!! ");
} else
{
lcd_gotoxy(3,0);
lcd_putchar('F');
}
break;
}
}
}
}
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;
}
