int16_t main(void)
{
// Initialize IO ports and peripherals.
ConfigureOscillator();
InitApp();
InitAdc();
AtpInit();
odo_init(); // TODO
motion_init(SendDone);
SendBoardId();
__delay_ms(3000);
SendBoardId();
while(1) {
//SendBoardId();
if (odoBroadcast) {
OnGetPos();
}
__delay_ms(odoDelay);
}
}
void
main(void)
{
twi_master_init();
mpu6500_init();
mpu6500_stop();
disable_i2c();
simble_init("Motion");
motion_ctx.sampling_period = DEFAULT_SAMPLING_PERIOD;
//Set the timer parameters and initialize it.
struct rtc_ctx rtc_ctx = {
.rtc_x[NOTIF_TIMER_ID] = {
.type = PERIODIC,
.period = motion_ctx.sampling_period,
.enabled = false,
.cb = notif_timer_cb,
}
};
batt_serv_init(&rtc_ctx);
rtc_init(&rtc_ctx);
ind_init();
motion_init(&motion_ctx);
simble_adv_start();
simble_process_event_loop();
}
handle_t mug_motion_init()
{
handle_t handle = mug_init(DEVICE_MPU);
motion_init(handle);
return handle;
}
int16_t main(void)
{
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
Init_PWM();
Init_QEI();
AtpInit();
motion_init(SendDone);
SendBoardId();
// Petit blink kikou au démarrage
int i;
for (i = 0 ; i < 14 ; i++) {
__delay_ms(50);
led = led ^ 1;
}
SendBoardId();
//pour les AX12
responseReadyAX = 0;
while (1){
if (odoBroadcast) {
OnGetPos();
}
__delay_ms(odoBroadcastDelay);
}
}
/**
* \brief Initializes the real time system
*/
int main(void) {
//Turn on wifi hotspot, id "ChicoTheStalker"
portENABLE_INTERRUPTS();
// Initialize USART for WiFi and LCD
initializeUSART();
gs_initialize_module(USART_2, BAUD_RATE_9600, USART_0, BAUD_RATE_115200);
gs_set_wireless_ssid("ChicoTheStalker");
gs_activate_wireless_connection();
/* Web server config */
configure_web_page("ChicoTheStalker", "~~ Control Interface ~~", HTML_DROPDOWN_LIST);
// Switching modes
add_element_choice('A', "Attachment Mode");
add_element_choice('C', "Command Mode");
// Command mode options
add_element_choice('0', "Stop");
add_element_choice('1', "Forward");
add_element_choice('2', "Backward");
add_element_choice('3', "Rotate Left");
add_element_choice('4', "Rotate Right");
add_element_choice('5', "Show Temperature");
add_element_choice('6', "Show Distance");
start_web_server();
// initialize motion
motion_init();
// Initialize thermal sensor
initializeSensor();
// Declare tasks
xTaskCreate(handleHttpRequestsTask, (const portCHAR *)"Handle server requests", 800, NULL /* parameters */,0, &taskHandles[0]);
currentCommand = COMMAND_MODE;
display("Command mode", "Wait for cmds.");
vTaskStartScheduler();
displayToTerminal("\r\n\n\nGoodbye... no space for idle task!\r\n"); // Doh, so we're dead...
}
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;
}
void
camera_start(void)
{
MMAL_STATUS_T status;
motion_init();
circular_buffer_init();
camera_create();
/* ====== Create the camera preview port path ====== :
| preview --(tunnel)--> resizer --> I420_callback --> jpeg_encoder --> mjpeg_callback
| (draws on frame) (writes stream mjpeg.jpg)
*/
pikrellcam.mjpeg_height = pikrellcam.mjpeg_width *
pikrellcam.camera_config.video_height / pikrellcam.camera_config.video_width;
pikrellcam.mjpeg_width &= ~0xf; /* Make resize multiple of 16 */
pikrellcam.mjpeg_height &= ~0xf;
resizer_create("stream_resizer", &stream_resizer,
camera.component->output[CAMERA_PREVIEW_PORT],
pikrellcam.mjpeg_width, pikrellcam.mjpeg_height);
ports_tunnel_connect(&camera, CAMERA_PREVIEW_PORT, &stream_resizer);
jpeg_encoder_create("mjpeg_encoder", &mjpeg_encoder,
stream_resizer.component->output[0], pikrellcam.mjpeg_quality);
ports_callback_connect(&stream_resizer, 0, &mjpeg_encoder,
I420_video_callback);
out_port_callback(&mjpeg_encoder, 0, mjpeg_callback);
/* ====== Create the camera still port path ====== :
| camera_capture --(tunnel)--> jpeg_encoder --> still_jpeg__callback
| (writes stills and timelapse jpegs)
*/
jpeg_encoder_create("still_jpeg_encoder", &still_jpeg_encoder,
NULL, pikrellcam.camera_adjust.still_quality);
ports_tunnel_connect(&camera, CAMERA_CAPTURE_PORT, &still_jpeg_encoder);
out_port_callback(&still_jpeg_encoder, 0, still_jpeg_callback);
/* ====== Create the camera video port path ====== :
| camera_video--(tunnel)-->h264 encoder-->video_h264_encoder_callback
| (writes data into video circular buffer)
| (records video / checks motion vectors)
| (schedules mjpeg.jpg copy into previews)
*/
h264_encoder_create("video_h264_encoder", &video_h264_encoder, NULL);
ports_tunnel_connect(&camera, CAMERA_VIDEO_PORT, &video_h264_encoder);
out_port_callback(&video_h264_encoder, 0, video_h264_encoder_callback);
time(&pikrellcam.t_start);
/* Turn on the video stream. It free runs into the video circular buffer.
*/
if ((status = mmal_port_parameter_set_boolean(
camera.component->output[CAMERA_VIDEO_PORT],
MMAL_PARAMETER_CAPTURE, 1)) != MMAL_SUCCESS)
log_printf("Video capture startup failed. Status %s\n",
mmal_status[status]);
/* With everything created and running, set the config'ed camera params.
*/
mmalcam_config_parameters_set_camera();
display_init();
video_circular_buffer.state = VCB_STATE_NONE;
video_circular_buffer.pause = FALSE;
}
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;
}
int main (int argc, char *argv[]) {
/* set up i18n */
bindtextdomain (PACKAGE, LOCALEDIR);
setlocale (LC_ALL, "");
textdomain (PACKAGE);
GError* error = NULL;
GOptionContext* context = g_option_context_new ("files");
g_option_context_add_main_entries (context, entries, PACKAGE);
g_option_context_parse (context, &argc, &argv, &error);
if (error) g_error("%s\n", error->message);
/* initialize GTK */
g_thread_init (NULL);
gdk_threads_init ();
gtk_init (&argc, &argv);
GError* ui_error = NULL;
GtkBuilder* builder = gtk_builder_new ();
gchar* ui = g_build_filename (DATADIR, "ui", "gummi.glade", NULL);
// exit program when gummi.glade can not be located:
if (!g_file_test (ui, G_FILE_TEST_EXISTS)) {
printf("Could not locate Glade interface file at:\n%s\n", ui);
return 0;
}
gtk_builder_add_from_file (builder, ui, &ui_error);
if (ui_error) {
g_error ("%s\n", ui_error->message);
}
gtk_builder_set_translation_domain (builder, PACKAGE);
g_free (ui);
/* Initialize logging */
slog_init (debug);
slog (L_INFO, PACKAGE_NAME" version: "PACKAGE_VERSION"\n");
/* Initialize configuration */
gchar* configname = g_build_filename (g_get_user_config_dir (), "gummi",
"gummi.cfg", NULL);
config_init (configname);
config_load ();
g_free (configname);
/* Initialize signals */
gummi_signals_register ();
/* Initialize Classes */
gchar* snippetsname = g_build_filename (g_get_user_config_dir (), "gummi",
"snippets.cfg", NULL);
// why do we not load this from environment, like gui-main does? -A
GuMotion* motion = motion_init ();
GuIOFunc* io = iofunctions_init();
GuLatex* latex = latex_init ();
GuBiblio* biblio = biblio_init (builder);
GuTemplate* templ = template_init (builder);
GuTabmanager* tabm = tabmanager_init ();
GuProject* proj = project_init ();
GuSnippets* snippets = snippets_init (snippetsname);
gummi = gummi_init (motion, io, latex, biblio, templ, snippets, tabm, proj);
slog (L_DEBUG, "Gummi created!\n");
g_free (snippetsname);
/* Initialize GUI */
gui = gui_init (builder);
slog_set_gui_parent (gui->mainwindow);
slog (L_DEBUG, "GummiGui created!\n");
/* Start compile thread */
if (external_exists (config_get_value("typesetter"))) {
typesetter_setup ();
motion_start_compile_thread (motion);
}
else {
infoscreengui_enable (gui->infoscreengui, "program_error");
slog (L_ERROR, "Could not locate the typesetter program\n");
}
/* Install acceleration group to mainwindow */
gtk_window_add_accel_group (gui->mainwindow, snippets->accel_group);
if (argc != 2)
tabmanager_create_tab (A_DEFAULT, NULL, NULL);
else {
if (!g_file_test(argv[1], G_FILE_TEST_EXISTS)) {
slog(L_ERROR, "Failed to open file '%s': No such file or "
"directory\n", argv[1]);
exit(1);
}
tabmanager_create_tab (A_LOAD, argv[1], NULL);
}
if (config_get_value ("autosaving")) iofunctions_start_autosave ();
gui_main (builder);
config_save ();
config_clean_up ();
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();
#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;
}
