void init()
{
	ResetReason = MCUSR;		
 	cli();
    chip_init ();    				// Chip initialization
	init_leds ();
	delay(1000000);					// ~ 2 sec
	read_cal();						// Read everything including motor stops.	
	// yes can_init() needs MyInstance to be set already for filtering!
	can_init(CAN_250K_BAUD);		/* Enables Mob0 for Reception! */

    // INIT MYINSTANCE:	
	config_init();
	can_instance_init();
	
    set_rx_callback			( can_file_message );
	set_configure_callback	( config_change    );
	sei();

	OS_InitTask();
	pot_init();	
	motor_init ();

	can_prep_instance_request( &msg2, 0xBB );
	can_send_msg( 0, &msg2 );
}

//------------------------------------------------------------------------------
//! This program performs a response to an remote frame of a master.
//! The response is a data frame that contents (in the order) the local
//! temperature, the local luminosity and the local VCC values.
//------------------------------------------------------------------------------
int main (void)
{	
    CLKPR = 0x80;  CLKPR = 0x00;  // Clock prescaler Reset

    can_init(0);

	DDRA = 0xFF; // LED'er output
	PORTA = 0xFF; // LED'er tændt

    DDRD = 0xFF;
    PORTD |= 0b1000000;

    // Interrupt
    sei();

    // Modtag CAN interrupt

    CANIE2 = 0xFF;
    CANIE1 = 0xFF;

    CANGIE |=(1<<ENIT);
    CANGIE |=(1<<ENRX);
    CANGIE |=(1<<ENTX);

	send_data_CAN();
    while(1);
    return 0;
}

void shackbus_init(void)
{
    // Initialize MCP2515
    can_init(BITRATE_125_KBPS);
#ifdef UART_DEBUG
    uart_write("can_init(BITRATE_125_KBPS);");
#endif
	
    // Load filters and masks
    can_static_filter(can_filter);
#ifdef UART_DEBUG
    uart_write("can_static_filter(can_filter);");
#endif

	fifo_init (&can_outfifo,   can_outbuf, 10);
	framestorage_init();



	// Create a test messsage

	send_msg_blink_ret.id = ((3L<<26)+(4L<<22)+(6L<<14)+(5L<<6)+11L);  //Absender = 2   Empfänger = 1
	send_msg_blink_ret.flags.rtr = 0;

	send_msg_blink_ret.flags.extended = 1;

	send_msg_blink_ret.length  = 3;
	send_msg_blink_ret.data[0] = 0;
	send_msg_blink_ret.data[1] = 0;
	send_msg_blink_ret.data[2] = 0;

	shackbus_startup_message();

}

void epos_init(epos_node_p node, can_device_p can_dev, config_p config) {
  if (!can_dev) {
    can_dev = malloc(sizeof(can_device_t));
    can_init(can_dev, 0);
  }

  config_init_default(&node->config, &epos_default_config);
  if (config)
    config_set(&node->config, config);

  epos_device_init(&node->dev, can_dev,
    config_get_int(&node->config, EPOS_PARAMETER_ID),
    config_get_int(&node->config, EPOS_PARAMETER_RESET));
  epos_sensor_init(&node->sensor, &node->dev,
    config_get_int(&node->config, EPOS_PARAMETER_SENSOR_TYPE),
    config_get_int(&node->config, EPOS_PARAMETER_SENSOR_POLARITY),
    config_get_int(&node->config, EPOS_PARAMETER_SENSOR_PULSES),
    config_get_int(&node->config, EPOS_PARAMETER_SENSOR_SUPERVISION));
  epos_motor_init(&node->motor, &node->dev,
    config_get_int(&node->config, EPOS_PARAMETER_MOTOR_TYPE),
    config_get_float(&node->config, EPOS_PARAMETER_MOTOR_CURRENT));
  epos_gear_init(&node->gear, &node->sensor,
    config_get_float(&node->config, EPOS_PARAMETER_GEAR_TRANSMISSION));
  epos_input_init(&node->input, &node->dev);
  epos_control_init(&node->control, &node->dev,
    config_get_int(&node->config, EPOS_PARAMETER_CONTROL_TYPE));
}

