int main(void)
{
My_Init();
Init_Timer();
Init_I2C();
Init_Sensors();
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1);
/////////////////////////////////
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); //enable GPIO port for LED
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2); //enable pin for LED PF2
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
IntMasterEnable(); //enable processor interrupts
IntEnable(INT_UART0); //enable the UART interrupt
UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT); //only enable RX and TX interrupts
/////////////////////////////////
Kalman_Sim_initialize();
while(1)
{
Read_Accelerometer();
Calculate_Acc();
Read_Compass();
Compass_Heading();
Calculate_Compass();
Read_Gyro();
Calculate_Gyro();
fgyro[0] = sen_data.gyro_x;
fgyro[1] = sen_data.gyro_y;
fgyro[2] = sen_data.gyro_z;
facc[0] = sen_data.accel_x;
facc[1] = sen_data.accel_y;
facc[2] = sen_data.accel_z;
fmag[0] = sen_data.magnetom_x;
fmag[1] = sen_data.magnetom_y;
fmag[2] = sen_data.magnetom_z;
Kalman_Sim_step();
data[0]=Out1[0];
data[1]=Out1[1];
data[2]=Out1[2];
Timer_CyRun();
}
}
int Init (void)
{
Init_Oscillator();
Init_I2C();
Init_Interrupts();
Init_USART();
TRISB = 0x00;
//LATB = 0x01; // Turn on a little status LED;
return 1;
}
void Bucle_Principal() {
int salida=0;
int angulo = 0;
int who_a_I_tmp = 0;
//------------------------------------------------------------------------------------------//
double accXangle = (atan2(get_ax(), -get_ay()) * RAD_TO_DEG );
double gyroXrate = (double) (get_gz() ) / 131.0;
if (tipoFiltro==1){
angulo = (signed int) Complementary2(accXangle, gyroXrate,Tsample)-90;
}
else {
angulo = (signed int) kalman(accXangle, gyroXrate,Tsample)-90;
}
if (act_motor==1){
salida = pid(setpoint, angulo,5, KP, KI, KD, 5000, -5000, 10000, -10000);
SetPwm1(BIAS1 - salida);
SetPwm2(BIAS2 + salida);
who_a_I_tmp = get_who_I_AM();
if(who_a_I_tmp != 104)
{
Init_I2C();
reset();
}
enviar_valor_NOCR("ax= ",get_ax());
enviar_valor_NOCR(" ay = ",get_ay());
enviar_valor_NOCR(" gz= ",get_gz());
enviar_valor(" who = ",get_who_I_AM());
}
else {
SetPwm1(0);
SetPwm2(0);
ErrorI=0;
}
plot4(accXangle-90,angulo,salida/10,setpoint);
LED_ROJO=!LED_ROJO;
}
//============================================
// initialize system hardware config
// timer,uart,port
//============================================
void Init_Hardware(void)
{
WDTCTL=WDTPW+WDTHOLD;
BCSCTL1=CALBC1_1MHZ;
DCOCTL=CALDCO_1MHZ;
P3SEL=BIT5|BIT4|BIT2|BIT1
Init_I2C();
// Timer Initialize
TACCTL0=CCIE;
TACCR0=10000;
TACTL=TASSEL_2+MC_1;
// Port Initialize
P3DIR |= 0x01;
P4DIR |= 0x07;
BuzzerOFF;
__bis_SR_register(GIE);
}
void Test_SL030(void)
{
#define RCVCMD g_cRxBuf[1]
#define RCVSTA g_cRxBuf[2]
WAKEUP_H;
Start_Time(5);
while(g_bOverTime==0);
WAKEUP_L;
Start_Time(5);
while(g_bOverTime==0);
WAKEUP_H;
g_cCardType=0xff;
if(CARDIN!=0) return;
Init_I2C();
if(I2C_SelectCard(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x01)||(RCVSTA!=0)) return;
if(g_cRxBuf[g_cRxBuf[0]]==1) g_cCardType=CARDTYPE_S50; //Mifare 1k 4 byte UID
else if(g_cRxBuf[g_cRxBuf[0]]==2) g_cCardType=CARDTYPE_S50; //Mifare 1k 7 byte UID
else if(g_cRxBuf[g_cRxBuf[0]]==3) g_cCardType=CARDTYPE_UL; //Ultralight 7 byte UID
else if(g_cRxBuf[g_cRxBuf[0]]==4) g_cCardType=CARDTYPE_S70; //Mifare 4k 4 byte UID
else if(g_cRxBuf[g_cRxBuf[0]]==5) g_cCardType=CARDTYPE_S70; //Mifare 4k 7 byte UID
else if(g_cRxBuf[g_cRxBuf[0]]==6) g_cCardType=CARDTYPE_DES; //DesFire 7 byte UID
switch(g_cCardType)
{
case CARDTYPE_S50:
case CARDTYPE_S70:
if(I2C_LoginSector0(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x02)||(RCVSTA!=0x02)) return;
if(I2C_WriteBlock1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x04)||(RCVSTA!=0x00)) return;
if(I2C_ReadBlock1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x03)||(RCVSTA!=0x00)) return;
if(memcmp(&IIC_ComWriteBlock1[3],&g_cRxBuf[3],16)!=0) return;
if(I2C_ComIntiPurse1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x06)||(RCVSTA!=0x00)) return;
if(I2C_ComIncrPurse1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x08)||(RCVSTA!=0x00)) return;
if(I2C_ComDecrPurse1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x09)||(RCVSTA!=0x00)) return;
if(I2C_ComReadPurse1(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x05)||(RCVSTA!=0x00)) return;
//Check value
lPurseValue=g_cRxBuf[6];
lPurseValue=(lPurseValue<<8)+g_cRxBuf[5];
lPurseValue=(lPurseValue<<8)+g_cRxBuf[4];
lPurseValue=(lPurseValue<<8)+g_cRxBuf[3];
if(lPurseValue!=0x12345678+0x00000002-0x00000001) return;
if(I2C_ComCopyValue(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x0a)||(RCVSTA!=0x00)) return;
I2C_ComHalt();
BuzzerOn();
break;
case CARDTYPE_UL:
if(I2C_ComWriteUltralightPage5(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x11)||(RCVSTA!=0x00)) return;
if(I2C_ComReadUltralightPage5(g_cRxBuf,sizeof(g_cRxBuf))==0) return;
if((RCVCMD!=0x10)||(RCVSTA!=0x00)) return;
if(memcmp(&IIC_ComWriteUltralightPage5[3],&g_cRxBuf[3],4)!=0) return;
I2C_ComHalt();
BuzzerOn();
break;
default:
break;
}
}
int main()
{
SerialDebug(250000); //PrintString() and PrintFloat() using UART
BeginBasics();
Blink();
Enable_PeriphClock();
/*Roll-0 Pitch-1 Yaw-2..Roll rotation around X axis.Pitch rotation around Y axis and Yaw rotation around Z axis
note: It does not mean rotation along the X Y or Z axis*/
float RPY_c[3],RPY_k[3]; //RPY_c and RPY_k..Roll pitch and yaw obtained from complemntary filter and kalman filter
float Accel[3],Gyro[3],Tempreature; //raw values
float Accel_RealWorld[3]; //Real World Acceleration
while(Init_I2C(400)){PrintString("\nI2C Connection Error");}
while(MPU6050_Init()){PrintString("MPU6050 Initialization Error");}
MPU6050_UpdateOffsets(&MPU6050_Offsets[0]);
//MPU6050_ConfirmOffsets(&MPU6050_Offsets[0]);
while(0) //to play around with quaternions and vector rotation
{
float vector[3]= {1,0,0};
float axis_vector[3]= {0,1,0}; //rotate around Y axis
float rot_angle=90; //with 90 degrees
Quaternion q;
q=RotateVectorY(vector,rot_angle); //Rotates vector around Y axis with rot_angle
PrintString("\nRotated Vector's Quaternion\t");
DisplayQ(q);
PrintString("\n");
}
while(1)
{
MPU6050_GetRaw(&Accel[0],&Gyro[0],&Tempreature); //Reads MPU6050 Raw Data Buffer..i.e Accel Gyro and Tempreature values
if(Gyro[2]<0.3 && Gyro[2]>-0.3) Gyro[2]=0; //this actually reduces Yaw drift..will add magnetometer soon
spudnut=tics(); //tics() return current timing info..using SysTick running at CPU_Core_Frequency/8..Counter Runs from 0xFFFFFF to 0 therefore overflows every 1.864135 secs
delt=spudnut-donut; //small time dt
donut=spudnut;
//Display_Raw(Accel,Gyro,Tempreature);
Attitude_k(Accel,Gyro,RPY_k,delt); //Estimates YPR using Kalman
PrintString("\nYPR\t");
PrintFloat(RPY_k[2]);
PrintString("\t");
PrintFloat(RPY_k[1]);
PrintString("\t");
PrintFloat(RPY_k[0]);
RemoveGravity(RPY_k,Accel,Accel_RealWorld);
PrintString("\tReal World Accel with Gravity\t"); //still glitchish..working on it
DisplayVector(Accel_RealWorld);
PrintString("\t");
PrintFloat(1/delt);
}
}
int main(void) {
//***************************************************************************************//
//*****************ARRANCAMOS LA CONFIGURACION DEL PIC **********************************//
//***************************************************************************************//
Init_Hw();
Delay1msT1(0); //Configuramos el puertos y quitamos analogicos
Init_Pll();
Delay1msT1(0); //configuramos el pll que abiasmos arracamos sin él
Init_PWM();
Delay1msT1(0); //configuramos el PWM
init_interr_extern();
Delay1msT1(0); //configuramos las interrupciones externas
Init_Bluetooh();
Delay1msT1(0); //Configuramos el serial-Bluetooth
cargar_datos_ajuste();
Init_I2C();
Delay1msT1(0); //incializamos el I2c
set_inicial();
Delay1msT1(0); //Configuramos la incialicacion de sensor
getAngle_init();
Delay1msT1(0); //Incializamos el filtro kalman
set_inicial();
Delay1msT1(0); //Incializamos el acelerometro
LED_ALL_ON();
for (i = 0; i < 2500; i++) Delay_Nop(2000);
LED_ALL_OFF();
//***************************************************************************************//
//***************************************************************************************//
//*****************CALIBRAMOS EL ACELEROMETRO Y GIROSCOPO *******************************//
// get_calibrado_acelerometro(5, 50); //cojemos los valres por defecto del giroscopo y aceletometro
LED_ALL_OFF();
//***************************************************************************************//
//*****************INICIALIZAMOS EL PID Y LAS VARIABLES ********************************//
// pid_dsp_configuracion();
//***************************************************************************************//
LED_ALL_OFF();
//*****************ARRANCAMOS INTERRUPCION DEL BUCLE PRINCIPAL *************************//
SetupT3ForXmsPID(Tsample); //configuramos la interrupcion principal
SetupT4For20msPID(); //configuramos la interrupcion principal
StartInterrup4(); //incializamos la interrupcion
StartInterrup3(); //incializamos la interrupcion
enviar_mensaje("------------------------------------------------------");
//***************************************************************************************//
while (1) {
}
}
