/* The initialize function configures the PLL to set the internal clock
* frequency. It also configures the digital IO and calls the initialization
* functions for each of the modules. A light sequence signals the end of
* initialization.
*/
void initialize(void){
/* Configure Phase Lock Loop for the system clock reference at 40MHz */
// Fosc (Clock frequency) is set at 80MHz
// Fin is 7.37 MHz from internal FRC oscillator
// FPLLI = Fin/N1 = 3.685 MHz
CLKDIVbits.PLLPRE = 0; // N1 = 2
// FVCO = FPLLI*M1 = 162.14MHz
PLLFBDbits.PLLDIV = 42; // M = 44
// FPLLO = FVCO/N2 = 81.07 MHz
// FOSC ~= 80MHz, FCY ~= 40MHz
CLKDIVbits.PLLPOST = 0; // N2 = 2
/* Initiate Clock Switch */
//The __builtin macro handles unlocking the OSCCON register
__builtin_write_OSCCONH(1); //New oscillator is FRC with PLL
__builtin_write_OSCCONL(OSCCON | 0x01); //Enable clock switch
while (OSCCONbits.COSC!= 1); //Wait for FRC with PLL to be clock source
while (OSCCONbits.LOCK!= 1); //Wait for PLL to lock
/* Configure IO*/
TRISDbits.TRISD10 = 1; //USER input
//LED outputs
ANSELBbits.ANSB13 = 0; //Disable Analog on B13
TRISBbits.TRISB13 = 0; //LED1
ANSELBbits.ANSB12 = 0; //Disable Analog on B12
TRISBbits.TRISB12 = 0; //LED2
TRISDbits.TRISD11 = 0; //LED3
TRISDbits.TRISD0 = 0; //LED4
//Magnet Control
TRISBbits.TRISB14 = 0; //Top Magnet
//Store bits indicating reason for reset
resetStat = RCON;
//Clear reset buffer so next reset reading is correct
RCON = 0;
/* Initialize peripherals*/
initialize_PWM();
initialize_CN();
initialize_ADC();
initialize_QEI();
initialize_UART();
initialize_UART2();
//initialize_I2C_Master();
lights();
__delay32(10000000);
//initialize_MPU();
initialize_encoder_values(1600,1700,1800);
}
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++;
}
}
