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);
}
/**
* @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)
{
}
}
}
}
}
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);
//
// }
}