void Aquarium::loop() {
SerialDebug();
if (display)
display->OnLoop();
// Heater Routine
CheckTemp();
// Light Routine
CheckTime();
int32_t val = myEnc.read();
if (val != encval) {
Serial.println(val - encval);
encval = val;
}
// testcode
/*
switch (counter) {
case 0:
mySwitch.switchOn("10101", "10000");
break;
case 1:
mySwitch.switchOn("10101", "01000");
break;
case 2:
mySwitch.switchOn("10101", "00100");
break;
case 3:
mySwitch.switchOff("10101", "10000");
break;
case 4:
mySwitch.switchOff("10101", "01000");
break;
case 5:
mySwitch.switchOff("10101", "00100");
break;
}
counter ++;
if (counter == 6)
counter = 0;
*/
}
void kbdSetLeds(uint8_t ledstate)
{
// LEDS should be NUM, CAPS, SCROLL, COMPOSE, KANA
// In theory we should be able to get Power, Shift and
// Do Not Disturb, but I don't think the linux input driver
// can handle those.
// The input layer does handle other LEDS with higher usage
// codes, but that would need another report format. TBD.
SerialDebug(1, "kbdSetLeds %02x\r\n", (int)ledstate);
// Assignment for now:
// NUM: D0
// CAPS: D1
// SCROLL: D2
// COMPOSE: F0
// KANA: F1
if (ledstate & 0x01) {
NUMLEDPORT |= _BV(NUMLEDPIN);
} else {
NUMLEDPORT &= ~(_BV(NUMLEDPIN));
}
if (ledstate & 0x02) {
CAPSLEDPORT |= _BV(CAPSLEDPIN);
} else {
CAPSLEDPORT &= ~(_BV(CAPSLEDPIN));
}
if (ledstate & 0x04) {
SCRLLEDPORT |= _BV(SCRLLEDPIN);
} else {
SCRLLEDPORT &= ~(_BV(SCRLLEDPIN));
}
if (ledstate & 0x08) {
COMPLEDPORT |= _BV(COMPLEDPIN);
} else {
COMPLEDPORT &= ~(_BV(COMPLEDPIN));
}
if (ledstate & 0x10) {
KANALEDPORT |= _BV(KANALEDPIN);
} else {
KANALEDPORT &= ~(_BV(KANALEDPIN));
}
}
void kbdSetExtraLeds(uint8_t ledstate)
{
/* These come in in the following mapping, from the Linux
* kernel and our report structure:
* 0x01 - LED_MUTE
* 0x02 - LED_MAIL
* 0x04 - LED_SLEEP
* 0x08 - LED_MISC
* 0x10 - LED_SUSPEND
* 0x20 - LED_CHARGING
* This isn't the order they are defined in <linux/input.h>,
* it is ordered by usage number (since that allowed me to
* coalesce usages 0x4b-0x4d into one report stanza).
*/
SerialDebug(1, "kbdSetExtraLeds %02x\r\n", (int)ledstate);
if (ledstate & 0x04) {
debugLedOn(1);
} else {
debugLedOff(1);
}
}
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);
}
}
