Exemple #1
0
/**
 * @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);

	/* Configure the USB */
	USB_Config();

	/* Accelerometer Configuration */
	Acc_Config();

	/* Init push buttons */
	STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_GPIO);

	/* Init LEDs */
	STM_EVAL_LEDInit(LED3);

	/* Infinite loop */
	while (1) {

		/* Wait for data ready */
		while (DataReady != 0x04) {
		}
		DataReady = 0x00;

		/* Get Data Accelerometer */
		Acc_ReadData(AccBuffer);

		for (i = 0; i < 3; i++)
			AccBuffer[i] /= 100.0f;

		// Get mouse info
		Mouse_Buffer = USBD_HID_GetPos();
		/* Update the cursor position */
		if (!mouseZeroPacketOutputted || (Mouse_Buffer[0] != 0 || Mouse_Buffer[1] != 0 || Mouse_Buffer[2] != 0 || Mouse_Buffer[3] != 0))
		{
			/* Reset the control token to inform upper layer that a transfer is ongoing */
			PrevXferComplete = 0;

			/* Copy mouse position info in ENDP1 Tx Packet Memory Area*/
			USB_SIL_Write(EP1_IN, Mouse_Buffer, 4);

			/* Enable endpoint for transmission */
			SetEPTxValid(ENDP1 );
		}

		// We find that the mouse buffer is zero so we know that we have outputted the zero packet
		// This means we can stop transferring data
		if((Mouse_Buffer[0] == 0 && Mouse_Buffer[1] == 0 && Mouse_Buffer[2] == 0 && Mouse_Buffer[3] == 0))
			mouseZeroPacketOutputted = 1;
		else
			mouseZeroPacketOutputted = 0;

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

  /* Configure the USB */
  USB_Config();

  /* Accelerometer Configuration */
  Acc_Config();

  /* Infinite loop */
  while (1)
  {

    /* Wait for data ready */
    while(DataReady !=0x02)
    {}
    DataReady = 0x00;

    /* Get Data Accelerometer */
    Acc_ReadData(AccBuffer);

    for(i=0;i<3;i++)
      AccBuffer[i] /= 100.0f;

    /* Get position */
    Mouse_Buffer = USBD_HID_GetPos();
    /* Update the cursor position */
    if((Mouse_Buffer[1] != 0) ||(Mouse_Buffer[2] != 0))
    {
      /* Reset the control token to inform upper layer that a transfer is ongoing */
      PrevXferComplete = 0;

      /* Copy mouse position info in ENDP1 Tx Packet Memory Area*/
      USB_SIL_Write(EP1_IN, Mouse_Buffer, 4);

      /* Enable endpoint for transmission */
      SetEPTxValid(ENDP1);
    }
  }
}
Exemple #3
0
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;
}