/**
* @brief Mems gyroscope Demo application.
* @param None
* @retval None
*/
void Routine_MEMS(void)
{
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
Buffer[0] = (int8_t)Buffer[0] - (int8_t)Gyro[0];
Buffer[1] = (int8_t)Buffer[1] - (int8_t)Gyro[1];
/* Update autoreload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
if ( Xval>Yval)
{
if ((int16_t)Buffer[0] > 40)
{
/* Clear the LCD */
/* LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_MAGENTA);
LCD_DrawFullRect(100, 40, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 310); */
log_append("docea_monitoring accel_state down\n");
// Delay(50);
}
if ((int16_t)Buffer[0] < -40)
{
/* Clear the LCD */
/* LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_RED);
LCD_DrawFullRect(100, 160, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 10); */
log_append("docea_monitoring accel_state up\n");
// Delay(50);
}
}
else
{
if ((int16_t)Buffer[1] < -40)
{
/* Clear the LCD */
/* LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DrawFullRect(120, 140, 100, 40);
LCD_FillTriangle(120, 120, 5, 60, 260, 160); */
log_append("docea_monitoring accel_state left\n");
// Delay(50);
}
if ((int16_t)Buffer[1] > 40)
{
/* Clear the LCD */
/* LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_BLUE);
LCD_DrawFullRect(20, 140, 100, 40);
LCD_FillTriangle(120, 120, 235, 60, 260, 160); */
log_append("docea_monitoring accel_state right\n");
// Delay(50);
}
}
}
/**
* @brief Mems gyroscope Demo application.
* @param None
* @retval None
*/
static void Demo_MEMS(void)
{
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
Buffer[0] = (int8_t)Buffer[0] - (int8_t)Gyro[0];
Buffer[1] = (int8_t)Buffer[1] - (int8_t)Gyro[1];
/* Update autoreload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
if ( Xval>Yval)
{
if ((int16_t)Buffer[0] > 40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_MAGENTA);
LCD_DrawFullRect(100, 40, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 310);
Delay(50);
}
if ((int16_t)Buffer[0] < -40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_RED);
LCD_DrawFullRect(100, 160, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 10);
Delay(50);
}
}
else
{
if ((int16_t)Buffer[1] < -40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DrawFullRect(120, 140, 100, 40);
LCD_FillTriangle(120, 120, 5, 60, 260, 160);
Delay(50);
}
if ((int16_t)Buffer[1] > 40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_BLUE);
LCD_DrawFullRect(20, 140, 100, 40);
LCD_FillTriangle(120, 120, 235, 60, 260, 160);
Delay(50);
}
}
}
/**
* @brief USB device HID GetPosition
* @param None
* @retval Pointer to report
*/
static uint8_t *USBD_HID_GetPos (void)
{
static uint8_t HID_Buffer [4];
static float Buffer[6] = {0};
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
HID_Buffer[0] = 0;
HID_Buffer[1] = -(int8_t)(Buffer[2])/6;
HID_Buffer[2] = (int8_t)(Buffer[1])/6;
HID_Buffer[3] = 0;
STM_EVAL_LEDToggle(LED3);
STM_EVAL_LEDToggle(LED6);
return HID_Buffer;
}
void SysTick_Handler(void)
{
uint8_t Delay_Ticks = 1;
uint8_t* Buf;
uint32_t idx;
if (Freq == 20)
{
Delay_Ticks = 100;
}
if (Freq == 2000)
{
Delay_Ticks = 1;
}
DataReady ++;
if (DataReady >= Delay_Ticks)
{
STM_EVAL_LEDOn(LED3);
Demo_GyroReadAngRate((float*)Buffer);
Demo_CompassReadMag((float*)MagBuffer);
Demo_CompassReadAcc((float*)AccBuffer);
Buf = (uint8_t*)Buffer;
for(idx = 0; idx < 12; idx++)
Send_Buffer[idx] = Buf[idx];
Buf = (uint8_t*)MagBuffer;
for(idx = 12; idx < 24; idx++)
Send_Buffer[idx] = Buf[idx-12];
Buf = (uint8_t*)AccBuffer;
for(idx = 24; idx < 36; idx++)
Send_Buffer[idx] = Buf[idx-24];
Buf = (uint8_t*)ADC_Result;
for(idx = 36; idx < 54; idx++)
Send_Buffer[idx] = Buf[idx-36];
CDC_Send_DATA ((unsigned char*)Send_Buffer,54);
DataReady = 0;
STM_EVAL_LEDOff(LED3);
}
}
/**
* @brief USBD_HID_GetPos
* @param None
* @retval Pointer to report
*/
void USB_GetPointerData_Demo(uint8_t *pbuf)
{
static float Buffer[6] = {0};
BSP_GYRO_Init();
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
pbuf[0] = 0;
pbuf[1] = -(int8_t)(Buffer[2])/6;
pbuf[2] = (int8_t)(Buffer[1])/6;
pbuf[3] = 0;
BSP_LED_Toggle(LED3);
BSP_LED_Toggle(LED10);
BSP_LED_Toggle(LED6);
BSP_LED_Toggle(LED7);
}
/**
* @brief Calculate offset of the angular Data rate Gyroscope.
* @param GyroData : Data out pointer
* @retval None
*/
static void Gyro_SimpleCalibration(float* GyroData)
{
uint32_t BiasErrorSplNbr = 500;
int i = 0;
for (i = 0; i < BiasErrorSplNbr; i++)
{
Demo_GyroReadAngRate(GyroData);
X_BiasError += GyroData[0];
Y_BiasError += GyroData[1];
Z_BiasError += GyroData[2];
}
/* Set bias errors */
X_BiasError /= BiasErrorSplNbr;
Y_BiasError /= BiasErrorSplNbr;
Z_BiasError /= BiasErrorSplNbr;
/* Get offset value on X, Y and Z */
GyroData[0] = X_BiasError;
GyroData[1] = Y_BiasError;
GyroData[2] = Z_BiasError;
}
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 MEMS Demo using polling mode, Angular rate are detected,
* when moving Discovery, and correspondent LED is ON.
* @param None
* @retval None
*/
void MEMS_Demo(void)
{
float Buffer[6] = {0};
uint8_t Xval, Yval = 0x00;
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_SET)
{
/* 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 ((int16_t)Buffer[0] > 40)
{
/* LED5 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
if ((int16_t)Buffer[0] < -40)
{
/* LED4 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
}
else
{
if ((int16_t)Buffer[1] > 40)
{
/* LED3 On */
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
if ((int16_t)Buffer[1] < -40)
{
/* LED6 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOn(LED6);
/* Insert 250ms delay */
Delay(25);
}
}
}
/* Wait for User button is released */
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_RESET)
{}
/* Turn Off Leds */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}
/**
* @brief Angular rate are detected when moving Discovery,MEMS Test :
* Interrupt mode : LED3 On (Test KO) or LED5 is On (Interrupt mode OK)
* polling mode : correspondent LED is ON
* @param None
* @retval None
*/
void MEMS_Test(void)
{
float Buffer[6] = {0};
uint8_t Xval, Yval = 0x00;
/* Test in Interrupt mode*/
/* Enable INT1 interrupt */
L3GD20_INT1InterruptCmd(ENABLE);
/* Configure interrupts on all axes */
L3GD20_INT1InterruptConfig(L3GD20_AXES_INTERRUPT_ENABLE);
/* LED3 On */
STM_EVAL_LEDOn(LED3);
/* Wait for User button to be pressed to switch to polling mode */
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_SET)
{}
/* Wait for User button is released */
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_RESET)
{}
/* Turn Off Leds */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
/* End of Test in Interrupt mode*/
/* Start Test in polling mode*/
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_SET)
{
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
/* Update auto-reload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
if ( Xval>Yval)
{
if ((int16_t)Buffer[0] > 40)
{
/* LED5 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
if ((int16_t)Buffer[0] < -40)
{
/* LED4 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
}
else
{
if ((int16_t)Buffer[1] > 40)
{
/* LED3 On */
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
/* Insert 250ms delay */
Delay(25);
}
if ((int16_t)Buffer[1] < -40)
{
/* LED6 On */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOn(LED6);
/* Insert 250ms delay */
Delay(25);
}
}
}
/* Wait for User button is released */
while (STM_EVAL_PBGetState(BUTTON_USER) != Bit_RESET)
{}
/* Turn Off Leds */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}