static void cmd_adjust(BaseSequentialStream *chp, int argc, char *argv[]) { float gyrodata[2][3]; (void) argc; (void) argv; chprintf(chp, "Adjust the motorcycle vertically and press the User button\r\n"); while (!palReadPad(GPIOA, GPIOA_BUTTON)) { } if (!readGyro(gyrodata[0])) { chprintf(chp, "Error getting gyrodata!\r\n"); return; } else { chprintf(chp, "Got gyrodata for the 1st time!\r\n"); } chThdSleepMilliseconds(5000); chprintf(chp, "Now place it on the side stand and press the button again\r\n"); while (!palReadPad(GPIOA, GPIOA_BUTTON)) { } if (!readGyro(gyrodata[1])) { chprintf(chp, "Error getting gyrodata!\r\n"); return; } else { chprintf(chp, "Got gyrodata for the 2nd time!\r\n"); } chprintf(chp, "Got your gyrodata successfully\r\n"); uint8_t i = 0; for (i = 0; i < 2; i++) { chprintf(chp, "%d\t %f\t %f\t %f\r\n", i, gyrodata[i][0], gyrodata[i][1], gyrodata[i][2]); } }
void computeGyroRTBias(void) { uint8_t axis; uint16_t samples; float gyroSum[3] = { 0.0f, 0.0f, 0.0f }; gyroCalibrating = true; for (samples = 0; samples < 2000; samples++) { readGyro(); computeGyroTCBias(); gyroSum[ROLL] += rawGyro[ROLL].value - gyroTCBias[ROLL]; gyroSum[PITCH] += rawGyro[PITCH].value - gyroTCBias[PITCH]; gyroSum[YAW] += rawGyro[YAW].value - gyroTCBias[YAW]; delayMicroseconds(1000); } for (axis = ROLL; axis < 3; axis++) { gyroRTBias[axis] = (float) gyroSum[axis] / 2000.0f; } gyroCalibrating = false; }
/*************************************************************************** PUBLIC FUNCTIONS ***************************************************************************/ void Adafruit_LSM9DS0::read() { /* Read all the sensors. */ readAccel(); readMag(); readGyro(); readTemp(); }
/* * * * * * * * * * * Systick Handler * * * * * * * * * * */ void systick() { if (b_useIR) { readSensor(); } if (b_useGyro) { readGyro(); } if (b_useSpeedProfile) { speedProfile(); } }
void MPU6000::init() { // Reset device. txbuf[0] = mpu6000::REG_PWR_MGMT_1; txbuf[1] = 0x80; exchange(2); chThdSleepMilliseconds(100); // TODO(yoos): Check whether or not datasheet specifies this holdoff. // Set sample rate to 1 kHz. txbuf[0] = mpu6000::REG_SMPLRT_DIV; txbuf[1] = 0x00; exchange(2); // Set DLPF to 4 (20 Hz gyro bandwidth1 Hz accelerometer bandwidth) txbuf[0] = mpu6000::REG_CONFIG; txbuf[1] = 0x04; exchange(2); // Wake up device and set clock source to Gyro Z. txbuf[0] = mpu6000::REG_PWR_MGMT_1; txbuf[1] = 0x03; exchange(2); // Set gyro full range to 2000 dps. We first read in the current register // value so we can change what we need and leave everything else alone. // See DS p. 12. txbuf[0] = mpu6000::REG_GYRO_CONFIG | (1<<7); exchange(2); chThdSleepMicroseconds(0); // TODO(yoos): Without this, the GYRO_CONFIG register does not get set. This was not the case in the old C firmware. Why? txbuf[0] = mpu6000::REG_GYRO_CONFIG; txbuf[1] = (rxbuf[1] & ~0x18) | 0x18; exchange(2); // Set accelerometer full range to 16 g. See DS p. 13. txbuf[0] = mpu6000::REG_ACCEL_CONFIG | (1<<7); exchange(2); txbuf[0] = mpu6000::REG_ACCEL_CONFIG; txbuf[1] = (rxbuf[1] & ~0x18) | 0x18; exchange(2); // Read once to clear out bad data? readGyro(); readAccel(); }
static void cmd_gyrodata(BaseSequentialStream *chp, int argc, char *argv[]) { float gyrodata[3]; uint32_t times = 5; uint32_t delay = 200; if (argc >= 1) { times = atoi(argv[0]); } if (argc >= 2) { delay = atoi(argv[1]); } uint32_t i = 0; chprintf(chp, " Number 1\t Number 2\t Number 3\t #\r\n"); for (i = 0; i < times; i++) { chThdSleepMilliseconds(delay); if (readGyro(gyrodata)) { chprintf(chp, " %f\t %f\t %f\t %d\r\n", gyrodata[0], gyrodata[1], gyrodata[2], i); } } }
void Gyroscope::calibrateStep() { // Don't continue to average if acceptable offsets have already been computed. if (calibrated()) { return; } GyroscopeReading reading = readGyro(); for (std::size_t i = 0; i < 3; i++) { offsets[i] = (offsets[i] * calibrationCount + reading.axes[i]) / (calibrationCount + 1); } calibrationCount++; if (std::abs(reading.axes[0] > 0.1) || std::abs(reading.axes[1] > 0.1) || std::abs(reading.axes[2] > 0.1)) { calibrationCount = 0; } }
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average // them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to // subtract the biases ourselves. This results in a more accurate measurement in general and can // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner // is good practice. void LSM9DS1::calibrate(bool autoCalc) { uint8_t data[6] = {0, 0, 0, 0, 0, 0}; uint8_t samples = 0; int ii; int32_t aBiasRawTemp[3] = {0, 0, 0}; int32_t gBiasRawTemp[3] = {0, 0, 0}; // Turn on FIFO and set threshold to 32 samples enableFIFO(true); setFIFO(FIFO_THS, 0x1F); while (samples < 0x1F) { samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples } for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO readGyro(); gBiasRawTemp[0] += gx; gBiasRawTemp[1] += gy; gBiasRawTemp[2] += gz; readAccel(); aBiasRawTemp[0] += ax; aBiasRawTemp[1] += ay; aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up! } for (ii = 0; ii < 3; ii++) { gBiasRaw[ii] = gBiasRawTemp[ii] / samples; gBias[ii] = calcGyro(gBiasRaw[ii]); aBiasRaw[ii] = aBiasRawTemp[ii] / samples; aBias[ii] = calcAccel(aBiasRaw[ii]); } enableFIFO(false); setFIFO(FIFO_OFF, 0x00); if (autoCalc) _autoCalc = true; }
void MAIN(void) { Quaternion lage(1, 0,0,0); Vector3D vx(1,0,0), v; double giroX, giroY, giroZ; Quaternion deltaLage; YPR ypr; for(int i = 0; i <100; i++) { // shall be: while(1) ypr = lage.toYPR(); v = vx.qRotate(lage); PRINTF("------------ Step %d\n", i); PRINTF("Currten attitude quat:"); lage.print(); PRINTF("Currten attitude v :"); v.print(); PRINTF("Currten attitude ypr :"); ypr.print(); ypr = lage.toYPRnils(); PRINTF("Currten attitude yprN:"); ypr.print(); readGyro(giroX, giroY, giroZ); ypr = YPR(giroX, giroY, giroZ); PRINTF("gyro-ypr :\t\t "); ypr.print(); deltaLage = ypr.toQuaternion().normalize(); PRINTF("gyro-quat:"); deltaLage.print(); lage = deltaLage * lage; // operator* for quaternions } PRINTF("----------- Test END -------------------\n"); }
void GY_85::GyroCalibrate() { static int tmpx = 0; static int tmpy = 0; static int tmpz = 0; g_offx = 0; g_offy = 0; g_offz = 0; for( uint8_t i = 0; i < 10; i ++ ) //take the mean from 10 gyro probes and divide it from the current probe { delay(10); float* gp = readGyro(); tmpx += *( gp); tmpy += *(++gp); tmpz += *(++gp); } g_offx = tmpx/10; g_offy = tmpy/10; g_offz = tmpz/10; }
Window::Window(QWidget* parent) : QWidget(parent) { QGridLayout* grid = new QGridLayout; grid->addWidget(createAccelerometerGroup(), 0, 0); grid->addWidget(createGyroscopeGroup(), 0, 1); grid->addWidget(createMagnetometerGroup(), 1, 0); grid->addWidget(createButtonsGroup(), 1, 1); setLayout(grid); setWindowTitle(tr("psmoveosc")); resize(480, 320); psm = new PSMoveQt(0); psm->setColor(Qt::red); psm->setEnabled(true); connect(psm, SIGNAL(accelerometerChanged()), this, SLOT(readAccelerometer())); connect(psm, SIGNAL(gyroChanged()), this, SLOT(readGyro())); connect(psm, SIGNAL(magnetometerChanged()), this, SLOT(readMagnetometer())); connect(psm, SIGNAL(triggerChanged()), this, SLOT(setTrigger())); connect(psm, SIGNAL(buttonPressed(int)), this, SLOT(onButton(int))); connect(psm, SIGNAL(buttonReleased(int)), this, SLOT(onButton(int))); }
void ITG3200::readGyro(float *_GyroXYZ){ readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); }
void L3G4200D::readGyro(float *_GyroXYZ){ readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); }
int main(void) { int i; Systick_Configuration(); LED_Configuration(); button_Configuration(); usart1_Configuration(9600); SPI_Configuration(); TIM4_PWM_Init(); Encoder_Configration(); buzzer_Configuration(); ADC_Config(); //curSpeedX = 0; //curSpeedW = 0; //shortBeep(2000, 8000); while(1) { readSensor(); readGyro(); readVolMeter(); printf("LF %d RF %d DL %d DR %d aSpeed %d angle %d voltage %d lenc %d renc %d\r\n", LFSensor, RFSensor, DLSensor, DRSensor, aSpeed, angle, voltage, getLeftEncCount(), getRightEncCount()); displayMatrix("UCLA"); setLeftPwm(100); setRightPwm(100); delay_ms(1000); } //forwardDistance(4000,0,0,true); //displayMatrix("Sped"); //targetSpeedX = 100; //delay_ms(2000); // //targetSpeedX = 100; //delay_ms(2000); //printf("==============================================\n\r=======================================================\r\n"); //delay_ms(1000); //displayMatrix("Wat"); //delay_ms(1000); //displayMatrix("STOP"); //displayMatrix("GO"); turnRightAngle(LEFT); targetSpeedW = 0; delay_ms(1000); turnRightAngle(RIGHT); targetSpeedW = 0; delay_ms(1000); displayMatrix("STOP"); delay_ms(3000); targetSpeedX = 0; // targetSpeedW = 10; delay_ms(1000); targetSpeedX = 0; targetSpeedW = 0; return 0; }
void readSensors() { readGyro(); readAccel(); readMag(); }
void SysTick_Handler(void) { uint8_t index; uint32_t currentTime; sysTickCycleCounter = *DWT_CYCCNT; sysTickUptime++; #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) if ((systemReady == true) && (accelCalibrating == false) && (gyroCalibrating == false) && (magCalibrating == false)) #endif #if defined(USE_CHR6DM_AHRS) if ((systemReady == true) && (accelCalibrating == false) && (gyroCalibrating == false) && (magCalibrating == false) && (ahrsCalibrating == false)) #endif { frameCounter++; if (frameCounter > FRAME_COUNT) frameCounter = 1; /////////////////////////////// currentTime = micros(); deltaTime1000Hz = currentTime - previous1000HzTime; previous1000HzTime = currentTime; readAccel(); #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) accelSum200Hz[XAXIS] += rawAccel[XAXIS].value; accelSum200Hz[YAXIS] += rawAccel[YAXIS].value; accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value; #endif accelSum100Hz[XAXIS] += rawAccel[XAXIS].value; accelSum100Hz[YAXIS] += rawAccel[YAXIS].value; accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value; readGyro(); gyroSum200Hz[ROLL] += rawGyro[ROLL].value; gyroSum200Hz[PITCH] += rawGyro[PITCH].value; gyroSum200Hz[YAW] += rawGyro[YAW].value; gyroSum100Hz[ROLL] += rawGyro[ROLL].value; gyroSum100Hz[PITCH] += rawGyro[PITCH].value; gyroSum100Hz[YAW] += rawGyro[YAW].value; gyroSum500Hz[ROLL] += rawGyro[ROLL].value; gyroSum500Hz[PITCH] += rawGyro[PITCH].value; gyroSum500Hz[YAW] += rawGyro[YAW].value; /////////////////////////////// if ((frameCounter % COUNT_500HZ) == 0) { frame_500Hz = true; for (index = 0; index < 3; index++) { gyroSummedSamples500Hz[index] = gyroSum500Hz[index]; gyroSum500Hz[index] = 0.0f; } } /////////////////////////////// if ((frameCounter % COUNT_200HZ) == 0) { frame_200Hz = true; for (index = 0; index < 3; index++) { #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) accelSummedSamples200Hz[index] = accelSum200Hz[index]; accelSum200Hz[index] = 0.0f; #endif gyroSummedSamples200Hz[index] = gyroSum200Hz[index]; gyroSum200Hz[index] = 0.0f; } } /////////////////////////////// if ((frameCounter % COUNT_100HZ) == 0) { frame_100Hz = true; for (index = 0; index < 3; index++) { accelSummedSamples100Hz[index] = accelSum100Hz[index]; accelSum100Hz[index] = 0.0f; gyroSummedSamples100Hz[index] = gyroSum100Hz[index]; gyroSum100Hz[index] = 0.0f; } if (frameCounter == COUNT_100HZ) { readTemperatureRequestPressure(); } else if (frameCounter == FRAME_COUNT) { readPressureRequestTemperature(); } else { readPressureRequestPressure(); } pressureSum += uncompensatedPressure.value; } /////////////////////////////// if (((frameCounter + 1) % COUNT_50HZ) == 0) { readMag(); magSum[XAXIS] += rawMag[XAXIS].value; magSum[YAXIS] += rawMag[YAXIS].value; magSum[ZAXIS] += rawMag[ZAXIS].value; } if ((frameCounter % COUNT_50HZ) == 0) { frame_50Hz = true; } /////////////////////////////// if ((frameCounter % COUNT_10HZ) == 0) frame_10Hz = true; /////////////////////////////// if ((frameCounter % COUNT_5HZ) == 0) frame_5Hz = true; /////////////////////////////// if ((frameCounter % COUNT_1HZ) == 0) frame_1Hz = true; /////////////////////////////////// executionTime1000Hz = micros() - currentTime; /////////////////////////////// } }
task MapRoom() { float botLastXCoordinate = 0.0; float botLastYCoordinate = 0.0; float botCurrentXCoordinate = 0.0; float botCurrentYCoordinate = 0.0; int wallXCoordinate = 0; int wallYCoordinate = 0; int xCoordinateDisplayOffset = 50; int yCoordinateDisplayOffset = 40; startGyro(); resetGyro(); eraseDisplay(); while(true) { //Robot position botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro()); botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro()); //Wall mapping wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro()); wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro()); nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset); ResetEncoderValue(); botLastXCoordinate = botCurrentXCoordinate; botLastYCoordinate = botCurrentYCoordinate; wait1Msec(20); } }
int main() { int32_t bemf_vals[4] = {0,0,0,0}; int32_t bemf_vals_filt[4] = {0,0,0,0}; init(); // set up DMA/SPI buffers init_rx_buffer(); init_tx_buffer(); // set up pid structs pid_struct pid_structs[4]; { uint8_t i; for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i); } update_dig_pin_configs(); config_adc_in_from_regs(); //debug_printf("starting\n"); int low_volt_alarmed = 0; // Loop until button is pressed uint32_t count = 0; while (1) { count += 1; { // only sample motor backemf 1/4 of the time const uint8_t bemf_update_time = (count % 4 == 1); if (bemf_update_time) { // idle breaking MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1; MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2; MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1; MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2; MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1; MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2; MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1; MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2; } // let the motor coast // delay_us(700); // now I squeeze in most sensor updates instead of just sleeping uint32_t before = usCount; update_dig_pins(); int16_t batt = adc_update(); readAccel(); readMag(); readGyro(); if (batt < 636) // about 5.75 volts { configDigitalOutPin(LED1_PIN, LED1_PORT); low_volt_alarmed = 1; if (count % 50 < 10) // low duty cycle to save battery { LED1_PORT->BSRRL |= LED1_PIN; // ON } else { LED1_PORT->BSRRH |= LED1_PIN; // OFF } } else if (low_volt_alarmed) { // make sure led is off coming out of the low voltage alarm configDigitalOutPin(LED1_PIN, LED1_PORT); LED1_PORT->BSRRH |= LED1_PIN; // OFF low_volt_alarmed = 0; } if (adc_dirty) { adc_dirty = 0; config_adc_in_from_regs(); } if (dig_dirty) { dig_dirty = 0; update_dig_pin_configs(); } uint32_t sensor_update_time = usCount - before; const uint16_t us_delay_needed = 700; const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed; if (got_time_to_burn) { // sleep the remainder of the coasting period before sampling bemf delay_us(us_delay_needed-sensor_update_time); } if (bemf_update_time) { update_bemfs(bemf_vals, bemf_vals_filt); // set the motor directions back up update_motor_modes(); uint8_t channel; for (channel = 0; channel < 4; ++channel) { uint8_t shift = 2*channel; uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift; motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode); } } else { // updating sensors doesnt' take as long as all of the adc for backemf updates // sleep a bit so that our time through the loop is consistent for PID control delay_us(222); } } }
void SysTick_Handler(void) { uint8_t index; uint32_t currentTime; sysTickCycleCounter = *DWT_CYCCNT; sysTickUptime++; if ((systemReady == true ) && (cliBusy == false) && (accelCalibrating == false) && (escCalibrating == false) && (gyroCalibrating == false) && (magCalibrating == false)) { frameCounter++; if (frameCounter > FRAME_COUNT) frameCounter = 1; /////////////////////////////// currentTime = micros(); deltaTime1000Hz = currentTime - previous1000HzTime; previous1000HzTime = currentTime; readAccel(); accelSum200Hz[XAXIS] += rawAccel[XAXIS].value; accelSum200Hz[YAXIS] += rawAccel[YAXIS].value; accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value; accelSum100Hz[XAXIS] += rawAccel[XAXIS].value; accelSum100Hz[YAXIS] += rawAccel[YAXIS].value; accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value; readGyro(); gyroSum200Hz[ROLL ] += rawGyro[ROLL ].value; gyroSum200Hz[PITCH] += rawGyro[PITCH].value; gyroSum200Hz[YAW ] += rawGyro[YAW ].value; /////////////////////////////// if ((frameCounter % COUNT_200HZ) == 0) { frame_200Hz = true; for (index = 0; index < 3; index++) { accelSummedSamples200Hz[index] = accelSum200Hz[index]; accelSum200Hz[index] = 0.0f; gyroSummedSamples200Hz[index] = gyroSum200Hz[index]; gyroSum200Hz[index] = 0.0f; } } /////////////////////////////// if ((frameCounter % COUNT_100HZ) == 0) { frame_100Hz = true; for (index = 0; index < 3; index++) { accelSummedSamples100Hz[index] = accelSum100Hz[index]; accelSum100Hz[index] = 0.0f; } if (frameCounter == COUNT_100HZ) readTemperatureRequestPressure(); else if (frameCounter == FRAME_COUNT) readPressureRequestTemperature(); else readPressureRequestPressure(); pressureSum += uncompensatedPressure.value; } /////////////////////////////// if ((frameCounter % COUNT_50HZ) == 0) { frame_50Hz = true; } /////////////////////////////// if (((frameCounter + 1) % COUNT_10HZ) == 0) newMagData = readMag(); if ((frameCounter % COUNT_10HZ) == 0) frame_10Hz = true; /////////////////////////////// if ((frameCounter % COUNT_5HZ) == 0) frame_5Hz = true; /////////////////////////////// if ((frameCounter % COUNT_1HZ) == 0) frame_1Hz = true; /////////////////////////////////// executionTime1000Hz = micros() - currentTime; /////////////////////////////// } }
void SAT_Gyro::readGyro(float *_GyroXYZ){ readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); }
int ITG3200::readGyro(float *_GyroXYZ){ int a=0; a=readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); return a; }
void mpu9150::getGyro(double *returnArray){ readGyro(); returnArray[0] = this->xGyro; returnArray[1] = this->yGyro; returnArray[2] = this->zGyro; }