volatile void initialize_IMU(void) { twim_init(); Enable_Acc(TWIM); Enable_Gyro(TWIM); Enable_Magn(TWIM); delay_ms(20); for (int i = 0; i < 32; i++) { Read_Gyro(); Read_Accel(); RAW_OFFSET[0] += RAW[0]; RAW_OFFSET[1] += RAW[1]; RAW_OFFSET[2] += RAW[2]; RAW_OFFSET[3] += RAW[3]; RAW_OFFSET[4] += RAW[4]; RAW_OFFSET[5] += RAW[5]; delay_ms(20); } RAW_OFFSET[0] = 0; RAW_OFFSET[1] = 0; RAW_OFFSET[2] = 0; RAW_OFFSET[3] = 0; RAW_OFFSET[4] = 0; RAW_OFFSET[5] = 0; timer_current = rtc_get_value(&AVR32_RTC); delay_ms(20); get_Angles(); }
volatile void get_Angles() { static float prev_g_d_roll = 0; timer_old = timer_current; timer_current = rtc_get_value(&AVR32_RTC); if (timer_current > timer_old) { g_dt = (float)((timer_current-timer_old))*0.001*0.125*0.98; // Time elapsed in seconds -- sensors are in seconds } else { g_dt = 0; } // GET DATA Read_Gyro(); Read_Accel(); // GYROSCOPE // Y = roll float g_d_roll = gyro_x * GYRO_GAIN_RPS; // delta roll in radians/sec //g_roll = roll + g_d_roll * g_dt; g_roll = roll + ((g_d_roll+prev_g_d_roll)/2)*g_dt; prev_g_d_roll = g_d_roll; // X = pitch float g_d_pitch = gyro_y * GYRO_GAIN_RPS; // delta pitch in radians/sec g_pitch = pitch + g_d_pitch * g_dt; // Z = yaw float g_d_yaw = gyro_z * GYRO_GAIN_RPS; // delta yaw in radians/sec g_yaw = yaw + g_d_yaw * g_dt; // ACCELEROMETER float acc_x = accel_x * 0.002; float acc_y = accel_y * 0.002; float acc_z = accel_z * 0.002; a_roll = atan2(-acc_y, acc_z); a_pitch = atan(acc_x/sqrt(acc_y*acc_y + acc_z*acc_z)); // Weigh roll = TRUST_GAIN * a_roll + (1-TRUST_GAIN) * g_roll; pitch = TRUST_GAIN * a_pitch + (1-TRUST_GAIN) * g_pitch; }
// // delay the current execution by us microseconds // using the real time clock running at 115000 Hz. // void widget_delay_rtc(unsigned us) { #define RTC_HZ 115000 // 115kHz Real Time Counter unsigned tick = (unsigned long long)us * RTC_HZ / 1000000; unsigned start = rtc_get_value(&AVR32_RTC); while ( (rtc_get_value(&AVR32_RTC) - start) < tick ); }
int main (void) { // Variables -- Misc const twim_options_t twi_option_GYRO = twi_opt_GYRO; const twim_options_t twi_option_ACC = twi_opt_ACC; const twim_options_t twi_option_MAGN = twi_opt_MAGN; // Variables -- Control system float prev_e_z = 0; float prev_e_roll = 0; float prev_e_pitch = 0; float prev_e_yaw = 0; float eint_z = 0; float eint_roll = 0; float eint_pitch = 0; float eint_yaw = 0; float pitch_ederiv = 0; // Error derivative float roll_ederiv = 0; float prev_deriv_e_roll = 0; float yaw_ederiv = 0; float z_ederiv = 0; float e = 0; float dt; // Time between samples unsigned long t1 = 0; unsigned long t2 = 0; float u1, u2, u3, u4 = 0; float m1, m2, m3, m4 = 0; float g_force = 240; int xcount = 0; float gain = 1; int time_to_update_data = 0; int deriv_count=0; const usart_options_t usart_option = usart_opt; const usart_options_t usart_option_2 = usart_opt_2; // Setting up the board pcl_configure_clocks(&pcl_freq_param); board_init(); // Initialize Bluetooth configure_AT(FPBA_HZ, usart_option); irq_initialize_vectors(); cpu_irq_enable(); Disable_global_interrupt(); // Initialize interrupt vectors. INTC_init_interrupts(); INTC_register_interrupt(&usart_int_handler, USART_IRQ, AVR32_INTC_INT0); USART->ier = AVR32_USART_IER_RXRDY_MASK; // Enable all interrupts. Enable_global_interrupt(); // Enable Ultrasonic sensors enable_Ultrasonic(); // Initialize motors initialize_PWM(); tc_start(tc1, PWM_MOTOR1_CHANNEL); tc_start(tc0, PWM_MOTOR2_CHANNEL); tc_start(tc1, PWM_MOTOR3_CHANNEL); tc_start(tc1, PWM_MOTOR4_CHANNEL); // Waits for button press here after battery is connected while (gpio_get_pin_value(GPIO_PUSH_BUTTON_0)); delay_ms(1000); /* while(1){ while (motor2_speed<60) { motor2_speed += 1; update_Motors(); delay_ms(300); } while (motor2_speed>0) { motor2_speed -= 1; update_Motors(); delay_ms(300); } } while (gpio_get_pin_value(GPIO_PUSH_BUTTON_0));*/ //delay_ms(3000); // Initialize RTC //AVR32_RTC->ctrl & AVR32_RTC_CTRL_BUSY_MASK = 0; rtc_init(&AVR32_RTC, RTC_OSC_32KHZ, 1); rtc_enable(&AVR32_RTC); // Initialize IMU parts initialize_IMU(); // TESTING ONLY float test_array[2000]; float test_array_2[2000]; float test_array_3[2000]; for (int k = 0; k < FILTER_LENGTH; k++){ FIFO_deriv_roll[k] = roll; } time_to_update_data = 0; // Control system code begins while(1) { t1 = rtc_get_value(&AVR32_RTC); // Current time in ms dt = ((float)(t1-t2))*0.001*0.125; // dt in seconds if (t1 < t2) // Timer overflowed { dt = ((float)(t1 + rtc_get_top_value(&AVR32_RTC))*0.001*0.125); } t2 = t1; get_Angles(); //pitch = 0; yaw = 0; e = des_z-z; //calculating height error eint_z = eint_z + ((e+prev_e_z)/2)*dt; //calculate error integral term z_ederiv = (e - prev_e_z)/dt; //calculate error derivative term u1 =(KP_Z*e) + (KI_Z*eint_z) + (KD_Z*z_ederiv)+g_force; //calculating control output prev_e_z=e; //ROLL e = des_roll-roll; //calculating roll error eint_roll = eint_roll + ((e+prev_e_roll)/2)*dt; //calculate error integral term prev_e_roll=e; for (int i = 0; i < (FILTER_LENGTH - 1); i++) { FIFO_deriv_roll[i] = FIFO_deriv_roll[i+1]; } FIFO_deriv_roll[FILTER_LENGTH-1] = e; roll_ederiv = (FIFO_deriv_roll[FILTER_LENGTH-1] - FIFO_deriv_roll[0])/(FILTER_LENGTH*dt); u2 =(KP_ROLL*e) + (KI_ROLL*eint_roll) + (KD_ROLL*roll_ederiv); //calculating control output if(xcount < 2000){ test_array[xcount] = roll_ederiv; test_array_2[xcount] = roll; test_array_3[xcount] = g_roll; } xcount++; //PITCH e = des_pitch-pitch; //calculating pitch error eint_pitch = eint_pitch + ((e+prev_e_pitch)/2)*dt; //calculate error integral term pitch_ederiv = (e - prev_e_pitch)/dt; //calculate error derivative term u3 =(KP_PITCH*e) + (KI_PITCH*eint_pitch) + (KD_PITCH*pitch_ederiv); //calculating control output prev_e_pitch=e; //YAW e = des_yaw-yaw; //calculating yaw error eint_yaw = eint_yaw + ((e+prev_e_yaw)/2)*dt; //calculate error integral term yaw_ederiv = (e - prev_e_yaw)/dt; //calculate error derivative term u4 =(KPYAW*e) + (KIYAW*eint_yaw) + (KDYAW*yaw_ederiv); //calculating control output prev_e_yaw=e; //MOTOR SPEEDS m1=(0.25*u1+0.5*u2+0.25*u4)*gain; m2=(0.25*u1+0.5*u3-0.25*u4)*gain; m3=(0.25*u1-0.5*u2+0.25*u4)*gain; m4=(0.25*u1-0.5*u3-0.25*u4)*gain; if (m1 > 95) m1 = 95; else if (m1 < 5) m1 = 5; if (m2 > 95) { m2 = 95; } else if (m2 < 5) { m2 = 5; } if (m3 > 95) { m3 = 95; } else if (m3 < 5) { m3 = 5; } if (m4 > 95) { m4 = 95; } else if (m4 < 5) { m4 = 5; } motor1_speed = m2; //m2 motor2_speed = m1; //m3 motor3_speed = m4; //m4 motor4_speed = m3; //m1..... the imu was turned 90 degrees.... update_Motors(); //Bluetooth Send if (time_to_update_data > 3) { // Get Ultrasonic data update_Ultrasonic(); // Send out update through Bluetooth meas_roll = roll; meas_pitch = pitch; meas_yaw = yaw; transmitted_data(); time_to_update_data = 0; received_data(); } else { delay_ms(7); } time_to_update_data++; } }