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