static void init(void)
{
	ACSR = _BV(ACD); // Disable Analog Comparator (power save)

	//motion_init();

	TCCR0 = _BV(CS02) | _BV(CS00); // clk / 256 
	TIMSK = _BV(TOIE0);

	io_init();

	// init twi
	if (!TWIM_Init())
	{
		while (1);
	}

	ds1631_init(I2C_ADRESSE_DS1631); // Init twi temperature sensor

	spi_init(); // initialize spi port
	can_read_addr();
	can_init(); // initialize can communication

	sei(); // turn on interrupts
	wdt_enable(WDTO_250MS);

#ifndef NO_NETVAR
	switch_netvars_init();
	lamp_out_init();
#endif
}

void can_setup(void)
{
    /* Enable peripheral clocks. */
    rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_AFIOEN);
    rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
    rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_CANEN);

    AFIO_MAPR = AFIO_MAPR_CAN1_REMAP_PORTB;

    /* Configure CAN pin: RX (input pull-up). */
    gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
                  GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_PB_RX);
    gpio_set(GPIOB, GPIO_CAN1_PB_RX);

    /* Configure CAN pin: TX. */
    gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ,
                  GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_PB_TX);

    /* NVIC setup. */
    nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ);
    nvic_set_priority(NVIC_USB_LP_CAN_RX0_IRQ, 1);

    /* Reset CAN. */
    can_reset(CAN1);

    /* CAN cell init. */
    if (can_init(CAN1,
                 false,           /* TTCM: Time triggered comm mode? */
                 true,            /* ABOM: Automatic bus-off management? */
                 false,           /* AWUM: Automatic wakeup mode? */
                 false,           /* NART: No automatic retransmission? */
                 false,           /* RFLM: Receive FIFO locked mode? */
                 false,           /* TXFP: Transmit FIFO priority? */
                 CAN_BTR_SJW_1TQ,
                 CAN_BTR_TS1_3TQ,
                 CAN_BTR_TS2_4TQ,
                 12))             /* BRP+1: Baud rate prescaler */
    {
        gpio_set(GPIOA, GPIO8);		/* LED0 off */
        gpio_set(GPIOB, GPIO4);		/* LED1 off */
        gpio_set(GPIOC, GPIO15);       	/* LED2 off */
        gpio_clear(GPIOC, GPIO2);       /* LED3 on */
        gpio_set(GPIOC, GPIO5);	        /* LED4 off */

        /* Die because we failed to initialize. */
        while (1)
            __asm__("nop");
    }

    /* CAN filter 0 init. */
    can_filter_id_mask_32bit_init(CAN1,
                                  0,     /* Filter ID */
                                  0,     /* CAN ID */
                                  0,     /* CAN ID mask */
                                  0,     /* FIFO assignment (here: FIFO0) */
                                  true); /* Enable the filter. */

    /* Enable CAN RX interrupt. */
    can_enable_irq(CAN1, CAN_IER_FMPIE0);
}

//////////////////////////////////////////////////////////////////////////////
/// Inizialization. This function is called once before entering 
/// main loop. 
///
//////////////////////////////////////////////////////////////////////////////
void init() {
	VMC_INT_16 motorID;

	init_aisc167b();
    init_asccom();    
    init_cycletime_counter();
	i2c_init();
	init_bioport();
	if (can_init(CAN_BAUD, 0)) while(1){led_swap_green();}

    led_set_green(0);
    led_set_red(0);
	ledseq_init();

    ais_system_init();


	//init_Thermic();
	init_error();
	init_calculatestate();
	init_motorcontrol();
	init_controller();
	init_current_limiter();
	mpwr_init();

	set_default_configuration();

	for (motorID = 0; motorID < 3; motorID++) {
	//	init_Limiter(i, 100);
		init_encoder(motorID);
	}
   


}

