int main(void){
servos_init();
uint8_t servo_pin=2;
add_servo(servo_pin);
for(int i=1200;i<1800;i++){
set_servo(servo_pin,i);
_delay_ms(10);
}
set_servo(servo_pin,0);
return 0;
}
/**
* Setting up the board
*/
static void internal_setup()
{
// Intialise les servomoteurs
servos_init();
// Initialise le mode WiFly
WiFly.begin(921600);
terminal_init(&WiFly);
// Définit l'interruption @50hz
servos_attach_interrupt(setFlag);
// Configure la led de la board
pinMode(BOARD_LED_PIN, OUTPUT);
digitalWrite(BOARD_LED_PIN, LOW);
// Lance la configuration de l'utilisateur
setup();
}
void pinguino_main(void)
{
PIE1=0;
PIE2=0;
ADCON1=0x0F;
#ifdef __USB__
PIE2bits.USBIE = 1;
INTCON = 0xC0;
#endif
setup();
#ifdef ANALOG
analog_init();
#endif
#ifdef MILLIS
millis_init();
#endif
#ifdef SERVOSLIBRARY
servos_init();
#endif
#ifdef __USBCDC
init_CDC();
PIE2bits.USBIE = 1;
INTCON = 0xC0;
#endif
#ifdef __SERIAL__
INTCONbits.PEIE=1;
INTCONbits.GIE=1;
#endif
#ifdef MILLIS
INTCONbits.TMR0IE=1;
INTCONbits.GIE=1;
#endif
#ifdef SERVOSLIBRARY
INTCONbits.PEIE=1;
INTCONbits.GIE=1;
#endif
while (1)
{
loop();
}
}
/**
* Setting up the board
*/
static void internal_setup()
{
// Initializing servos
servos_init();
// Initializing RC
RC.begin(921600);
terminal_init(&RC);
// Configuring board LED
pinMode(BOARD_LED_PIN, OUTPUT);
digitalWrite(BOARD_LED_PIN, LOW);
// Runing user setup
setup();
#if defined(DXL_AVAILABLE)
// Enabling asychronous dynamixel
dxl_async(true);
#endif
// Enabling 50hz interrupt
servos_attach_interrupt(setFlag);
}
void quad_init(void)
{
servos_init();
}
// Application entry point called from bootloader v4.x
void main(void)
#endif
{
#if defined(__18f25k50) || defined(__18f45k50) || \
defined(__18f26j50) || defined(__18f46j50) || \
defined(__18f26j53) || defined(__18f46j53) || \
defined(__18f27j53) || defined(__18f47j53)
u16 pll_startup_counter = 600;
#endif
/// ----------------------------------------------------------------
/// If we start from a Power-on reset, set NOT_POR bit to 1
/// ----------------------------------------------------------------
if (RCONbits.NOT_POR == 0)
{
RCON |= 0b10010011; // set all reset flag
// enable priority levels on interrupts
}
/// ----------------------------------------------------------------
/// Disables all interrupt
/// ----------------------------------------------------------------
//INTCONbits.GIEH = 0; // Disables all HP interrupts
//INTCONbits.GIEL = 0; // Disables all LP interrupts
/// ----------------------------------------------------------------
/// Perform a loop for some processors until their frequency is stable
/// ----------------------------------------------------------------
#if defined(__18f2455) || defined(__18f4455) || \
defined(__18f2550) || defined(__18f4550)
// If Internal Oscillator is used
if (OSCCONbits.SCS > 0x01)
// wait INTOSC frequency is stable (IOFS=1)
while (!OSCCONbits.IOFS);
// PLL is enabled by Config. Bits
#elif defined(__18f25k50) || defined(__18f45k50)
// If Internal Oscillator is used
if (OSCCONbits.SCS > 0x01)
// wait HFINTOSC frequency is stable (HFIOFS=1)
while (!OSCCONbits.HFIOFS);
// Enable the PLL and wait 2+ms until the PLL locks
OSCCON2bits.PLLEN = 1;
OSCTUNEbits.SPLLMULT = 1; // 1=3xPLL, 0=4xPLL
while (pll_startup_counter--);
#elif defined(__18f26j50) || defined(__18f46j50)
// If Internal Oscillator is used
// if (OSCCONbits.SCS > 0x02)
// Seems there is no time to wait
// Enable the PLL and wait 2+ms until the PLL locks
OSCTUNEbits.PLLEN = 1;
while (pll_startup_counter--);
#elif defined(__18f26j53) || defined(__18f46j53) || \
defined(__18f27j53) || defined(__18f47j53)
// If Internal Oscillator is used
if (OSCCONbits.SCS > 0x02)
// wait INTOSC frequency is stable (FLTS=1)
while(!OSCCONbits.FLTS);
// Enable the PLL and wait 2+ms until the PLL locks
OSCTUNEbits.PLLEN = 1;
while (pll_startup_counter--);
#endif
/// ----------------------------------------------------------------
/// I/O init
/// ----------------------------------------------------------------
IO_init();
IO_digital();
#if defined(__18f26j50) || defined(__18f46j50) || \
defined(__18f26j53) || defined(__18f46j53) || \
defined(__18f27j53) || defined(__18f47j53)
IO_remap();
#endif
/// ----------------------------------------------------------------
/// Various Init.
/// ----------------------------------------------------------------
#ifdef __USB__
usb_init();
#endif
#ifdef __USBCDC
CDC_init();
#endif
#ifdef __USBBULK
bulk_init();
#endif
#if defined(ANALOGREFERENCE) || defined(ANALOGREAD)
analog_init();
#endif
#ifdef ANALOGWRITE
analogwrite_init();
#endif
#ifdef __MILLIS__ // Use Timer 0
millis_init();
#endif
#ifdef SERVOSLIBRARY // Use Timer 1
servos_init();
#endif
#ifdef __PS2KEYB__
keyboard_init()
#endif
////////////////////////////////////////////////////////////////////////
setup();
////////////////////////////////////////////////////////////////////////
#if defined(TMR0INT) || defined(TMR1INT) || \
defined(TMR2INT) || defined(TMR3INT) || \
defined(TMR4INT) || defined(TMR5INT) || \
defined(TMR6INT) || defined(TMR8INT)
IntTimerStart(); // Enable all defined timers interrupts
// at the same time
#endif
#ifdef ON_EVENT
//IntInit();
INTCONbits.GIEH = 1; // Enable global HP interrupts
INTCONbits.GIEL = 1; // Enable global LP interrupts
#endif
while (1)
{
////////////////////////////////////////////////////////////////////////
loop();
////////////////////////////////////////////////////////////////////////
}
}
void pinguino_main(void)
{
#if defined(PIC18F26J50)
// Enable the PLL and wait 2+ms until the PLL locks
u16 pll_startup_counter = 600;
OSCTUNEbits.PLLEN = 1;
while(pll_startup_counter--);
#endif
PIE1 = 0;
PIE2 = 0;
IOsetSpecial();
IOsetDigital();
IOsetRemap();
#ifdef ON_EVENT // Enable General/Peripheral interrupts
int_init(); // Disable all individual interrupts
#endif
#ifdef __USB__
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
//setup();
//#ifdef ON_EVENT
//int_start(); // Enable all defined timers interrupts
//#endif
#ifdef ANALOG
analog_init();
#endif
#ifdef __MILLIS__ // Use Timer 0
millis_init();
#endif
#ifdef SERVOSLIBRARY
servos_init();
#endif
#ifdef __USBCDC
CDC_init();
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
#ifdef __USBBULK
bulk_init();
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
#ifdef __PS2KEYB__
keyboard_init()
#endif
#if defined(__SERIAL__) || defined(SERVOSLIBRARY)
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
/* RB : millis.c/millis_init() did already the job
#ifdef MILLIS
INTCONbits.TMR0IE= 1;
INTCONbits.GIE = 1;
#endif
*/
setup();
#ifdef ON_EVENT
int_start(); // Enable all defined timers interrupts
#endif
while (1)
loop();
}
int main(void) {
uart_init();
servos_init();
steppers_init();
pumps_init();
*idle_ddr &= ~idle_mask;
*idle_port |= idle_mask;
*setup_ddr &= ~setup_mask;
*setup_port |= setup_mask;
*ready_ddr |= ready_mask;
*ready_port &= ~(ready_mask);
sei();
uart_puts("PumpControl 0.1 ready\n\r");
uint8_t active_pump = 0;
while(1) {
pumps_run();
// Setup is active low
if(!(*setup_pin & setup_mask)) {
for(uint8_t i = 0; i < PumpCount; i++) {
if(pump_states[i] != PUMP_SETUP) {
pump_enter_setup(i);
}
}
}
// We only activate extruders if we are idle
if(*idle_pin & idle_mask) {
/*
* Check if we need a new active pump
* if full -> nice we've got a fresh one
* if dispense -> still some stuff left in this one
*/
if(pump_states[active_pump] != PUMP_FULL &&
pump_states[active_pump] != PUMP_DISPENSE) {
for(uint8_t i = 0; i < PumpCount; i++) {
if(pump_states[i] == PUMP_FULL) {
active_pump = i;
break;
}
}
}
// We've selected a new active pump, now we need to start it
if(pump_states[active_pump] == PUMP_FULL) {
uart_debug_pump(active_pump, "is new acitve pump");
pump_enter_dispense(active_pump);
}
}
/*
* We are ready if we picked a new active pump and set it to dispense,
* or if the old one still has stuff left in it and is in dispense.
*/
if(pump_states[active_pump] == PUMP_DISPENSE) {
*ready_port |= ready_mask;
}
else {
*ready_port &= ~(ready_mask);
}
/*
* If we are idle, try to fill all pumps
* If the active pump is also refilled, a other full one will be chosen
* in the next iteration.
*/
if(!(*idle_pin & idle_mask)) {
for(uint8_t i = 0; i < PumpCount; i++) {
if(pump_states[i] == PUMP_DISPENSE) {
uart_debug("Idle state detected, refilling");
pump_enter_fill(i);
}
}
}
}
}
/**
* @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);
}
}
