示例#1
0
static inline void main_periodic_task( void ) {
	servos[0]+=10;
	servos[1]+=10;
	servos[2]+=10;
	servos[3]+=10;

	if ((can_error_warning = CAN_GetFlagStatus(CAN1, CAN_FLAG_EWG)) == SET) {
		LED_ON(2);
	} else {
		LED_OFF(2);
	}
	if ((can_error_passive = CAN_GetFlagStatus(CAN1, CAN_FLAG_EPV)) == SET) {
		LED_ON(3);
	} else {
		LED_OFF(3);
	}
	if ((can_bus_off = CAN_GetFlagStatus(CAN1, CAN_FLAG_BOF)) == SET) {
		LED_ON(0);
	} else {
		LED_OFF(0);
	}

	cscp_transmit(0, 0, (uint8_t *)servos, 8);

	LED_PERIODIC();
	DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
}
示例#2
0
/**
  * @brief  Main program
  * @param  None
  * @retval None
  */
int main(void)
{
  /*!< At this stage the microcontroller clock setting is already configured, 
       this is done through SystemInit() function which is called from startup
       files (startup_stm32f40_41xxx.s/startup_stm32f427_437xx.s/startup_stm32f429_439xx.s)
       before to branch to application main. 
       To reconfigure the default setting of SystemInit() function, refer to
       system_stm32f4xx.c file
     */     
       
  /* NVIC configuration */
  NVIC_Config();

   /* Initialize LEDs mounted on EVAL board */
  STM_EVAL_LEDInit(LED1);
  STM_EVAL_LEDInit(LED2);
  STM_EVAL_LEDInit(LED3);
  STM_EVAL_LEDInit(LED4);
  
  /* Initialize Key Button mounted on EVAL board */
  STM_EVAL_PBInit(BUTTON_KEY, BUTTON_MODE_GPIO); 
   
  /* CAN configuration */
  CAN_Config();
  
  while(1)
  {
    while(STM_EVAL_PBGetState(BUTTON_KEY) == KEY_PRESSED)
    {
      if(ubKeyNumber == 0x4) 
      {
        ubKeyNumber = 0x00;
      }
      else
      {
        LED_Display(++ubKeyNumber);
        TxMessage.Data[0] = ubKeyNumber;
        CAN_Transmit(CANx, &TxMessage);
        /* Wait until one of the mailboxes is empty */
        while((CAN_GetFlagStatus(CANx, CAN_FLAG_RQCP0) !=RESET) || \
              (CAN_GetFlagStatus(CANx, CAN_FLAG_RQCP1) !=RESET) || \
              (CAN_GetFlagStatus(CANx, CAN_FLAG_RQCP2) !=RESET));
        
        while(STM_EVAL_PBGetState(BUTTON_KEY) != KEY_NOT_PRESSED)
        {
        }
      }
    }
  }
}
示例#3
0
void CAN1_RX0_IRQHandler(){

CanRxMsg RxMessage;
CanRxMsg* RxMsg;
RxMsg = &RxMessage;
volatile long DataLow;
volatile long DataHigh;

/*-----During initialization, just reading the FIFO to clear it-----*/
if (init==1)
{
	//toggle_led(green2);
	//Delay(10000);
	CAN_Receive(CAN1,CAN_FIFO0,&RxMsg3);
	//CAN_Receive(CAN1,CAN_FIFO1,&RxMsg3);
}

/*-------------------------After initialization--------------------*/

if ((CAN_GetLastErrorCode(CAN1)== CAN_ErrorCode_NoErr)&(init==0))		//If No error occurred
	{

	if (CAN_GetFlagStatus(CANx,CAN_FLAG_FMP0) == SET)		// check FIFO_0
	{
		//toggle_led(red);

		CAN_Receive(CANx,CAN_FIFO0,RxMsg);					//Read the message

		DataLow = RxMsg -> Data[3]<<24 |RxMsg -> Data[2]<<16 |RxMsg -> Data[1]<<8 | RxMsg -> Data[0];
		DataHigh = RxMsg -> Data[7]<<24 |RxMsg -> Data[6]<<16 |RxMsg -> Data[5]<<8 | RxMsg -> Data[4];

		/*---------------Check the Filter Match Index to see which filter is activated-----------*/
		/*----------------------*/
		/*	FIFO 0				*/
		/*	FMI = 0 -> EPOS 1	*/
		/*	FMI = 1 -> EPOS 2	*/
		/*----------------------*/
		if ((RxMessage.FMI == 0))// && (available_data.EPOS1 != 1))			//avoid taking a message already taken from the other FIFO mailbox
		{
			toggle_led(red);
			switch ( DataLow )
					{
						case RECEIVE_ACT_POS:
							Sensor_val.motor_pos_1 = DataHigh ;
							break;
						case RECEIVE_ACT_VEL:
							Sensor_val.motor_vel_1 = DataHigh;
							break;
						case RECEIVE_ACT_CUR:
							Sensor_val.current_EPOS1 = DataHigh;
							break;
						case ACK_POS_SEND:	 	; break;
						case ACK_VEL_SEND:		; break;
						case ACK_CUR_SEND:		; break;
						default: break;
					}
			available_data.EPOS1 = 1;
		}

		if (RxMessage.FMI == 1)// && (available_data.EPOS2 != 1))
		{
			toggle_led(yellow);

			switch ( DataLow )
			{
				case RECEIVE_ACT_POS :
				Sensor_val.motor_pos_2 = DataHigh ;
					break;
				case RECEIVE_ACT_VEL:
				Sensor_val.motor_vel_2 = DataHigh;
					break;
				case RECEIVE_ACT_CUR:
				Sensor_val.current_EPOS2 = DataHigh;
					break;
				case ACK_POS_SEND:	 	; break;
				case ACK_VEL_SEND:		; break;
				case ACK_CUR_SEND:		; break;
				default: break;
			}
			available_data.EPOS2 = 1;
		}
	}
}
//-----NOTE: At the old board nothing happens if there are errors-----------------//



//if ((available_data.EPOS1 == 1 ) && (available_data.EPOS2 == 1) && (available_data.EPOS3 == 1))
//		{
//		// Odometry and control
//		Gyro_Values_to_Theta_dot();
//		odometry();
//		//kalman();
//
//		setpoint_filter();
//		state_feedback_control();
//		//non_linear_control();
//		//planar_control();
//
//
//		// Send the commands to the EPOS
//		Safety_first();
//		//Motor_current_real.I_1 = 0;
//		//Motor_current_real.I_2 = 0;
//		//Motor_current_real.I_3 = 0;
//		EPOS_set_current_SDO(1,(short)Motor_current_real.I_1);
//		EPOS_set_current_SDO(2,(short)Motor_current_real.I_2);
//		EPOS_set_current_SDO(4,(short)Motor_current_real.I_3);
//
//
//		// Reset the EPOS data available flag
//		available_data.EPOS1=0;
//		available_data.EPOS2=0;
//		available_data.EPOS3=0;
//
//		//Disable CAN interrupt
//		//CANx -> IER &= ~(0x0000007e);
//		CAN_ITConfig(CANx,CAN_IT_FF0 | CAN_IT_FOV0 | CAN_IT_FMP0 | CAN_IT_FF1 | CAN_IT_FOV1 | CAN_IT_FMP1, DISABLE);
//		//Clear pending interrupts
//		CAN_ClearITPendingBit(CANx,CAN_IT_FF0 | CAN_IT_FOV0 | CAN_IT_FF1 | CAN_IT_FOV1);
//
//		//Enable IMU interrupts
//		 USART_ITConfig(USART3,USART_IT_RXNE,ENABLE);
//		 USART_ITConfig(USART3,USART_IT_TC,ENABLE);
//
//		}

}