int main(void)
{
	union uKeyRoster scan;
	
    chip_init();    /* Chip initialization			*/
	can_init();		/* Enables Mob0 for Reception!	*/
	an_init();		/* Analog SPI module init 		*/
	init_leds();

	can_init_test_msg();
	byte result = CANSTMOB;

	/* TEST B)  Button boards configured as receivers;  LEDs showing upper nibble.
		1 Analog board on network will transmit a message.			*/
	send_test_msgs();

    while (1)
    {  	  
		// READ ALL ACTIVE ANALOG SIGNALS:
		an_read_actives();
		
		// TRANSMIT ACTIVE SIGNALS
		can_send_analog_msgs();

		/* Send about every 1 second */
		delay( one_second );
    }
    return(0);
} 

void can_interface_init(void)
{
    rcc_periph_clock_enable(RCC_CAN);

    /*
    STM32F3 CAN on 36MHz configured APB1 peripheral clock
    36MHz / 2 -> 18MHz
    18MHz / (1tq + 10tq + 7tq) = 1MHz => 1Mbit
    */
    can_init(CAN,             // Interface
             false,           // Time triggered communication mode.
             true,            // Automatic bus-off management.
             false,           // Automatic wakeup mode.
             false,           // No automatic retransmission.
             false,           // Receive FIFO locked mode.
             true,            // Transmit FIFO priority.
             CAN_BTR_SJW_1TQ, // Resynchronization time quanta jump width
             CAN_BTR_TS1_10TQ,// Time segment 1 time quanta width
             CAN_BTR_TS2_7TQ, // Time segment 2 time quanta width
             2,               // Prescaler
             false,           // Loopback
             false);          // Silent

    // filter to match any standard id
    // mask bits: 0 = Don't care, 1 = mute match corresponding id bit
    can_filter_id_mask_32bit_init(
        CAN,
        0,      // filter nr
        0,      // id: only std id, no rtr
        6 | (7<<29), // mask: match only std id[10:8] = 0 (bootloader frames)
        0,      // assign to fifo0
        true    // enable
    );
}

// Just clearing the errors and setting the operation Mode
bool SailMotor::init(const char * device) {

	can_init(device);

	epos_fault_reset(AV_SAIL_NODE_ID);
	epos_set_output_current_limit(AV_SAIL_NODE_ID, AV_MAX_SAIL_CURRENT);
	epos_set_continous_current_limit(AV_SAIL_NODE_ID, AV_MAX_SAIL_CONT_CURRENT);

	epos_shutdown(AV_SAIL_NODE_ID);
	epos_enable_operation(AV_SAIL_NODE_ID);

	epos_set_mode_of_operation(AV_SAIL_NODE_ID, EPOS_OPERATION_MODE_PROFILE_POSITION);

	epos_shutdown(AV_SAIL_NODE_ID);
	epos_enable_operation(AV_SAIL_NODE_ID);

	epos_set_profile_acceleration(AV_SAIL_NODE_ID, AV_SAIL_ACCELERATION);
	epos_set_profile_deceleration(AV_SAIL_NODE_ID, AV_SAIL_DECELERATION);
	epos_set_profile_velocity(AV_SAIL_NODE_ID, AV_SAIL_SPEED);

	epos_set_maximum_following_error(AV_SAIL_NODE_ID, AV_SAIL_MAX_FOLLOWING_ERROR);

    epos_set_brake_mask(AV_SAIL_NODE_ID, AV_SAIL_BRAKE_MASK);
    epos_set_brake_polarity(AV_SAIL_NODE_ID, AV_SAIL_BRAKE_POLARITY);

	printf("\n\nThe Sail motor at %s (CAN-ID %d) has the following status:\n", device, AV_SAIL_NODE_ID);
	epos_get_error(AV_SAIL_NODE_ID);

    epos_set_brake_state(AV_SAIL_NODE_ID, AV_SAIL_BRAKE_OPEN);

	return true;
}

static void init() {
	//initialize spi port
	spi_init();

	//initialize can communication
	can_init();
	read_can_addr();
}

