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(); }