/********************************************************************
This is the primary implementation for the motion sensor. It checks the
gyroscope and accelerometer for changing values. If the values have changed
beyond the starting threshold, it will return ASCII 1.
********************************************************************/
char motion_update(){
float accelXdiff;
float accelYdiff;
float accelZdiff;
float gyroXdiff;
float gyroYdiff;
float gyroZdiff;
//check accelerometer against initial values
Get_Accel_Values();
Get_Accel_Angles();
accelXdiff = ACCEL_XANGLE - ACCEL_XOUT_ARMED;
accelYdiff = ACCEL_YANGLE - ACCEL_YOUT_ARMED;
//TODO: fix
//accelZdiff = ACCEL_ZANGLE - ACCEL_ZANGLE_ARMED;
if((fabsf(accelXdiff) > ACCEL_X_TOLERANCE)|| (fabsf(accelYdiff) > ACCEL_Y_TOLERANCE)){
// || (fabsf(accelZdiff) > ACCEL_Z_TOLERANCE)){
return('1'); //something accelerometery is going on
}
//check gyroscope against initial values
Get_Gyro_Rates();
gyroXdiff = GYRO_XOUT - GYRO_XOUT_ARMED;
gyroYdiff = GYRO_YOUT - GYRO_YOUT_ARMED;
gyroZdiff = GYRO_ZOUT - GYRO_ZOUT_ARMED;
if((fabsf(gyroXdiff) > GYRO_X_TOLERANCE)|| (fabsf(gyroYdiff) > GYRO_Y_TOLERANCE) || (fabsf(gyroZdiff) > GYRO_Z_TOLERANCE)){
return('1'); //something gyroscopey is going on
}
return('0'); //nothing fishy is going on
}
void motion_arm_position(void){
Get_Gyro_Rates();
GYRO_XOUT_ARMED = GYRO_XOUT;
GYRO_YOUT_ARMED = GYRO_YOUT;
GYRO_ZOUT_ARMED = GYRO_ZOUT;
Get_Accel_Values();
Get_Accel_Angles();
ACCEL_XOUT_ARMED = ACCEL_XANGLE;
ACCEL_YOUT_ARMED = ACCEL_YANGLE;
ACCEL_ZOUT_ARMED = ACCEL_ZANGLE;
}
void _ISR _T1Interrupt(void)
{
IFS0bits.T1IF = 0; //Clear interrupt flag
IEC0bits.T1IE = 0; //Disable timer1 interrupt
LATAbits.LATA0 = !LATAbits.LATA0;
Get_Gyro_Rates();
Get_Accel_Values();
Get_Accel_Angles();
//complementary_filter();
second_order_complementary_filter();
update_PID();
update_motors_single_shot();
IEC0bits.T1IE = 1; //Enable timer1 interrupt
}
void Callback_20ms() {
desiredState.key.abs.pos.z = altitudeValue / 1000.0;
/* Setting motor speed based on altitude */
lcd_buffer_print(LCD_LINE4, "In: %1.3f", sonarDistance);
outValue = PID_Compute(sonarDistance, desiredState.key.abs.pos.z, &z_axis_PID);
desiredState.key.avg_motor_us = map(outValue * map(analogRead, 0, 4096, 0, 1), 0, 0.5, MOTOR_MIN_US, MOTOR_MAX_US);
lcd_buffer_print(LCD_LINE5, "Mot: %4.0f", desiredState.key.avg_motor_us);
//lcd_buffer_print(LCD_LINE7, "Diff: %3.0f", desiredState.key.motor_diff_us);
/* Obtain accelerometer an gyro values */
Get_Gyro_Rates(¤tState.key.gyro.vel.x, ¤tState.key.gyro.vel.y, ¤tState.key.gyro.vel.z);
Get_Accel_Angles(¤tState.key.accel.pos.x, ¤tState.key.accel.pos.y);
//Get_Mag_Value_Normalized(¤tState.key.magn.pos.x, ¤tState.key.magn.pos.y, ¤tState.key.magn.pos.z);
Get_Accel_Gravity_Power(¤tState.key.accel.vel.x, ¤tState.key.accel.vel.y, ¤tState.key.accel.vel.z);
get_Angle_AHRS(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.acc.x, ¤tState.key.Kalman.acc.y, ¤tState.key.Kalman.acc.z);
// get_Angle_AHRS_Mahony(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.vel.x, ¤tState.key.Kalman.vel.y, ¤tState.key.Kalman.vel.z);
/* Calcolo Roll e Pitch con il filtro di Kalman */
currentState.key.Kalman.pos.x = Compute_AVG(getAngle(currentState.key.accel.pos.x, currentState.key.gyro.vel.x, dt, rollKalman), &rollAVG);
currentState.key.Kalman.pos.y = Compute_AVG(getAngle(currentState.key.accel.pos.y, currentState.key.gyro.vel.y, dt, pitchKalman), &pitchAVG);
//currentState.key.Kalman.pos.z = currentState.key.Kalman.acc.z;
// Angolo di Yaw mediante giroscopio
// currentState.key.Kalman.pos.z = currentState.key.gyro.vel.z;
// Get_Mag_Heading_Compensated (¤tState.key.Kalman.pos.z, currentState.key.Kalman.pos.x, currentState.key.Kalman.pos.y);
// Get_Mag_Heading(¤tState.key.Kalman.acc.z);
desiredState.key.x_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.x, 0, &Roll_PID), -30, 30, 60, 120);
desiredState.key.y_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.y, 0, &Pitch_PID), -30, 30, 60, 120);
currentState.key.Kalman.pos.z += currentState.key.gyro.vel.z*dt;
Motor_Write_us(MOTOR_UPPER, desiredState.key.avg_motor_us + desiredState.key.motor_diff_us);
Motor_Write_us(MOTOR_BOTTOM, desiredState.key.avg_motor_us - desiredState.key.motor_diff_us);
Servo_Write_deg(SERVO_ROLL, desiredState.key.x_servo_deg);
Servo_Write_deg(SERVO_PITCH, desiredState.key.y_servo_deg); // pitch servo is trimmed 18°
}
int main(void)
{
Setup_I2C();
//SYSTEMConfigPerformance(SYSCLK);
// Peripheral function that configures for best performance given the system's
// clock frequency--this is for general practice.
// This part is analogous to the void setup(){}; in Arduino.
//TRISDCLR = 0x0100; // Set pin RD8 as an output, could be written as TRISD = 0xFEFF;
// but takes more clock cycles to perform.
//TRISESET = 0x0080; // Set pin RE7 (PROG button) as an input, could be written as TRISE = 0x0080;
// but takes more clock cycles to perform.
// This part is analogous to the void loop(){}; in Arduino.
//SDA = SCL = 0;
//SCL_IN = SDA_IN = 0;
Setup_MPU6050();
Calibrate_Gyros();
MPU6050_Test_I2C();
while(1) // Loop forever...
{
// Reads the state of pin RE7, and latches RD8 accordingly.
// Note that RE7 is normally set HIGH (3.3V) through an internal pull-up
// resistor and will actually be set LOW when the PROG button is pressed.
// LATDbits.LATD8 = PORTEbits.RE7;
//Get_Accel_Angles();
//i2c_start();
// i2c_tx(0x68);
//i2c_start(); //ok
//i2c_stop();
//DelayMs(1000);
//MPU6050_Test_I2C();
Get_Accel_Values();
Get_Accel_Angles();
GetTemp();
}
return 0; // Included because main returns an int which is being expected.
} // Make sure the program ends in a new line