static void init(void) {
	usart1_init(115200, buf_in, ARR_LEN(buf_in), buf_out, ARR_LEN(buf_out));
	sysclock_init();
	can_init();

	sei();
	puts_P(PSTR("Init complete\n\n"));
}

/**
 * \brief Initializes and enables CAN0 & CAN1 tranceivers and clocks. 
 * CAN0/CAN1 mailboxes are reset and interrupts disabled.
 */
void can_initialize(void)
{
	uint32_t ul_sysclk;
	uint32_t x = 1;

	/* Initialize CAN0 Transceiver. */
	sn65hvd234_set_rs(&can0_transceiver, PIN_CAN0_TR_RS_IDX);
	sn65hvd234_set_en(&can0_transceiver, PIN_CAN0_TR_EN_IDX);
	/* Enable CAN0 Transceiver. */
	sn65hvd234_disable_low_power(&can0_transceiver);
	sn65hvd234_enable(&can0_transceiver);

	/* Initialize CAN1 Transceiver. */
	sn65hvd234_set_rs(&can1_transceiver, PIN_CAN1_TR_RS_IDX);
	sn65hvd234_set_en(&can1_transceiver, PIN_CAN1_TR_EN_IDX);
	/* Enable CAN1 Transceiver. */
	sn65hvd234_disable_low_power(&can1_transceiver);
	sn65hvd234_enable(&can1_transceiver);

	/* Enable CAN0 & CAN1 clock. */
	pmc_enable_periph_clk(ID_CAN0);
	pmc_enable_periph_clk(ID_CAN1);

	ul_sysclk = sysclk_get_cpu_hz();
	if (can_init(CAN0, ul_sysclk, CAN_BPS_250K) &&
	can_init(CAN1, ul_sysclk, CAN_BPS_250K)) {

	/* Disable all CAN0 & CAN1 interrupts. */
	can_disable_interrupt(CAN0, CAN_DISABLE_ALL_INTERRUPT_MASK);
	can_disable_interrupt(CAN1, CAN_DISABLE_ALL_INTERRUPT_MASK);
		
	NVIC_EnableIRQ(CAN0_IRQn);
	NVIC_EnableIRQ(CAN1_IRQn);
	
	can_reset_all_mailbox(CAN0);
	can_reset_all_mailbox(CAN1);
	
	/* Initialize the CAN0 & CAN1 mailboxes */
	x = can_init_mailboxes(x); // Prevent Random PC jumps to this point.
	//configASSERT(x);
	
	
	}
	return;
}

static void init(void) {
	sysclock_init();
	_delay_ms(100);
	button_init();
	can_init();

	sei();
	puts_P(PSTR("Init complete\n\n"));
}

int main(void)
{
	_delay_ms(500);
	uint8_t data_buf[8];
	ioinit();
	uartinit();
	can_init(0);
    pwm16Init2();
	adcInit(1);
	st_cmd_t rpm_msg;
	counter0Init();

	_delay_ms(500);
	sei();
    
	sendtekst("\n\rUnicorn Gearnode v1.0 \n\r");

	//rpm_msg.pt_data = rpm_response_buffer;
	//rpm_msg.status = 0;

	//can_update_rx_msg(&rpm_msg, rpm_msgid, 8);
    can_update_rx_msg(&rpm_msg, gear_msgid, 8);

	Can_sei();
	Can_set_tx_int();
	Can_set_rx_int();
    
	while(1)
	{

        _delay_ms(100);
		data_buf[0] = GearNeutral;
		data_buf[1] = GEARNEUTRALMEAS;
		data_buf[2] = 0;
		can_send_non_blocking(gear_msgid, data_buf, 3);
        
		data_buf[0] = GearEst;
		data_buf[1] = 0;
		data_buf[2] = GearEst_val;
		can_send_non_blocking(rpm_msgid, data_buf, 3);
        
        /*
        gearUp();
		gearUp();
		gearUp();
		gearUp();
        gearDown();
		gearDown();
		gearDown();
		gearDown();
		*/
		
        

    }
    return 0;
}

