int InitMEMS(void) { /* SysTick end of count event each 10ms */ // RCC_GetClocksFreq(&RCC_Clocks); // SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* Initialize the LCD */ // LCD_Init(); /* Initialize the LCD Layers*/ // LCD_LayerInit(); /* Enable the LTDC */ // LTDC_Cmd(ENABLE); /* Set LCD Background Layer */ // LCD_SetLayer(LCD_FOREGROUND_LAYER); /* Clear the Background Layer */ // LCD_Clear(LCD_COLOR_WHITE); /* Gyroscope configuration */ Demo_GyroConfig(); /* Gyroscope calibration */ Gyro_SimpleCalibration(Gyro); /* Infinite loop */ // while (1) // { // Demo_MEMS(); // } }
/** * @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_stm32f429_439xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f4xx.c file */ /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* Initialize the LCD */ LCD_Init(); /* Initialize the LCD Layers*/ LCD_LayerInit(); /* Enable the LTDC */ LTDC_Cmd(ENABLE); /* Set LCD Background Layer */ LCD_SetLayer(LCD_FOREGROUND_LAYER); /* Clear the Background Layer */ LCD_Clear(LCD_COLOR_WHITE); /* Gyroscope configuration */ Demo_GyroConfig(); /* Gyroscope calibration */ Gyro_SimpleCalibration(Gyro); /* Infinite loop */ while (1) { Demo_MEMS(); } }
void MainTask_Gyro(void) { SysTICK(); float Buffer[6] = {0}; uint8_t Xval, Yval = 0x00; Demo_GyroConfig(); SetemWinRunning(1); GUI_SelectLayer(1); // select foregroung layer GUI_SetBkColor(STBLUE); // select background color as a solid color GUI_Clear(); // fill with the background color GRAPH_DATA_Handle hData1, hData2, hData3; GRAPH_SCALE_Handle hScale; WM_HWIN hGraph; hGraph = GRAPH_CreateEx(0, 0, 240, 320, WM_HBKWIN, WM_CF_SHOW, 0, GUI_ID_GRAPH0); hData1 = GRAPH_DATA_YT_Create(GUI_GREEN, 240, 0, 0); hData2 = GRAPH_DATA_YT_Create(GUI_RED, 240, 0, 0); //hData3 = GRAPH_DATA_YT_Create(GUI_BLUE, 240, 0, 0); GRAPH_AttachData(hGraph, hData1); GRAPH_AttachData(hGraph, hData2); //GRAPH_AttachData(hGraph, hData3); hScale = GRAPH_SCALE_Create(19, GUI_TA_RIGHT, GRAPH_SCALE_CF_VERTICAL, 20); GRAPH_SCALE_SetFactor(hScale, 0.5); GRAPH_SCALE_SetNumDecs(hScale, 0.01); GRAPH_SCALE_SetFont(hScale, &GUI_Font16_1); GRAPH_SCALE_SetTextColor(hScale, GUI_BLACK); GRAPH_AttachScale(hGraph, hScale); GRAPH_SetBorder(hGraph, 20, 0, 0, 0); GRAPH_SetGridVis(hGraph, 1); GRAPH_SetColor(hGraph, GUI_WHITE, GRAPH_CI_BK); GRAPH_SetColor(hGraph, GUI_LIGHTGRAY, GRAPH_CI_BORDER); GRAPH_SetColor(hGraph, GUI_BLACK, GRAPH_CI_FRAME); GRAPH_SetColor(hGraph, GUI_DARKGRAY, GRAPH_CI_GRID); GRAPH_SetLineStyleH(hGraph, GUI_LS_DOT); GRAPH_SetLineStyleV(hGraph, GUI_LS_DOT); GRAPH_SetGridDistX(hGraph, 50); GRAPH_SetGridDistY(hGraph, 20); GUI_Exec(); STM_EVAL_LEDToggle(LED4); STM_EVAL_LEDToggle(LED3); // the GUI is now rendered // in never ending loop just check if an incon is touched while(!tamperPushed) { if(open){ /* Read Gyro Angular data */ Demo_GyroReadAngRate(Buffer); /* Update autoreload and capture compare registers value*/ Xval = ABS((int8_t)(Buffer[0])); Yval = ABS((int8_t)(Buffer[1])); GRAPH_DATA_YT_AddValue(hData1, Xval*2); GRAPH_DATA_YT_AddValue(hData2, Yval*2); GUI_Exec(); open=0; } } // WM_DeleteWindow(hGraph); SetemWinRunning(0); GUI_CURSOR_Hide(); }
/** * @brief Main program. * @param None * @retval None */ int main(void) { uint8_t i = 0; /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* Initialize LEDs and User Button available on STM32F3-Discovery board */ STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_LEDInit(LED5); STM_EVAL_LEDInit(LED6); STM_EVAL_LEDInit(LED7); STM_EVAL_LEDInit(LED8); STM_EVAL_LEDInit(LED9); STM_EVAL_LEDInit(LED10); STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_EXTI); /* Configure the USB */ Demo_USB(); /* Reset UserButton_Pressed variable */ UserButtonPressed = 0x00; /* Infinite loop */ while (1) { /* LEDs Off */ STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); /* Waiting User Button is pressed */ while (UserButtonPressed == 0x00) { /* Toggle LD3 */ STM_EVAL_LEDToggle(LED3); /* Insert 50 ms delay */ Delay(5); /* Toggle LD5 */ STM_EVAL_LEDToggle(LED5); /* Insert 50 ms delay */ Delay(5); /* Toggle LD7 */ STM_EVAL_LEDToggle(LED7); /* Insert 50 ms delay */ Delay(5); /* Toggle LD9 */ STM_EVAL_LEDToggle(LED9); /* Insert 50 ms delay */ Delay(5); /* Toggle LD10 */ STM_EVAL_LEDToggle(LED10); /* Insert 50 ms delay */ Delay(5); /* Toggle LD8 */ STM_EVAL_LEDToggle(LED8); /* Insert 50 ms delay */ Delay(5); /* Toggle LD6 */ STM_EVAL_LEDToggle(LED6); /* Insert 50 ms delay */ Delay(5); /* Toggle LD4 */ STM_EVAL_LEDToggle(LED4); /* Insert 50 ms delay */ Delay(5); } DataReady = 0x00; /* All LEDs Off */ STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); /* Demo Gyroscope */ Demo_GyroConfig(); /* Waiting User Button is pressed */ while (UserButtonPressed == 0x01) { /* Wait for data ready */ while(DataReady != 0x05) {} DataReady = 0x00; /* LEDs Off */ STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); /* Read Gyro Angular data */ Demo_GyroReadAngRate(Buffer); /* Update autoreload and capture compare registers value*/ Xval = ABS((int8_t)(Buffer[0])); Yval = ABS((int8_t)(Buffer[1])); if ( Xval>Yval) { if ((int8_t)Buffer[0] > 5.0f) { /* LD10 On */ STM_EVAL_LEDOn(LED10); } if ((int8_t)Buffer[0] < -5.0f) { /* LD3 On */ STM_EVAL_LEDOn(LED3); } } else { if ((int8_t)Buffer[1] < -5.0f) { /* LD6 on */ STM_EVAL_LEDOn(LED6); } if ((int8_t)Buffer[1] > 5.0f) { /* LD7 On */ STM_EVAL_LEDOn(LED7); } } } DataReady = 0x00; /* LEDs Off */ STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); /* Demo Compass */ Demo_CompassConfig(); /* Waiting User Button is pressed */ while (UserButtonPressed == 0x02) { /* Wait for data ready */ while(DataReady !=0x05) {} DataReady = 0x00; /* Read Compass data */ Demo_CompassReadMag(MagBuffer); Demo_CompassReadAcc(AccBuffer); for(i=0;i<3;i++) AccBuffer[i] /= 100.0f; fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2])); fSinRoll = -AccBuffer[1]/fNormAcc; fCosRoll = (sqrt(1.0-(fSinRoll * fSinRoll))); fSinPitch = AccBuffer[0]/fNormAcc; fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch)); if ( fSinRoll >0) { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } else { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI + 360; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } if ( fSinPitch >0) { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } else { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI + 360; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } if (RollAng >=360) { RollAng = RollAng - 360; } if (PitchAng >=360) { PitchAng = PitchAng - 360; } fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch; fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch; HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI; if (HeadingValue < 0) { HeadingValue = HeadingValue + 360; } if ((RollAng <= 40.0f) && (PitchAng <= 40.0f)) { if (((HeadingValue < 25.0f)&&(HeadingValue >= 0.0f))||((HeadingValue >=340.0f)&&(HeadingValue <= 360.0f))) { STM_EVAL_LEDOn(LED10); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); } else if ((HeadingValue <70.0f)&&(HeadingValue >= 25.0f)) { STM_EVAL_LEDOn(LED9); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED5); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED7); } else if ((HeadingValue < 115.0f)&&(HeadingValue >= 70.0f)) { STM_EVAL_LEDOn(LED7); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED5); } else if ((HeadingValue <160.0f)&&(HeadingValue >= 115.0f)) { STM_EVAL_LEDOn(LED5); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED3); } else if ((HeadingValue <205.0f)&&(HeadingValue >= 160.0f)) { STM_EVAL_LEDOn(LED3); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED7); } else if ((HeadingValue <250.0f)&&(HeadingValue >= 205.0f)) { STM_EVAL_LEDOn(LED4); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED5); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED7); } else if ((HeadingValue < 295.0f)&&(HeadingValue >= 250.0f)) { STM_EVAL_LEDOn(LED6); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED8); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED5); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED7); } else if ((HeadingValue < 340.0f)&&(HeadingValue >= 295.0f)) { STM_EVAL_LEDOn(LED8); STM_EVAL_LEDOff(LED6); STM_EVAL_LEDOff(LED10); STM_EVAL_LEDOff(LED7); STM_EVAL_LEDOff(LED9); STM_EVAL_LEDOff(LED3); STM_EVAL_LEDOff(LED4); STM_EVAL_LEDOff(LED5); } } else { /* Toggle All LEDs */ STM_EVAL_LEDToggle(LED7); STM_EVAL_LEDToggle(LED6); STM_EVAL_LEDToggle(LED10); STM_EVAL_LEDToggle(LED8); STM_EVAL_LEDToggle(LED9); STM_EVAL_LEDToggle(LED3); STM_EVAL_LEDToggle(LED4); STM_EVAL_LEDToggle(LED5); /* Delay 50ms */ Delay(5); } } } }
int main(void) { CanTxMsg TxMessage; float X_offset =0.0f,Y_offset =0.0f,Z_offset =0.0f; float test_float=0.0f; uint8_t test_int=0; float GyX =0.0f, GyY =0.0f, GyZ =0.0f; float GyX_prev=0.0f,GyY_prev=0.0f,GyZ_prev=0.0f; uint16_t x_len=240; uint16_t y_len=320; uint16_t i=0; uint16_t buffer_screen[x_len][y_len]; /* For gyro receiving */ float receivedGyro1=0,receivedGyro1_prev=0; float receivedGyro2=0,receivedGyro2_prev=0; float receivedGyro3=0,receivedGyro3_prev=0; uint8_t *ptr = & receivedGyro1; // uint16_t *buf_ptr = &buffer_screen; float runner=-8.0; rectangular_t rect1; rectangular_t prev_rect; rectangular_t rect_screen; char lcd_text_main[100]; /* LCD Initialization */ lcd_init(); lcd_drawBackground(20,60,250); //lcd_drawBGPersimmon(20, 60, 250); /* LED Initialization */ LED_Initialization(); /* CAN Initialization */ CAN2_Config(); CAN2_NVIC_Config(); /* MEMS Initialization */ Demo_GyroConfig(); Delay_1us(10000); #define CALIBRATE_COUNT 1000 for (i=0;i<CALIBRATE_COUNT ;i++){ Demo_GyroReadAngRate (Buffer); X_offset+= Buffer[0]; Y_offset+= Buffer[1]; Z_offset+= Buffer[2]; } X_offset = X_offset/ (float)CALIBRATE_COUNT; Y_offset = Y_offset/ (float)CALIBRATE_COUNT; Z_offset = Z_offset/ (float)CALIBRATE_COUNT; rect_screen.xlen = x_len; rect_screen.ylen = y_len; rect_screen.xpos = 0; rect_screen.ypos = 0; #define NEEDLE_RADIUS 65 #define NEEDLE_BASE_WIDTH 14 #define NEEDLE_FRAME_THICKNESS 5 #define NEEDLE1_CENTER_X 80 #define NEEDLE1_CENTER_Y 100 #define NEEDLE2_CENTER_X 80 #define NEEDLE2_CENTER_Y 200 #define NEEDLE3_CENTER_X 80 #define NEEDLE3_CENTER_Y 300 /* Drawing Needle frame 1 */ LCD_SetLayer(LCD_BACKGROUND_LAYER); DrawThickCircle(NEEDLE1_CENTER_X ,NEEDLE1_CENTER_Y,NEEDLE_RADIUS+NEEDLE_FRAME_THICKNESS, 4,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_FOREGROUND_LAYER); DrawThickCircle(NEEDLE1_CENTER_X ,NEEDLE1_CENTER_Y,NEEDLE_BASE_WIDTH, 6,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_BACKGROUND_LAYER); LCD_SetColors(ASSEMBLE_RGB(20, 60, 250),LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE1_CENTER_X- NEEDLE_RADIUS-NEEDLE_FRAME_THICKNESS*2,NEEDLE1_CENTER_Y+ NEEDLE_BASE_WIDTH+3,NEEDLE_RADIUS*2+NEEDLE_FRAME_THICKNESS*4,NEEDLE_RADIUS); LCD_SetLayer(LCD_FOREGROUND_LAYER); LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE1_CENTER_X- NEEDLE_RADIUS,NEEDLE1_CENTER_Y+ NEEDLE_BASE_WIDTH,NEEDLE_RADIUS*2,NEEDLE_FRAME_THICKNESS-1); /* Drawing Needle frame 2 */ LCD_SetLayer(LCD_BACKGROUND_LAYER); DrawThickCircle(NEEDLE2_CENTER_X ,NEEDLE2_CENTER_Y,NEEDLE_RADIUS+NEEDLE_FRAME_THICKNESS, 4,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_FOREGROUND_LAYER); DrawThickCircle(NEEDLE2_CENTER_X ,NEEDLE2_CENTER_Y,NEEDLE_BASE_WIDTH, 6,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_BACKGROUND_LAYER); LCD_SetColors(ASSEMBLE_RGB(20, 60, 250),LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE2_CENTER_X- NEEDLE_RADIUS-NEEDLE_FRAME_THICKNESS*2,NEEDLE2_CENTER_Y+ NEEDLE_BASE_WIDTH+3,NEEDLE_RADIUS*2+NEEDLE_FRAME_THICKNESS*4,NEEDLE_RADIUS); LCD_SetLayer(LCD_FOREGROUND_LAYER); LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE2_CENTER_X- NEEDLE_RADIUS,NEEDLE2_CENTER_Y+ NEEDLE_BASE_WIDTH,NEEDLE_RADIUS*2,NEEDLE_FRAME_THICKNESS-1); /* Drawing Needle frame 2 */ LCD_SetLayer(LCD_BACKGROUND_LAYER); DrawThickCircle(NEEDLE3_CENTER_X ,NEEDLE3_CENTER_Y,NEEDLE_RADIUS+NEEDLE_FRAME_THICKNESS, 4,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_FOREGROUND_LAYER); DrawThickCircle(NEEDLE3_CENTER_X ,NEEDLE3_CENTER_Y,NEEDLE_BASE_WIDTH, 6,LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); LCD_SetLayer(LCD_BACKGROUND_LAYER); LCD_SetColors(ASSEMBLE_RGB(20, 60, 250),LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE3_CENTER_X- NEEDLE_RADIUS-NEEDLE_FRAME_THICKNESS*2,NEEDLE3_CENTER_Y+ NEEDLE_BASE_WIDTH+3,NEEDLE_RADIUS*2+NEEDLE_FRAME_THICKNESS*4,NEEDLE_RADIUS); LCD_SetLayer(LCD_FOREGROUND_LAYER); LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_BLACK); LCD_DrawFullRect(NEEDLE3_CENTER_X- NEEDLE_RADIUS,NEEDLE3_CENTER_Y+ NEEDLE_BASE_WIDTH,NEEDLE_RADIUS*2,NEEDLE_FRAME_THICKNESS-1); /* Clear drawing buffer */ PadRectangular(&buffer_screen,x_len,y_len,LCD_COLOR_WHITE, &rect_screen); while(1) { board_ID = PIN_ID_Read(); LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); sprintf(lcd_text_main," CAN Demo ID:%d ",board_ID); LCD_DisplayStringLine(LINE(0), (uint8_t*)lcd_text_main); Demo_GyroReadAngRate (Buffer); //Delay_1us(1000); /* MEMS Filtering */ #define LP_ALPHA 0.1f GyX = GyX*(1.0f - LP_ALPHA) + (Buffer[0] - X_offset)*LP_ALPHA; GyY = GyY*(1.0f - LP_ALPHA) + (Buffer[1] - Y_offset)*LP_ALPHA; GyZ = GyZ*(1.0f - LP_ALPHA) + (Buffer[2] - Z_offset)*LP_ALPHA; if(GyX > 90.0f) GyX = 90.0f; if(GyX < -90.0f) GyX = -90.0f; if(GyY > 90.0f) GyY = 90.0f; if(GyY < -90.0f) GyY = -90.0f; if(GyZ > 90.0f) GyZ = 90.0f; if(GyZ < -90.0f) GyZ = -90.0f; /* Start drawing rectangular */ prev_rect = rect1; rect1.xlen = 25; rect1.ylen = 30; rect1.xpos = x_len/2+ (int16_t)(GyY)-10; rect1.ypos = y_len/2 + (int16_t)(GyX)-10; if(board_ID == 1){ MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_GREEN,NEEDLE1_CENTER_X,NEEDLE1_CENTER_Y,-GyZ,-GyZ_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); }else if(board_ID == 2){ MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_GREEN,NEEDLE2_CENTER_X,NEEDLE2_CENTER_Y,-GyZ,-GyZ_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); }else { MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_GREEN,NEEDLE3_CENTER_X,NEEDLE3_CENTER_Y,-GyZ,-GyZ_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); } CAN2_TransmitGyro(board_ID,GyZ); /* Received Data */ if( can2_rx_isr_flag ==1){ do{ if( can2_rx_isr_flag ==1){ can2RxMessage = CAN2_PassRXMessage(); can2_rx_isr_flag=0; }else{ CAN_Receive(CAN2, CAN_FIFO0, &can2RxMessage); } GPIO_ToggleBits(GPIOG,GPIO_Pin_14); ptr[0] = can2RxMessage.Data[0]; ptr[1] = can2RxMessage.Data[1]; ptr[2] = can2RxMessage.Data[2]; ptr[3] = can2RxMessage.Data[3]; if(( can2RxMessage.ExtId & 0x0000FFFF) == 1){ ptr = & receivedGyro1; ptr[0] = can2RxMessage.Data[0]; ptr[1] = can2RxMessage.Data[1]; ptr[2] = can2RxMessage.Data[2]; ptr[3] = can2RxMessage.Data[3]; MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_RED,NEEDLE1_CENTER_X,NEEDLE1_CENTER_Y,-receivedGyro1,-receivedGyro1_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); receivedGyro1_prev = receivedGyro1; }else if (( can2RxMessage.ExtId & 0x0000FFFF) == 2){ ptr = & receivedGyro2; ptr[0] = can2RxMessage.Data[0]; ptr[1] = can2RxMessage.Data[1]; ptr[2] = can2RxMessage.Data[2]; ptr[3] = can2RxMessage.Data[3]; MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_RED,NEEDLE2_CENTER_X,NEEDLE2_CENTER_Y,-receivedGyro2,-receivedGyro2_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); receivedGyro2_prev = receivedGyro2; }else if (( can2RxMessage.ExtId & 0x0000FFFF) == 3){ ptr = & receivedGyro3; ptr[0] = can2RxMessage.Data[0]; ptr[1] = can2RxMessage.Data[1]; ptr[2] = can2RxMessage.Data[2]; ptr[3] = can2RxMessage.Data[3]; MoveNeedle(LCD_BACKGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_RED,NEEDLE3_CENTER_X,NEEDLE3_CENTER_Y,-receivedGyro3,-receivedGyro3_prev,NEEDLE_RADIUS,NEEDLE_BASE_WIDTH); receivedGyro3_prev = receivedGyro3; } // LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); // sprintf(lcd_text_main," ID :%d ", can2RxMessage.StdId); // LCD_DisplayStringLine(LINE(1), (uint8_t*)lcd_text_main); // LCD_SetColors(LCD_COLOR_BLACK,LCD_COLOR_WHITE-1); // sprintf(lcd_text_main," Data :%f ", receivedGyro); // LCD_DisplayStringLine(LINE(2), (uint8_t*)lcd_text_main); }while(CAN_MessagePending(CAN2, CAN_FIFO0) > 0); } // { // uint8_t status=0; // while(CAN_TransmitStatus(CAN2, 0) != CAN_TxStatus_Ok ){ // status = CAN_TransmitStatus(CAN2, 0); // if(status == CAN_TxStatus_Failed){ // GPIO_ToggleBits(GPIOG,GPIO_Pin_14); // } // } // } // TxMessage.StdId = (uint32_t)board_ID; // TxMessage.RTR = CAN_RTR_DATA; // TxMessage.IDE = CAN_ID_STD; // TxMessage.DLC = 8; // TxMessage.Data[0] = 0x01; // TxMessage.Data[1] = 0x01; // TxMessage.Data[2] = 0x01; // TxMessage.Data[3] = 0x01; // TxMessage.Data[4] = 0x01; // TxMessage.Data[5] = 0x01; // TxMessage.Data[6] = 0x01; // TxMessage.Data[7] = 0x01; // CAN_Transmit(CAN2, &TxMessage); //CAN2_TransmitGyro(test_int++,test_float); test_float += 0.1f; GyX_prev = GyX; GyZ_prev = GyZ; GyY_prev = GyY; runner += 1.0f; /* Faster method */ //MoveAndUpdateRectangular(LCD_FOREGROUND_LAYER,&buffer_screen,x_len,y_len,LCD_COLOR_BLACK,&prev_rect, &rect1); /* Regular method */ // PadRectangular(&buffer_screen,x_len,y_len,LCD_COLOR_WHITE, &prev_rect); // PadRectangular(&buffer_screen,x_len,y_len,LCD_COLOR_BLACK, &rect1); //DrawBufferToScreen(LCD_FOREGROUND_LAYER,buf_ptr,0,0, x_len,y_len); } }
int main(void) { int scope1dt,scope1t,scope1s,scope1d,scope1j; int scope2dt,scope2t,scope2s,scope2d,scope2j; char znakr; char znak; GPIO_InitTypeDef GPIO_str; uint8_t ii; uint8_t i = 0; /* SysTick end of count event each 0,01ms 0,00001*/// 0,01ms -100000 RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100000); /* Accelerometer Configuration */ Acc_Config(); Demo_GyroConfig(); ii = SystemCoreClock; /* This is a way to read the System core clock */ CONF_TIMERS(); CONF_PWMIN(); confI2C(); while(1) I2C_start(); /////////// Przyciski RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); GPIO_str.GPIO_Pin= GPIO_Pin_4 | GPIO_Pin_5; GPIO_str.GPIO_Mode=GPIO_Mode_IN; GPIO_str.GPIO_PuPd=GPIO_PuPd_UP; GPIO_str.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_str); ii = 0; USART2_Init(115200); GPIO_SetBits(GPIOD, GPIO_Pin_0); LEFT =3400; RIGHT=3400; FRONT=3400; REAR=3400; //delay_ms(4000); while (1) { esf=SystemCoreClock/360/(TIM2->CCR2); esd = (TIM2->CCR1*100);///(TIM2->CCR2); throttle=(int)(esd*32.0/156.0-1236.0); esf3=SystemCoreClock/360/(TIM3->CCR2); if(throttle>6600) throttle=3400; esd3 = (TIM3->CCR1*100);///(TIM3->CCR2); pitch_zadany=(float)(-6.0*esd3/1800.0+100.0)+0.0; if(pitch_zadany>40 || pitch_zadany<-40) pitch_zadany=0; esd4 = (TIM4->CCR1*100); roll_zadany=(float)(-6.0*esd4/1800.0+100.0); if(roll_zadany>40 || roll_zadany<-40) roll_zadany=0; //esd5 =(TIM8->CCR2*100)/TIM8->CCR1; //yaw_zadany=(float)(-6.0*esd5/1800.0+100.0); //if(GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)==0) //flaga=0; //IMU/////////////////////////////////////////////////////// Acc_ReadData(AccBuffer); for(i=0;i<3;i++) AccBuffer[i] /= 100.0f; acc_x=AccBuffer[0]; acc_y=AccBuffer[1]; acc_z=AccBuffer[2]; Demo_GyroReadAngRate(Buffer); gyr_x=Buffer[0]; gyr_y=Buffer[1]; gyr_z=Buffer[2]; MadgwickAHRSupdateIMU( -gyr_y*0.01745, gyr_x*0.01745, gyr_z*0.01745, acc_x, acc_y, acc_z); roll=180/PI*atan2(2*(q2*q3+q0*q1),1-2*(q1*q1+q2*q2));//(q0*q0-q1*q1-q2*q2+q3*q3)); pitch=180/PI*asin(-2*(q1*q3-q0*q2)); yaw=180/PI*atan2(2*(q1*q2+q0*q3),(1-2*(q3*q3+q2*q2))); scope1=PIDY*1000;//niebieski scope2=yaw*1000;//czerwony if(scope1>0) znak='+'; else znak='-'; if(scope2>0) znakr='+'; else znakr='-'; scope1=abs(scope1); scope2=abs(scope2); CZAS_S=(float)CZAS/100000.0; CZAS=0; scope1dt=scope1/10000; scope1t=scope1/1000-scope1dt*10; scope1s=scope1/100-scope1dt*100-scope1t*10; scope1d=scope1/10-scope1dt*1000-scope1t*100-scope1s*10; scope1j=scope1-scope1dt*10000-scope1t*1000-scope1s*100-scope1d*10; scope2dt=scope2/10000; scope2t=scope2/1000-scope2dt*10; scope2s=scope2/100-scope2dt*100-scope2t*10; scope2d=scope2/10-scope2dt*1000-scope2t*100-scope2s*10; scope2j=scope2-scope2dt*10000-scope2t*1000-scope2s*100-scope2d*10; printf("%c%d%d%d%d%d %c%d%d%d%d%d\r\n",znak,scope1dt,scope1t,scope1s,scope1d,scope1j,znakr,scope2dt,scope2t,scope2s,scope2d,scope2j); /*PIDY=-PIDyawrate(yaw_zadany, yaw); rollratezadane=PIDroll(roll_zadany, roll); PIDR=PIDrollrate(rollratezadane, -gyr_y); speedLeft=(int)((float)throttle+PIDR-PIDY);//throttle+PIDR; speedRight=(int)((float)throttle-PIDR-PIDY); pitchratezadane=PIDpitch(pitch_zadany, pitch); PIDP=PIDpitchrate(pitchratezadane, gyr_x);//float PIDpitchrollrate(float zadana, float aktualna) speedFront=(int)((float)throttle-PIDP+PIDY); speedRear=(int)((float)throttle+PIDP+PIDY); */ if(speedFront>speedmax) speedFront=speedmax; else if(speedFront<speedmin) speedFront=speedmin; else ; if(speedRear>speedmax) speedRear=speedmax; else if(speedRear<speedmin) speedRear=speedmin; else ; if(speedLeft>speedmax) speedLeft=speedmax; else if(speedLeft<speedmin) speedLeft=speedmin; else ; if(speedRight>speedmax) speedRight=speedmax; else if(speedRight<speedmin) speedRight=speedmin; else ; if(throttle>3450) { LEFT =(int)speedLeft; RIGHT=(int)speedRight; FRONT=3400;//(int)speedFront; REAR=3400;//(int)speedRear; } else { LEFT =3400; RIGHT=3400; FRONT=3400; REAR=3400; } } return 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 file (startup_stm32f0xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f0xx.c file*/ /* Setup SysTick Timer for 1 msec interrupts.*/ if (SysTick_Config(SystemCoreClock / 1000)) { /* Capture error */ while (1); } /* Initialize LEDs, User Button on STM32F072B-DISCO board ***********/ STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_EXTI); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_LEDInit(LED5); STM_EVAL_LEDInit(LED6); /* Initialize Mems Gyroscope */ Demo_GyroConfig(); /* Initialize USB Device */ USBD_Init(&USB_Device_dev, &USR_desc, &USBD_HID_cb, &USR_cb); /* Init STMTouch driver */ TSL_user_Init(); /* End of Initialisation */ /* Delay 1s to select Test Program or to go directly through the demo*/ Delay (1000); if (STM_EVAL_PBGetState(BUTTON_USER) == Bit_SET ) { /* Wait for User button is released */ while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_RESET) {} /* Set ButtonPressed at 0 value */ ButtonPressed = 0; /* LED test : Blinking LEDs */ LED_Test(); /* Wait for User button to be pressed to switch to USB Test the cursor move in square path and led On corresponding to such direction */ USB_Test(); /* Move Discovery board to execute MEMS Test, Mems detect the angular rate and led On corresponding to such direction*/ MEMS_Test(); /* Wait for User button to be pressed to switch to Touch Sensor Test each TouchKey pointed correspond to specific Leds On, test can performed in both direction */ LTS_Test(); } /* Infinite loop */ while (1) { Demo(); } }