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++;
}
}
