void Drive::update() { updateSensors(); // Determine elapsed time since last update() struct timeval currenttime; gettimeofday(¤ttime, NULL); float dtime = currenttime.tv_sec - mLastUpdate.tv_sec + (currenttime.tv_usec - mLastUpdate.tv_usec) / 1000000.0f; mLastUpdate = currenttime; mTargetYaw += mRotate * dtime; // Calculate average sensor readings over time Vector3<float> accel = averageAccelerometer(); Vector3<float> gyro = averageGyroscope(); // Adjust for calibration accel.x -= mAccelOffset.x; accel.y -= mAccelOffset.y; accel.z -= mAccelOffset.z; gyro.x -= mGyroOffset.x; gyro.y -= mGyroOffset.y; gyro.z -= mGyroOffset.z; calculateOrientation(dtime, accel, gyro); stabilize(gyro); try { for (int i = 0; i < 4; ++i) mMotors[i]->update(); } catch (Exception &e) { std::cout << "Motor update failure." << std::endl; } }
void CalculateOrientation(void * Parameters) { /* Go to infinite loop when Bus Fault exception occurs */ while (1){ if(xSemaphore != NULL) { if(xSemaphoreTake(xSemaphore, 0)==pdTRUE){ calculateOrientation(); } } } }
/* * TIM2_IRQHandler - The interrupt handler for timer 2. There are 4 possible * interrupt channels. * * Inputs: None * Outputs: None * Modifies: Possibly extern global var pid_set_flag */ void TIM2_IRQHandler(void) { // Channel 1 is the interrupt for the detectEmegency() function if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET) { // Clear the IRQ TIM_ClearITPendingBit(TIM2, TIM_IT_CC1); detectEmergency(); // Interrupt needs to trigger again in 10ms capture = TIM_GetCapture1(TIM2); TIM_SetCompare1(TIM2, capture + DELAY_10MS); // Channel 2 is the interrupt for refreshSensorData() } else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET) { // Clear the IRQ TIM_ClearITPendingBit(TIM2, TIM_IT_CC2); refreshSensorData(); // Interrupt needs to trigger again in 100ms capture = TIM_GetCapture2(TIM2); TIM_SetCompare2(TIM2, capture + DELAY_100MS); // Channel 3 is the interupt for calculateOrientation() } else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET) { // Clear the IRQ TIM_ClearITPendingBit(TIM2, TIM_IT_CC2); calculateOrientation(); // This function refreshes once a second, so no need to reset the // compare register // Channel 4 is the interrupt for updatePid() } else { // Clear the IRQ TIM_ClearITPendingBit(TIM2, TIM_IT_CC4); // This function returns new motor PIDs, which means the main // program needs to do additional processing after the interrupt updatePid(&motor_speeds); pid_set_flag = True; // This function refreshes once a second, so no need to reset the // compare register } }
void vTaskCalculateOrientation(void *pvParameters) { // Initialize delay wake time portTickType wakeTime; portTickType frequency = 100; wakeTime = xTaskGetTickCount(); // Execute & delay while(1){ // Block until available xSemaphoreTake(dataSemaphore, portMAX_DELAY); calculateOrientation(); vTaskDelayUntil(&wakeTime, frequency); } }
Drive::Drive(PWM *pwm, Accelerometer *accel, Gyroscope *gyro, int frontleft, int frontright, int rearright, int rearleft, int update_rate, int smoothing) { mAccelerometer = accel; mGyroscope = gyro; mUpdateRate = update_rate; mTimerID = 0; mTimerEnabled = false; mSmoothing = smoothing; mAccelValue = new Vector3<float>[mSmoothing]; for (int i = 0; i < mSmoothing; ++i) { mAccelValue[i].x = 0.0f; mAccelValue[i].y = 0.0f; mAccelValue[i].z = 0.0f; } mGyroValue = new Vector3<float>[mSmoothing]; for (int i = 0; i < mSmoothing; ++i) { mGyroValue[i].x = 0.0f; mGyroValue[i].y = 0.0f; mGyroValue[i].z = 0.0f; } mRotate = 0.0f; mTranslate.x = 0.0f; mTranslate.y = 0.0f; mTranslate.z = 0.0f; mRoll = 0.0f; mPitch = 0.0f; mYaw = 0.0f; mTargetRoll = 0.0f; mTargetPitch = 0.0f; mTargetYaw = 0.0f; mPIDRollAngle = new PIDController(mTargetRoll, 0.00f, 0.00f, 0.00f, 5); mPIDPitchAngle = new PIDController(mTargetPitch, 0.00f, 0.00f, 0.00f, 5); mPIDYawAngle = new PIDController(mTargetYaw, 0.00f, 0.00f, 0.00f, 5); mPIDRollRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mPIDPitchRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mPIDYawRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mMotors[0] = new Motor(pwm, frontleft, 1.26f, 1.6f); mMotors[1] = new Motor(pwm, frontright, 1.26f, 1.6f); mMotors[2] = new Motor(pwm, rearright, 1.26f, 1.6f); mMotors[3] = new Motor(pwm, rearleft, 1.26f, 1.6f); mAccelOffset.x = 0.0f; mAccelOffset.y = 0.0f; mAccelOffset.z = 0.0f; mGyroOffset.x = 0.0f; mGyroOffset.y = 0.0f; mGyroOffset.z = 0.0f; try { loadCalibration("calibration.ini"); } catch (CalibrationException &e) { std::cout << "WARNING: Continuimg without calibration" << std::endl; } // Wait for motors to prime usleep(3000000); gettimeofday(&mLastUpdate, NULL); // Pre-populate mAccelValue and mGyroValue arrays Vector3<float> aval; Vector3<float> gval; for (int i = 0; i < mSmoothing; ++i) { aval = mAccelerometer->read(); gval = mGyroscope->read(); mAccelValue[i] = aval; mGyroValue[i] = gval; usleep(10000); // 10,000us = 100Hz } mAccelValueCurrent = 0; mGyroValueCurrent = 0; Vector3<float> accelAvg = averageAccelerometer(); Vector3<float> gyroAvg = averageGyroscope(); calculateOrientation(0.0f, accelAvg, gyroAvg); // Make sure the motors are resting to start stop(); }