void transmitGyroState(){
uint8_t payload[8];
// fill in payload
ReadGyro(payload);
CDC_Device_Flush(&VirtualSerial_CDC_Interface);
packet_t* pkt = PKT_Create(PKTYPE_STATUS_3GYRO_RAW, seq++, payload, 8);
uint8_t len = PKT_ToBuffer(pkt, txBuffer);
for(int i=0; i < len; i++) {
sendByte(txBuffer[i]);
handleUSB();
}
free(pkt);
}
void DemoGyro()
{
uint8_t Xval, Yval = 0x00;
/* Waiting User Button is pressed */
while (KeyPressCount()==0)
{
/* Wait for data ready - Demo just wait for 5ms */
Delay(5);
/* LEDs Off */
SetLed(ALL|OFF);
/* Read Gyro Angular data */
ReadGyro(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)
{
SetLed(LED10);
}
if ((int8_t)Buffer[0] < -5.0f)
{
SetLed(LED3);
}
}
else
{
if ((int8_t)Buffer[1] < -5.0f)
{
SetLed(LED6);
}
if ((int8_t)Buffer[1] > 5.0f)
{
SetLed(LED7);
}
}
}
SetLed(ALL|OFF);
}
void Controllore::run()
{
ReadGyro();
SPEED_CONTROL();
}