/*
 * Initializes CAN bus
 */
void Can::begin() {
  // Initialize CAN
  Serial.println("Initializing CAN Controller");
  if (can_init(0,0,0,0,0,0,0,0)){
    Serial.println("Error: CAN initialization :(");
    while(1); // hang up program
  }
  Serial.println("CAN Controller Initialized :)");
}

int main(void)
{
	canInitializeLibrary();
	can_init();
	i15765_init();

	UIN15765_DEBUG_TRACE("%s OK!\n\r",  __FUNCTION__ );
	return 0;
}

void main()
{
	char reponse='a';
	int flag=255;
	int id1=23;	
	int id2=12;

	int value1=15;
	int value2=16;
	int value3=17;
	
	//initialisation du PIC 
	setup_adc(ADC_OFF);  		//on gère toutes les entrées comme étant de type analogique et on mesure les tensions par rapport à une ref 5V


	enable_interrupts(INT_TIMER2);      //configuration des interruptions
	enable_interrupts(INT_AD);
	enable_interrupts(GLOBAL);

	setup_timer_2(T2_DIV_BY_4,250,5);   //setup up timer2 to interrupt every 0,1ms
	can_init();							//initialise le CAN
	can_set_baud();						//obsolète à priori à tester
	restart_wdt();

	#ifdef DEBUG
	   // Mise en évidence d'un problème lié au Watchdog
   	   switch ( restart_cause() )
       {
          case WDT_TIMEOUT:
          {
             printf("\r\nRestarted processor because of watchdog timeout!\r\n");
             break;
          }
          case NORMAL_POWER_UP:
          {
             printf("\r\nNormal power up! PIC initialized \r\n");
             break;
          }
       }
       restart_wdt();
    #endif


	//  BOUCLE DE TRAVAIL
	while(TRUE)
	{
		putc(flag);
		putc(id1);
		putc(value1);
		putc(flag);
		putc(id1);
		putc(value2);
		putc(flag);
		putc(id2);
		putc(value3);
	}
}

int main(void)
{
	uart_init ();
	stdout = &uart_stdout;
	printf ("Initializing...\n");

	can_init (0x10);
	uint8_t count = 0;
	sei();
		
	printf ("Initialized\n");

	while(1) {
		can_frame_t frame;
		frame.identifier = 0x00;
		frame.size = count%8;
		for(uint8_t i = 0; i < frame.size; i++) {
			frame.data[i] = ~(128-i)+count;
		}
		uint8_t s = can_send_frame(&frame);
		printf("Sent data %d %s\r",count, (s?"SUCCESS":"FAIL"));
		
		can_frame_t recieved;
		
		_delay_ms(100);
		uint8_t status = mcp2515_read_status();
		printf("  Status = 0x%x\r", status);
		s = can_recieve_frame(&recieved);
		printf("  Receieved data: L1: %d L2: %d %s\r", frame.size, recieved.size, (s?"SUCCESS":"FAIL"));
		
		_delay_ms(100);
		if(frame.size == recieved.size) {
			printf("  Identical length\r");
			uint8_t passed = 1;
			for(uint8_t i = 0; i < recieved.size; i++) {
				if(frame.data[i] != recieved.data[i]) {
					passed = 0;
					continue;
				}
			}
			if(passed) {
				printf("    AND identical data\r");
			}
			else {
				printf("    FAIL! NOT identical data\r");
				
			}
		} else {
			printf("  FAIL! NOT identical length\r");
		}
		status = mcp2515_read_status();
		printf("  Status = 0x%x\r", status);
		_delay_ms(500);
		
		count++;
	}
}

CAN::CAN(PinName rd, PinName td) : _can(), _irq() {
    // No lock needed in constructor

    for (int i = 0; i < sizeof _irq / sizeof _irq[0]; i++) {
        _irq[i].attach(donothing);
    }

    can_init(&_can, rd, td);
    can_irq_init(&_can, (&CAN::_irq_handler), (uint32_t)this);
}