//The setup function is called once at startup of the sketch
void setup()
{
// Add your initialization code here
// Note : This will initialize Serial port on Arduino at 115200 bauds
OC_LOG_INIT();
OC_LOG(DEBUG, TAG, ("Demoserver is starting..."));
// Connect to Ethernet or WiFi network
if (ConnectToNetwork() != 0) {
OC_LOG(ERROR, TAG, ("Unable to connect to network"));
return;
}
// Initialize the OC Stack in Server mode
if (OCInit(NULL, 0, OC_SERVER) != OC_STACK_OK) {
OC_LOG(ERROR, TAG, ("OCStack init error"));
return;
}
// Initialize Grove related Devices
sensor_init();
led_init();
lcd_init();
buzzer_init();
button_init();
// Declare and create the resource: grove
createDemoResource();
}
void AppColdStart(void)
{
/* default startup */
init_hardware();
buzzer_init();
rgb_leds_init();
sseg_init();
process_init();
init_net();
/* start the main processes */
procinit_init();
autostart_start(autostart_processes);
jts_init();
/* enable watchdog on JN5148, there is none on JN5139 */
watchdog_start();
/* default main loop */
while(1)
{
process_run();
etimer_request_poll();
watchdog_periodic();
}
}
void Setup()
{
debug_level = 2;
//set clock to max for init 32MHz
ClockSetSource(x32MHz);
//disable 2MHZ osc
OSC.CTRL = 0b00000010;
//save power
turnoff_subsystems();
EnableInterrupts();
//init basic peripherals
led_init();
uart_init_buffers();
uart_init();
time_init();
buzzer_init();
battery_init();
buttons_init();
//basic power control
mems_power_init();
io_init();
SD_EN_INIT;
//load configuration
cfg_load();
_delay_ms(100);
}
void init(void)
{
clock_init();
led_init();
uart_init();
buzzer_init();
lcd_init();
}
/*---------------------------------------------------------------------------*/
void
board_init()
{
/* Disable global interrupts */
uint8_t int_disabled = ti_lib_int_master_disable();
power_domains_on();
/* Configure all clock domains to run at full speed */
ti_lib_prcm_clock_configure_set(PRCM_DOMAIN_SYSBUS, PRCM_CLOCK_DIV_1);
ti_lib_prcm_clock_configure_set(PRCM_DOMAIN_CPU, PRCM_CLOCK_DIV_1);
ti_lib_prcm_clock_configure_set(PRCM_DOMAIN_TIMER, PRCM_CLOCK_DIV_1);
ti_lib_prcm_clock_configure_set(PRCM_DOMAIN_SERIAL, PRCM_CLOCK_DIV_1);
ti_lib_prcm_clock_configure_set(PRCM_DOMAIN_PERIPH, PRCM_CLOCK_DIV_1);
/* Enable GPIO peripheral */
ti_lib_prcm_peripheral_run_enable(PRCM_PERIPH_GPIO);
/* Apply settings and wait for them to take effect */
ti_lib_prcm_load_set();
while(!ti_lib_prcm_load_get());
/* Enable GPT0 module - Wait for the clock to be enabled */
ti_lib_prcm_peripheral_run_enable(PRCM_PERIPH_TIMER0);
ti_lib_prcm_load_set();
while(!ti_lib_prcm_load_get());
/* Keys (input pullup) */
ti_lib_rom_ioc_pin_type_gpio_input(BOARD_IOID_KEY_LEFT);
ti_lib_rom_ioc_pin_type_gpio_input(BOARD_IOID_KEY_RIGHT);
ti_lib_ioc_io_port_pull_set(BOARD_IOID_KEY_LEFT, IOC_IOPULL_UP);
ti_lib_ioc_io_port_pull_set(BOARD_IOID_KEY_RIGHT, IOC_IOPULL_UP);
/* I2C controller */
board_i2c_init();
/* Sensor interface */
ti_lib_rom_ioc_pin_type_gpio_input(BOARD_IOID_MPU_INT);
ti_lib_ioc_io_port_pull_set(BOARD_IOID_MPU_INT, IOC_IOPULL_DOWN);
ti_lib_rom_ioc_pin_type_gpio_input(BOARD_IOID_REED_RELAY);
ti_lib_ioc_io_port_pull_set(BOARD_IOID_REED_RELAY, IOC_IOPULL_DOWN);
ti_lib_rom_ioc_pin_type_gpio_output(BOARD_IOID_MPU_POWER);
/* Flash interface */
ti_lib_rom_ioc_pin_type_gpio_output(BOARD_IOID_FLASH_CS);
ti_lib_gpio_pin_write(BOARD_FLASH_CS, 1);
buzzer_init();
lpm_register_module(&sensortag_module);
/* Re-enable interrupt if initially enabled. */
if(!int_disabled) {
ti_lib_int_master_enable();
}
}
void sh_cmd1(void)
{
// time_tt t = {};
// clean_lcd(BACK_COL);
// gettime(&t);
buzzer_init();
buzzer_on();
udelay(1000*1000);
buzzer_off();
udelay(1000*1000);
}
int main()
{
buzzer_init();
buzzer_ON();
delay();
buzzer_OFF();
delay();
buzzer_ON();
delay();
buzzer_OFF();
delay();
return 0;
}
/** \brief Einsprungspunkt */
int main(void)
{
buttons_init();
adc_init();
timer_init();
buzzer_init(4000);
lcd_init();
machine_state_init();
irq_enable();
while (1)
handle_events();
}
int main()
{
/* Hardware Initialization */
buzzer_init();
button_init();
ui_init();
ADC3_CH13_Config();
ADC_SoftwareStartConv(ADC3);
/* Start to schedule */
rtenv_start_scheduler(first);
return 0;
}
static void init()
{
// OSCCAL = 71;
clock_prescale_set(CPU_DIV);
// power_twi_disable();
// power_usart0_disable();
// power_timer0_disable();
// power_timer1_disable();
// power_timer2_disable();
// power_adc_disable();
#if !UART_ENABLE
power_usart0_disable();
#endif
// Pull-up on unused pins
pinPullup(D0, PULLUP_ENABLE);
pinPullup(D1, PULLUP_ENABLE);
pinPullup(D3, PULLUP_ENABLE);
pinPullup(D4, PULLUP_ENABLE);
pinPullup(B7, PULLUP_ENABLE);
#if PIN_DEBUG != PIN_DEBUG_NONE
pinMode(PIN_DEBUG_PIN, OUTPUT);
#endif
// Pin change interrupt on USB power sense pin
PCICR |= _BV(PCIE0);
PCMSK0 |= _BV(PCINT6);
// Everything else
uart_init();
spi_init();
i2c_init();
watchconfig_init();
led_init();
buzzer_init();
battery_init();
ds3231_init();
buttons_init();
millis_init();
pwrmgr_init();
time_init();
alarm_init();
oled_init();
}
void Setup()
{
//set clock to max for init 32MHz
ClockSetSource(x32MHz);
//disable 2MHZ osc
OSC.CTRL = 0b00000010;
//get RAM info
free_ram_at_start = freeRam();
//get reset reason
system_rst = RST.STATUS;
RST.STATUS = 0b00111111;
//save power - peripherals are turned on on demand by drivers
turnoff_subsystems();
EnableInterrupts();
//load device id
GetID();
//init basic peripherals
led_init();
uart_init_buffers();
time_init();
buzzer_init();
battery_init();
buttons_init();
//basic power control
mems_power_init();
io_init();
SD_EN_INIT;
//load configuration from EE
cfg_load();
uart_init();
_delay_ms(100);
}
int test(void)
{
int i = 0;
led_init();
buzzer_init();
k1_init();
while(1)
{
if(k1_is_down())
{
buzzer_on();
led_on(i%4 + 1);
delay(1);
buzzer_off();
led_off(i%4 + 1);
delay(1);
i++;
}
}
return 0;
}
static __init int uart_mod_init(void)
{
int rc;
printk("uart module init ok!\n");
printk("v = %d\n", v);
printk("jiffies = 0x%x\n", (int)jiffies);
printk("HZ = %d\n", HZ);
uart_dev = MKDEV(241, 0);
rc = register_chrdev_region(uart_dev, 4, "ttyS");
DPRINT(rc);
cdev_init(&uart_cdev, &uart_fops);
cdev_add(&uart_cdev, uart_dev, 4);
buzzer_init();
buzzer_beep(1);
return 0;
}
/**************************************************************************//**
* \brief Initialize QTouch.
******************************************************************************/
void BSP_InitQTouch(BSP_TouchEventHandler_t handler)
{
/* initialise host app, pins, watchdog, etc */
init_system();
/* Reset touch sensing */
qt_reset_sensing();
/*Configure Burst Length*/
burst_len_config();
config_sensors();
/* Initialise and set touch params */
qt_init_sensing();
qt_set_parameters();
init_timer_isr();
buzzer_init();
/* Address to pass address of user functions */
/* This function is called after the library has made capacitive
* measurements,
* but before it has processed them. The user can use this hook to
* apply filter
* functions to the measured signal values.(Possibly to fix sensor
* layout faults) */
/* This function is also used to send signal values to simulate Accelero
* meter,
* Just for demo purpose */
qt_filter_callback = qt_avr477_filter_cb;
cpu_irq_enable();
handler = handler;
}
int mymain(void)
{
int nand_addr;
int sdram_addr;
int sec;
sec = 1000000;
uart_init();
nand_init();
buzzer_init();
nand_addr = LED_NAND_ADDR; //16K
sdram_addr = LED_DRAM_ADDR;
while(sec){
buzzer_ring();
delay(sec);
buzzer_slient();
delay(sec);
if(sec >= 100000)
sec -= 100000;
else
sec -= 5;
}
if(*(int *)0xE2900010 & 0x1){
nand_read((char *)sdram_addr, nand_addr, LED_NAND_SIZE);
go(sdram_addr);
}
else
help();
//load_shell();
return 0;
}
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
void main_init_generic(void)
{
// Reset to safe values
global_data_reset();
// Load default eeprom parameters as fallback
global_data_reset_param_defaults();
// LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS
hw_init();
enableIRQ();
led_init();
led_on(LED_GREEN);
buzzer_init();
sys_time_init();
sys_time_periodic_init();
sys_time_clock_init();
ppm_init();
pwm_init();
// Lowlevel periphel support init
adc_init();
// FIXME SDCARD
// MMC_IO_Init();
spi_init();
i2c_init();
// Sensor init
sensors_init();
debug_message_buffer("Sensor initialized");
// Shutter init
shutter_init();
shutter_control(0);
// Debug output init
debug_message_init();
debug_message_buffer("Text message buffer initialized");
// MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS
// Try to reach the EEPROM
eeprom_check_start();
// WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT
while (sys_time_clock_get_time_usec() < 2000000)
{
}
// Do the auto-gyro calibration for 1 second
// Get current temperature
led_on(LED_RED);
gyro_init();
// uint8_t timeout = 3;
// // Check for SD card
// while (sys_time_clock_get_time_usec() < 2000000)
// {
// while (GetDriveInformation() != F_OK && timeout--)
// {
// debug_message_buffer("MMC/SD-Card not found ! retrying..");
// }
// }
//
// if (GetDriveInformation() == F_OK)
// {
// debug_message_buffer("MMC/SD-Card SUCCESS: FOUND");
// }
// else
// {
// debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND");
// }
//FIXME redo init because of SD driver decreasing speed
//spi_init();
led_off(LED_RED);
// Stop trying to reach the EEPROM - if it has not been found by now, assume
// there is no EEPROM mounted
if (eeprom_check_ok())
{
param_read_all();
debug_message_buffer("EEPROM detected - reading parameters from EEPROM");
for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++)
{
param_handler();
//sleep 1 ms
sys_time_wait(1000);
}
}
else
{
debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH");
}
// Set mavlink system
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID];
//Magnet sensor
hmc5843_init();
acc_init();
// Comm parameter init
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255
mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255
// Comm init has to be
// AFTER PARAM INIT
comm_init(MAVLINK_COMM_0);
comm_init(MAVLINK_COMM_1);
// UART initialized, now initialize COMM peripherals
communication_init();
gps_init();
us_run_init();
servos_init();
//position_kalman3_init();
// Calibration starts (this can take a few seconds)
// led_on(LED_GREEN);
// led_on(LED_RED);
// Read out first time battery
global_data.battery_voltage = battery_get_value();
global_data.state.mav_mode = MAV_MODE_PREFLIGHT;
global_data.state.status = MAV_STATE_CALIBRATING;
send_system_state();
float_vect3 init_state_accel;
init_state_accel.x = 0.0f;
init_state_accel.y = 0.0f;
init_state_accel.z = -1000.0f;
float_vect3 init_state_magnet;
init_state_magnet.x = 1.0f;
init_state_magnet.y = 0.0f;
init_state_magnet.z = 0.0f;
//auto_calibration();
attitude_observer_init(init_state_accel, init_state_magnet);
debug_message_buffer("Attitude Filter initialized");
led_on(LED_RED);
send_system_state();
debug_message_buffer("System is initialized");
// Calibration stopped
led_off(LED_RED);
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
global_data.state.status = MAV_STATE_STANDBY;
send_system_state();
debug_message_buffer("Checking if remote control is switched on:");
// Initialize remote control status
remote_control();
remote_control();
if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok)
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED;
debug_message_buffer("RESULT: remote control switched ON");
debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens");
led_on(LED_GREEN);
}
else
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
debug_message_buffer("RESULT: remote control switched OFF");
led_off(LED_GREEN);
}
}
int peripheralInit()
{
buzzer_init();
ledswi_initLedSwitch(1,3);
display_7segments_initDisplays();
lcd_initLcd();
cooler_initForPwm();
heater_initForPwm();
adc_initAdc();
tach_init();
SIM_BASE_PTR->SCGC5 |= SIM_SCGC5_PORTB_MASK;
PORTB_BASE_PTR->PCR[18] = PORTB_BASE_PTR->PCR[19] = PORT_PCR_MUX(3);
PTB_BASE_PTR->PDDR = 0b11 << 19;
tpm_config_t tpmConfig =
{
.eClock_source = McgPllFllClk,
.uiPeriod_us = 1000,
.ePrescaler_value = Prescaler128,
.uiXtal_frequency = 40000000,
.eAlignment = Edge
};
/* Configure for 50% Duty Cycle */
channel_config_t channelConfig_cooler =
{
.eChannelOutput = NoInversion,
.uiChannel = 1,
.uiInterrupt_enable = 0,
.uiPulse_width_us = 500
};
pwm_initPwm(TPM1, tpmConfig);
pwm_channelInit (TPM1, tpmConfig, channelConfig_cooler);
interpreter_init();
tc_installLptmr0(CYCLIC_EXECUTIVE_PERIOD, main_cyclicExecuteIsr);
}
int main(void)
{
boardInit();
peripheralInit();
/* Set Red LED for Status */
SIM_SCGC5 |= (SIM_SCGC5_PORTB_MASK);
PORTB_PCR18 = PORT_PCR_MUX(1);
PTB_BASE_PTR->PDDR = 1 << 18;
/* Local variables */
char cTachReadout[15];
uint16_t uiTachValue;
/* Main loop */
for (;;) {
/* Blink Red LED for Status */
PTB_BASE_PTR->PTOR = 1 << 18;
/* Serial CLI */
interpreter_readCommand();
/* Get temperature */
PRINTF("%d\r\n", adc_getValue());
/* Read and print tach value */
uiTachValue = tach_Hz(CYCLIC_EXECUTIVE_PERIOD/1000);
lcd_itoa(uiTachValue, cTachReadout);
lcd_clearLine(0);
lcd_writeString(cTachReadout);
lcd_writeString(" Hz");
/* Wait for next cycle */
while(!uiFlagNextPeriod);
uiFlagNextPeriod = 0;
}
return 0;
}
int
main(int argc, char* argv[])
{
/************** VARIABLE DECLARATION */
KeyStruct keyPadState;
AlarmStruct state;
EdgeStruct sense;
char code[CODE_DIM+1];
DEFAULT_CODE(code);
bool isEquals;
/************* INIT ***************/
keypad_init(&keyPadState);
timer_start();
sense_init(&sense);
alarm_init(&state);
lcd_init();
buzzer_init();
/************** INFINITE LOOP *****/
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("SALVE PRF FOGGIA");
HD44780_GotoXY(0,1);
HD44780_PutStr("Alarm System 1.0");
keypad_flush(&keyPadState);
while(1){
state.eventsArray[MAGN1] = magn_one_read(&sense);
state.eventsArray[MAGN2] = magn_two_read(&sense);
state.eventsArray[MOVE1] = move_one_read(&sense);
state.eventsArray[MOVE2] = move_two_read(&sense);
state.eventsArray[MOVE3] = move_three_read(&sense);
state.eventsArray[MOVE4] = move_four_read(&sense);
state.eventsArray[KEYPAD] = get_code(&keyPadState);
/******** Update State ***********/
isEquals = true;
if(state.eventsArray[KEYPAD]){
pin_sound();
int i=0;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("PIN:");
HD44780_GotoXY(0,1);
alarm_off();
while(keyPadState.code[i] != '\0' && code[i] != '\0'){
HD44780_PutChar(keyPadState.code[i]);
(isEquals & (keyPadState.code[i] == code[i])) ? (isEquals = true) : (isEquals = false);
i++;
}
if(isEquals){
state.isActive = !state.isActive;
keypad_flush(&keyPadState);
if(state.isActive){
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("ALLARME");
HD44780_GotoXY(0,1);
HD44780_PutStr("ATTIVATO");
}else{
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("ALLARME");
HD44780_GotoXY(0,1);
HD44780_PutStr("DISATTIVATO");
}
}
if(!isEquals && keyPadState.index == CODE_DIM){
HD44780_GotoXY(0,1);
HD44780_PutStr("ERRATO");
keypad_flush(&keyPadState);
}
}
/******** Generate Outputs *******/
if(state.eventsArray[MAGN1] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("PORTA 1");
}
if(state.eventsArray[MAGN2] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("PORTA 2");
}
if(state.eventsArray[MOVE1] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("STANZA 1");
}
if(state.eventsArray[MOVE2] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("STANZA 2");
}
if(state.eventsArray[MOVE3] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("STANZA 3");
}
if(state.eventsArray[MOVE4] && state.isActive){
state.isRinging = true;
HD44780_ClrScr();
HD44780_GotoXY(0,0);
HD44780_PutStr("VIOLAZIONE");
HD44780_GotoXY(0,1);
HD44780_PutStr("STANZA 4");
}
if(!state.isActive){
alarm_off();
state.isRinging = false;
}
if(state.isRinging)
alarm_on();
if(keyPadState.prevChar == ENTER_CHAR)
change_pin(code, &keyPadState);
timer_sleep(1);
}
}
int main(void)
{
// local variables
int sensor_flag, command_flag, comm_status, sensor_process_flag, obstacle_flag, wait_flag;
unsigned long wait_timer = 0;
unsigned long wait_time = 0;
uint8 motion_flag = 0;
// TIMING: unsigned long timer1, timer2, timer3, timer4;
// Initialization Routines
led_init(); // switches all 6 LEDs on
serial_init(57600); // serial port at 57600 baud
buzzer_init(); // enable buzzer melodies
button_init(); // enable push buttons on CM-510
delay_ms(200); // wait 0.2s
led_off(ALL_LED); // and switch them back off
// initialize the clock
clock_init();
wait_flag = 0;
// enable interrupts
sei();
// print welcome message
printf("\nBioloid C Control V0.8\n");
printf("Press the START button on the CM-510 to continue.\n");
// reset the start button variable, something triggers the interrupt on start-up
start_button_pressed = FALSE;
// initialize motion pages
motionPageInit();
// Wait for the START Button before going any further
while(!start_button_pressed)
{
// PLAY LED is flashing at 5Hz
led_toggle(LED_PLAY);
delay_ms(200);
}
// Now turn LED solid
led_on(LED_PLAY);
// and reset the start button variable
start_button_pressed = FALSE;
// perform high level initialization of Dynamixel bus and servos
dxl_init(DEFAULT_BAUDNUMBER);
// assume initial pose
executeMotion(COMMAND_BALANCE_MP);
// set the walk state
walk_setWalkState(0);
obstacle_flag = 0;
// initialize the PID controller for balancing
#ifdef ACCEL_AND_ULTRASONIC
pid_init();
setupGyroKalman();
#endif
// initialize the ADC and take default readings
delay_ms(4000); // wait 4s for gyros to stabilize
adc_init();
sensor_process_flag = 0;
sensor_flag = 0;
// print out default sensor values
#ifdef GYRO_AND_DMS_ONLY
printf("\nBattery = %imV, Gyro X, Y Center = %i %i ", adc_battery_val, adc_gyrox_center, adc_gyroy_center);
#endif
#ifdef ACCEL_AND_ULTRASONIC
printf("\nBattery, Gyro X, Y Accel X, Y Center = %imV %i %i %i %i", adc_battery_val, adc_gyrox_center, adc_gyroy_center, adc_accelx_center, adc_accely_center);
#endif
// write out the command prompt
printf( "\nReady for command.\n> ");
// TIMING: timer4 = micros();
// main command loop (takes 28us when idle)
// keeps executing unless we encounter a major alarm
while( !major_alarm )
{
// Check if we received a new command
command_flag = serialReceiveCommand(); // takes 4ms if new command (largely because of printf)
// TIMING: timer1 = micros() - timer4;
// see if we are in a wait command
if ( bioloid_command == COMMAND_WAIT_MILLISECONDS || bioloid_command == COMMAND_WAIT_SECONDS )
{
// first look if we should continue waiting
if ( wait_flag == 1 )
{
// check timer
if ( millis() - wait_timer > wait_time )
{
// wait time is finished - reset the wait flag, timer and time
wait_flag = 0;
wait_timer = 0;
wait_time = 0;
// read next command from command sequence
if ( flag_motion_sequence == 1 )
{
bioloid_command = command_sequence_buffer[command_sequence_counter][0];
next_motion_page = command_sequence_buffer[command_sequence_counter][1];
// update pointer if there are more commands left
if ( command_sequence_counter < command_sequence_length ) {
command_sequence_counter++;
} else {
// sequence is finished, reset all sequence related variables
flag_motion_sequence = 0;
command_sequence_counter = 0;
command_sequence_length = 0;
bioloid_command = COMMAND_STOP;
}
}
}
}
if ( command_flag == 1 && wait_flag == 0 ) {
// wait command has only just been received, calculate wait time
wait_timer = millis();
command_flag = 0;
wait_flag = 1;
wait_time = next_motion_page;
}
else if ( command_flag == 1 && wait_flag == 1 )
{
// wait command is still in progress but new command has been received
// check for STOP, otherwise ignore
if ( bioloid_command == COMMAND_STOP )
{
// reset the wait flag, timer and time
wait_flag = 0;
wait_timer = 0;
wait_time = 0;
}
}
}
// check if start button has been pressed and we need to do emergency stop
if ( start_button_pressed && bioloid_command != COMMAND_STOP )
{
// disable torque & reset current command
comm_status = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 0);
last_bioloid_command = bioloid_command;
bioloid_command = COMMAND_STOP;
command_flag = 1;
// and reset the start button variable
start_button_pressed = FALSE;
} else if ( start_button_pressed && bioloid_command == COMMAND_STOP ) {
// we are resuming from an emergency stop, restore last command
bioloid_command = last_bioloid_command;
last_bioloid_command = COMMAND_STOP;
command_flag = 1;
// and reset the start button variable
start_button_pressed = FALSE;
}
// Check if we need to read the sensors
sensor_flag = adc_readSensors(); // takes 0.6ms for gyro/accel and 0.9ms including DMS/ultrasonic (156us per channel)
if ( sensor_flag == 1 ) {
// new sensor data - process and update command flag if necessary
sensor_process_flag = adc_processSensorData();
if ( command_flag == 0 && sensor_process_flag == 1 ) {
// robot has slipped, front/back getup command has been issued
command_flag = 1;
} else if ( sensor_process_flag == 2 ) {
// if the sensor process flag = 2 it means low voltage emergency stop
major_alarm = TRUE;
}
}
// obstacle avoidance for walking
if ( walk_getWalkState() != 0 ) {
// currently very basic - turn left until path is clear
obstacle_flag = walk_avoidObstacle(obstacle_flag);
if ( command_flag == 0 && (obstacle_flag == 1 || obstacle_flag == -1) ) {
command_flag = 1;
}
}
// set the new command global variable
if( command_flag == 1 ) {
new_command = TRUE;
command_flag = 0;
// if we are coming out of BAL command, reset joint offsets
if( last_bioloid_command == COMMAND_BALANCE && bioloid_command != COMMAND_BALANCE ) {
for (uint8 i=0; i<NUM_AX12_SERVOS; i++) { joint_offset[i] = 0; }
}
}
// TEST printf("\n Command %i, New %i, MP %i, Next MP %i ", bioloid_command, new_command, current_motion_page, next_motion_page);
// TIMING: timer2 = micros() - timer4 - timer1;
#ifdef ACCEL_AND_ULTRASONIC
// static balancing
if ( bioloid_command == COMMAND_BALANCE && major_alarm != TRUE ) {
// static balancing uses Kalman Filter or PID controller depending on availability of accelerometer
// first make sure the PID is turned on
if ( pid_getMode() != AUTOMATIC ) { pid_setMode(AUTOMATIC); }
staticRobotBalance();
} else if ( pid_getMode() == 1 ) {
pid_setMode(MANUAL);
}
#endif
// execute motion steps
if ( major_alarm != TRUE ) {
motion_flag = executeMotionSequence(); // takes 2.1ms when executing a step during walking or 3.3ms if unpacking a new motion page
}
// TIMING: timer3 = micros() - timer4 - timer1 - timer2;
// TIMING: printf("Timer 1 = %lu, 2 = %lu, 3 = %lu, SFlag = %i\n", timer1, timer2, timer3, sensor_flag);
// TIMING: timer4 = micros();
} // end of main command loop
}
void main_init(){
dashboard_state=DASHBOARD_STATE_STARTING;
ports_init();
Timer0_init(TMR0_PRESCALER);
CANInit();
#if HAS_50HZ|HAS_200HZ|HAS_50HZ
Timer1_init(TMR1_PRESCALER,FALSE);
#endif
#if HAS_10HZ|HAS_5HZ|HAS_4HZ
Timer3_init(TMR3_PRESCALER,FALSE);
#endif
#if HAS_50HZ
TIMER_Timer1_OCR1A_on();
#endif
#if HAS_25HZ
TIMER_Timer1_OCR1B_on();
#endif
#if HAS_200HZ
TIMER_Timer1_OCR1C_on();
#endif
#if HAS_10HZ
TIMER_Timer3_OCR3A_on();
#endif
#if HAS_BUZZER
buzzer_init();
TIMER_Timer3_OCR3C_on();
#endif
#if HAS_LEDS
led_init();
#endif
#if HAS_BUTTONS
button_init();
#endif
#if HAS_DISPLAY
display_init();
#endif
#if HAS_RADIO
radio_init();
#endif
InitWDT();
EventAddEvent(EVENT_INIT);
}
/**
* @brief Task which handles all UI including keypad, LCD and all user power outputs.
* Responds to events caused by other tasks and ISRs
* @param None
* @retval Should never exit
*/
__task void ui (void)
{
uint16_t event_flag = 0;
uint8_t key;
int i;
uint64_t entry_code = 0;
lcd_init();
keypad_init();
lcd_backlight(1);
buzzer_init();
pwr_sw_init();
usb_outputs_init();
dc_outputs_init();
lcd_clear();
lcd_write_string(" e.quinox ");
lcd_goto_XY(0,1);
lcd_write_string(" izuba.box ");
//2 second timeout
os_dly_wait(200);
if ( get_unlock_days () >= 0 )
{
ui_state = STATE_NORM;
}
if( local_ee_data.lvdc_flag == 1 )
{
ui_state = STATE_LVDC;
}
reset_display();
reset_outputs();
while(1)
{
//Wait for any task event or timeout after 1 second
if ( os_evt_wait_or(0xFFFF, 100) == OS_R_EVT )
{
//Find which event
event_flag = os_evt_get();
if ( event_flag & UI_BOX_SETUP )
{
ui_state = STATE_SETUP;
lcd_clear();
reset_display();
reset_outputs();
}
if ( (event_flag & UI_LVDC) )
{
if ( (ui_state != STATE_LVDC) && (ui_state != STATE_OFF) )
{
ui_state = STATE_LVDC;
local_ee_data.lvdc_flag = 1;
update_lvdc(1);
//Turn off outputs
reset_outputs();
lcd_power(1);
lcd_clear();
lcd_write_string_XY(0, 0, " Battery Empty ");
lcd_write_string_XY(0, 1, " Turning Off ");
//Delay and Buzz
//20 Seconds
for ( i = 0; i < 5; i++)
{
buzz(1);
//4 second wait
os_dly_wait(400);
}
//Turn off Screen
lcd_power(0);
}
}
if ( event_flag & UI_PWR_SW )
{
if ( ui_state != STATE_OFF)
{
//Turn off all outputs and UI devices
//Wait only for UI_PWR_SW tasks
lcd_power(0);
ui_state = STATE_OFF;
reset_outputs();
}
else
{
//Re-init LCD
lcd_clear();
lcd_power(1);
check_display_debug();
lcd_splash_screen(2);
if(get_soc() >= CHARGED)
{
local_ee_data.lvdc_flag = 0;
update_lvdc(0);
}
if(local_ee_data.lvdc_flag == 1)
{
ui_state = STATE_LVDC;
}
if(ui_state != STATE_LVDC)
{
if (get_unlock_days () >= 0 )
ui_state = STATE_NORM;
else
ui_state = STATE_AWAIT_PAYMENT;
}
reset_outputs();
reset_display();
}
//1 second delay
os_dly_wait(100);
EXTI_ClearITPendingBit(EXTI_Line0);
}
if ( event_flag & UI_EVT_USB_OC )
{
os_dly_wait(100);
if(EXTI_GetITStatus(EXTI_Line5) != RESET || EXTI_GetITStatus(EXTI_Line6) != RESET)
{
if(EXTI_GetITStatus(EXTI_Line5) != RESET)
USB1_DISABLE();
if(EXTI_GetITStatus(EXTI_Line6) != RESET)
USB2_DISABLE();
lcd_clear();
lcd_write_string_XY(0, 0, " USB ");
lcd_write_string_XY(0, 1, " error! ");
//2s wait
os_dly_wait(200);
reset_display();
}
}
if ( event_flag & (UI_EVT_KEYPAD_1 | UI_EVT_KEYPAD_2 | UI_EVT_KEYPAD_3) )
{
if ( (ui_state == STATE_AWAIT_PAYMENT) || (ui_state == STATE_NORM) || (ui_state == STATE_SETUP) )
{
//Read which key is pressed
i = 0;
do
{
key = keypad_get_key();
i++;
os_dly_wait(1);
if ( i > 20)
break;
} while (key == KEY_NONE);
if (key != KEY_NONE)
{
lcd_backlight(1);
buzz(1);
}
if (ui_state == STATE_SETUP)
{
//If 5 digits and tick then set box_id
if (key == KEY_CROSS) {
//'X' Pressed
//LCDWriteString("x");
display_str[0] = '\0';
digit_count = 0;
local_ee_data.box_id = 0;
reset_display();
} else if (key == KEY_NONE) {
//Do nothing
} else if ( key == KEY_TICK ) {
if(digit_count == 5){
os_dly_wait(50);
//Send message to payment control task
os_evt_set(PC_SET_BOX_ID, payment_control_t);
ui_state = STATE_AWAIT_PAYMENT;
display_str[0] = '\0';
digit_count = 0;
reset_display();
reset_outputs();
}
}else{
if(digit_count <5){
//Add the keypad value to the box id
display_str[0] = '\0';
local_ee_data.box_id = (local_ee_data.box_id * 10) + key;
utoa_b(display_str, local_ee_data.box_id, 10, digit_count);
lcd_write_string_XY(7, 0, display_str);
lcd_goto_XY((8 + digit_count), 0);
reset_display();
}
else { // do nothing
}
digit_count++;
}
}
else if (ui_state == STATE_AWAIT_PAYMENT)
{
if (key == KEY_CROSS) {
//'X' Pressed
//LCDWriteString("x");
display_str[0] = '\0';
digit_count = 0;
entry_code = 0;
lcd_write_string_XY(6, 0, "__________");
lcd_goto_XY(6, 0);
//key = KEY_NONE;
} else if (key == KEY_TICK) {
//Tick Pressed
//LCDWriteString("./");
} else if (key == KEY_NONE) {
//Tick Pressed
//LCDWriteString("./");
} else {
//Add the keypad to the entry code
entry_code = (entry_code * 10) + key;
//Make the
display_str[0] = '\0';
utoa_b(display_str, entry_code, 10, digit_count);
lcd_write_string_XY(6, 0, display_str);
lcd_goto_XY((7 + digit_count), 0);
if (digit_count++ == 9) {
os_dly_wait(50);
//Send code to payment control task
// but send (uint32_t)entry_code
if ( check_unlock_code((uint32_t)entry_code))
{
TRACE_INFO("2,1,%s\n", display_str);
ui_state = STATE_NORM;
// Edited Code
lcd_clear();
lcd_write_string_XY(0, 0, " Valid ");
lcd_write_string_XY(0, 1, " code! ");
entry_code = 0;
digit_count = 0;
display_str[0] = '\0';
//2s wait
os_dly_wait(200);
// End of Edit
reset_display();
reset_outputs();
}
else
{
TRACE_INFO("2,0,%s\n", display_str);
// Edited Code
lcd_clear();
lcd_write_string_XY(0, 0, " Wrong ");
lcd_write_string_XY(0, 1, " code! ");
//2s wait
os_dly_wait(200);
// End of Edit
entry_code = 0;
digit_count = 0;
display_str[0] = '\0';
reset_display();
reset_outputs();
}
}
}
}
else
{
switch (key)
{
case KEY_NONE:
//No Action
break;
//Special Key Cases
case KEY_TICK:
TRACE_DEBUG("Key: ./ \n");
break;
case KEY_CROSS:
TRACE_DEBUG("Key: X \n");
break;
default:
//Print the key number
TRACE_DEBUG("Key: %i \n", key);
}
}
//Wait for release of key (with time-out)
i = 0;
while( keypad_get_key() != KEY_NONE )
{
i++;
os_dly_wait(1);
if ( i > 20)
break;
}
}
}
if(local_ee_data.lvdc_flag == 0)
{
if ( event_flag & UI_PAYMENT_INVALID )
{
ui_state = STATE_AWAIT_PAYMENT;
lcd_clear();
reset_display();
reset_outputs();
}
}
if(local_ee_data.lvdc_flag == 1)
{
lcd_clear();
lcd_write_string_XY(0, 0, " Battery Low ");
lcd_batt_level( get_soc(), get_charging_rate() );
if(get_soc() >= CHARGED)
{
local_ee_data.lvdc_flag = 0;
update_lvdc(0);
}
}
//clear event flags
os_evt_clr(event_flag, ui_t);
}
/* Debugging Info on LCD
sprintf(str, "P=%.2f", get_adc_voltage(ADC_SOL_V)*get_adc_voltage(ADC_SOL_I));
lcd_goto_XY(0,0);
lcd_write_string(str);
str[0] = NULL;
sprintf(str, "T=%.2f", get_adc_voltage(ADC_TEMP));
lcd_goto_XY(8,0);
lcd_write_string(str);
str[0] = NULL;
sprintf(str, "V=%.2f", get_adc_voltage(ADC_BATT_V));
lcd_goto_XY(0,1);
lcd_write_string(str);
str[0] = NULL;
sprintf(str, "I=%.2f", get_adc_voltage(ADC_BATT_I));
lcd_goto_XY(8,1);
lcd_write_string(str);
str[0] = NULL;
*/
//Update battery levels, days remaining and if normal state then time/date
if(local_ee_data.lvdc_flag == 1)
{
lcd_write_string_XY(0, 0, " Battery Low ");
lcd_batt_level( get_soc(), get_charging_rate() );
if(get_soc() >= CHARGED)
{
local_ee_data.lvdc_flag = 0;
update_lvdc(0);
if(get_unlock_days () >= 0 )
{
ui_state = STATE_NORM;
}
else
{
ui_state = STATE_AWAIT_PAYMENT;
}
reset_display();
reset_outputs();
}
}
if ( (ui_state == STATE_NORM) || (ui_state == STATE_AWAIT_PAYMENT) )
{
lcd_batt_level( get_soc(), get_charging_rate() );
}
if (ui_state == STATE_NORM)
{
//Update remaining days
if(local_ee_data.full_unlock == EE_FULL_UNLOCK_CODE){
lcd_write_string_XY(0, 1, " Unlocked");
lcd_batt_level( get_soc(), get_charging_rate() );
}
else{
lcd_write_string_XY(0, 1, " days");
lcd_write_int_XY(10, 1, get_unlock_days() );
lcd_batt_level( get_soc(), get_charging_rate() );
}
}
}
}
void init_application(void)
{
volatile unsigned char *ptr;
// ---------------------------------------------------------------------
// Enable watchdog
// Watchdog triggers after 16 seconds when not cleared
#ifdef USE_WATCHDOG
WDTCTL = WDTPW + WDTIS__512K + WDTSSEL__ACLK;
#else
WDTCTL = WDTPW + WDTHOLD;
#endif
// ---------------------------------------------------------------------
// Configure port mapping
// Disable all interrupts
__disable_interrupt();
// Get write-access to port mapping registers:
PMAPPWD = 0x02D52;
// Allow reconfiguration during runtime:
PMAPCTL = PMAPRECFG;
// P2.7 = TA0CCR1A or TA1CCR0A output (buzzer output)
ptr = &P2MAP0;
*(ptr + 7) = PM_TA1CCR0A;
P2OUT &= ~BIT7;
P2DIR |= BIT7;
// P1.5 = SPI MISO input
ptr = &P1MAP0;
*(ptr + 5) = PM_UCA0SOMI;
// P1.6 = SPI MOSI output
*(ptr + 6) = PM_UCA0SIMO;
// P1.7 = SPI CLK output
*(ptr + 7) = PM_UCA0CLK;
// Disable write-access to port mapping registers:
PMAPPWD = 0;
// Re-enable all interrupts
__enable_interrupt();
// Init the hardwre real time clock (RTC_A)
rtca_init();
// ---------------------------------------------------------------------
// Configure ports
// ---------------------------------------------------------------------
// Reset radio core
radio_reset();
radio_powerdown();
#ifdef CONFIG_ACCELEROMETER
// ---------------------------------------------------------------------
// Init acceleration sensor
as_init();
#else
as_disconnect();
#endif
// ---------------------------------------------------------------------
// Init buttons
init_buttons();
// ---------------------------------------------------------------------
// Configure Timer0 for use by the clock and delay functions
timer0_init();
/* Init buzzer */
buzzer_init();
// ---------------------------------------------------------------------
// Init pressure sensor
ps_init();
/* drivers/battery */
battery_init();
/* drivers/temperature */
temperature_init();
#ifdef CONFIG_INFOMEM
if (infomem_ready() == -2) {
infomem_init(INFOMEM_C, INFOMEM_C + 2 * INFOMEM_SEGMENT_SIZE);
}
#endif
}
int main(int argc, char **argv)
{
int32_t r;
process_init(); // run before any function that starts a process
pic32_init();
watchdog_init();
leds_init();
leds_progress_init();
buzzer_init();
clock_init();
rtimer_init();
ctimer_init();
leds_on(LEDS_ALL);
/* Serial line init part 2/3: set up the UART port. */
uart_console_init(UART_BAUDRATE);
// usb_serial_init();
// usb_serial_set_input(serial_line_input_byte);
/* Serial line init part 3/3: start the OS process. */
serial_line_init();
asm volatile("ei"); // enable interrupts
PRINTF("CPU Clock: %uMhz\n",
pic32_clock_get_system_clock() / 1000000);
PRINTF("Peripheral Clock: %uMhz\n",
pic32_clock_get_peripheral_clock() / 1000000);
random_init(4321);
process_start(&etimer_process, NULL);
process_start(&sensors_process, NULL);
SENSORS_ACTIVATE(button_sensor);
/* Starting autostarting process */
print_processes(autostart_processes);
autostart_start(autostart_processes);
leds_off(LEDS_ALL);
watchdog_start();
PRINTF("Starting the main scheduler loop\n");
/*
* This is the scheduler loop.
*/
while (1) {
do {
/* Reset watchdog. */
watchdog_periodic();
r = process_run();
} while (r > 0);
#if LPM_MODE > LPM_MODE_NONE
watchdog_stop();
/* low-power mode start */
asm volatile("wait");
/* low-power mode end */
watchdog_start();
#endif // LPM_MODE
}
return 0;
}
//* ************************************************************************************************
/// @fn init_application(void)
/// @brief Init the watch's program
/// @return none
//* ************************************************************************************************
void init_application(void)
{
volatile unsigned char *ptr;
// ---------------------------------------------------------------------
// Enable watchdog
// Watchdog triggers after 16 seconds when not cleared
#ifdef USE_WATCHDOG
WDTCTL = WDTPW + WDTIS__512K + WDTSSEL__ACLK;
#else
WDTCTL = WDTPW + WDTHOLD;
#endif
// ---------------------------------------------------------------------
// Configure PMM
SetVCore(3);
// Set global high power request enable
PMMCTL0_H = 0xA5;
PMMCTL0_L |= PMMHPMRE;
PMMCTL0_H = 0x00;
// ---------------------------------------------------------------------
// Enable 32kHz ACLK
P5SEL |= 0x03; // Select XIN, XOUT on P5.0 and P5.1
UCSCTL6 &= ~XT1OFF; // XT1 On, Highest drive strength
UCSCTL6 |= XCAP_3; // Internal load cap
UCSCTL3 = SELA__XT1CLK; // Select XT1 as FLL reference
UCSCTL4 = SELA__XT1CLK | SELS__DCOCLKDIV | SELM__DCOCLKDIV;
// ---------------------------------------------------------------------
// Configure CPU clock for 12MHz
_BIS_SR(SCG0); // Disable the FLL control loop
UCSCTL0 = 0x0000; // Set lowest possible DCOx, MODx
UCSCTL1 = DCORSEL_5; // Select suitable range
UCSCTL2 = FLLD_1 + 0x16E; // Set DCO Multiplier
_BIC_SR(SCG0); // Enable the FLL control loop
// Worst-case settling time for the DCO when the DCO range bits have been
// changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
// UG for optimization.
// 32 x 32 x 8 MHz / 32,768 Hz = 250000 = MCLK cycles for DCO to settle
#if __GNUC_MINOR__ > 5 || __GNUC_PATCHLEVEL__ > 8
__delay_cycles(250000);
#else
__delay_cycles(62500);
__delay_cycles(62500);
__delay_cycles(62500);
__delay_cycles(62500);
#endif
// Loop until XT1 & DCO stabilizes, use do-while to insure that
// body is executed at least once
do
{
UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + XT1HFOFFG + DCOFFG);
SFRIFG1 &= ~OFIFG; // Clear fault flags
}
while ((SFRIFG1 & OFIFG));
// ---------------------------------------------------------------------
// Configure port mapping
// Disable all interrupts
__disable_interrupt();
// Get write-access to port mapping registers:
PMAPPWD = 0x02D52;
// Allow reconfiguration during runtime:
PMAPCTL = PMAPRECFG;
// P2.7 = TA0CCR1A or TA1CCR0A output (buzzer output)
ptr = &P2MAP0;
*(ptr + 7) = PM_TA1CCR0A;
P2OUT &= ~BIT7;
P2DIR |= BIT7;
// P1.5 = SPI MISO input
ptr = &P1MAP0;
*(ptr + 5) = PM_UCA0SOMI;
// P1.6 = SPI MOSI output
*(ptr + 6) = PM_UCA0SIMO;
// P1.7 = SPI CLK output
*(ptr + 7) = PM_UCA0CLK;
// Disable write-access to port mapping registers:
PMAPPWD = 0;
// Re-enable all interrupts
__enable_interrupt();
// Init the hardwre real time clock (RTC_A)
rtca_init();
// ---------------------------------------------------------------------
// Configure ports
// ---------------------------------------------------------------------
// Reset radio core
radio_reset();
radio_powerdown();
// ---------------------------------------------------------------------
// Init acceleration sensor
#ifdef CONFIG_MOD_ACCELEROMETER
as_init();
#else
as_disconnect();
#endif
// ---------------------------------------------------------------------
// Init LCD
lcd_init();
// ---------------------------------------------------------------------
// Init buttons
init_buttons();
// ---------------------------------------------------------------------
// Configure Timer0 for use by the clock and delay functions
timer0_init();
// Init buzzer
buzzer_init();
// ---------------------------------------------------------------------
// Init pressure sensor
#ifdef CONFIG_PRESSURE_SENSOR
ps_init();
#endif
// ---------------------------------------------------------------------
// Init other sensors
// From: "driver/battery"
battery_init();
// From: "drivers/temperature"
temperature_init();
/// @todo What is this ?
#ifdef CONFIG_INFOMEM
if (infomem_ready() == -2)
infomem_init(INFOMEM_C, INFOMEM_C + 2 * INFOMEM_SEGMENT_SIZE);
#endif
}
int app_main (void)
{
long timer1 = 0;
eParseResult parse_result;
buzzer_init();
buzzer_play(1500, 100); /* low beep */
buzzer_wait();
buzzer_play(2500, 200); /* high beep */
init();
read_config();
// grbl init
plan_init();
st_init();
// main loop
for (;;)
{
// process characters from the serial port
while (!serial_line_buf.seen_lf && (serial_rxchars() != 0) )
{
unsigned char c = serial_popchar();
if (serial_line_buf.len < MAX_LINE)
serial_line_buf.data [serial_line_buf.len++] = c;
if ((c==10) || (c==13))
{
if (serial_line_buf.len > 1)
serial_line_buf.seen_lf = 1;
else
serial_line_buf.len = 0;
}
}
// process SD file if no serial command pending
if (!sd_line_buf.seen_lf && sd_printing)
{
if (sd_read_file (&sd_line_buf))
{
sd_line_buf.seen_lf = 1;
}
else
{
sd_printing = false;
serial_writestr ("Done printing file\r\n");
}
}
// if queue is full, we wait
if (!plan_queue_full())
{
/* At end of each line, put the "GCode" on movebuffer.
* If there are movement to do, Timer will start and execute code which
* will take data from movebuffer and generate the required step pulses
* for stepper motors.
*/
// give priority to user commands
if (serial_line_buf.seen_lf)
{
parse_result = gcode_parse_line (&serial_line_buf);
serial_line_buf.len = 0;
serial_line_buf.seen_lf = 0;
}
else if (sd_line_buf.seen_lf)
{
parse_result = gcode_parse_line (&sd_line_buf);
sd_line_buf.len = 0;
sd_line_buf.seen_lf = 0;
}
}
/* Do every 100ms */
#define DELAY1 100
if (timer1 < millis())
{
timer1 = millis() + DELAY1;
/* If there are no activity during 30 seconds, power off the machine */
if (steptimeout > (30 * 1000/DELAY1))
{
power_off();
}
else
{
steptimeout++;
}
}
#ifdef USE_BOOT_BUTTON
// OPTION: enter bootloader on "Boot" button
check_boot_request();
#endif
}
}
void keypad_init(void)
{
buzzer_init();
GPNCON &=((0xffffffff)<<(2*6));
GPNPUD = 0x00000555;
}