//--------------------------------------------------------------------
//[calcBalOneLeg]
void PhoenixCore::calcBalOneLeg (u8 leg, long posX, long posZ, long posY)
{
long CPR_X; //Final X value for centerpoint of rotation
long CPR_Y; //Final Y value for centerpoint of rotation
long CPR_Z; //Final Z value for centerpoint of rotation
long lAtan;
//Calculating totals from center of the body to the feet
CPR_Z = (s16)pgm_read_word(&TBL_OFFSET_Z[leg]) + posZ;
CPR_X = (s16)pgm_read_word(&TBL_OFFSET_X[leg]) + posX;
CPR_Y = 150 + posY; // using the value 150 to lower the centerpoint of rotation 'mPtrCtrlState->c3dBodyPos.y +
mTotalTransY += (long)posY;
mTotalTransZ += (long)CPR_Z;
mTotalTransX += (long)CPR_X;
lAtan = arctan2(CPR_X, CPR_Z, NULL);
mTotalYBal1 += (lAtan * 1800) / 31415;
lAtan = arctan2 (CPR_X, CPR_Y, NULL);
mTotalZBal1 += ((lAtan * 1800) / 31415) -900; //Rotate balance circle 90 deg
lAtan = arctan2 (CPR_Z, CPR_Y, NULL);
mTotalXBal1 += ((lAtan * 1800) / 31415) - 900; //Rotate balance circle 90 deg
}
static float hue(float r, float g, float b) {
float x = r - (g+b) / 2;
float y = ((g-b) * (float) sqrt(3.0) / 2);
float hue = arctan2(y, x);
if (hue < 0)
hue += 2 * M_PI;
return hue;
}
static int hue(int r, int g, int b) {
int x = r - (g+b) / 2;
int y = (sqrt3d2 * (g-b)) / sMath_scale;
int hue = arctan2(y, x);
if (hue < 0)
hue += 2 * sMath_PI;
return hue;
}
void complex_power_complex(float* d, float* s)
{
/*
=(R*exp(i(θ+2kπ))) ^ (a+ib)
=(exp(i(θ+2kπ)+lnR)) ^ (a+ib)
=exp((i(θ+2kπ)+lnR) * (a+ib))
=exp(alnR-(θ+2kπ)*b)*exp(i((θ+2kπ)*a+blnR))
=exp(alnR-(θ+2kπ)*b)*(cos((θ+2kπ)*a+blnR)+i*sin((θ+2kπ)*a+blnR))
*/
float lnr = 0.5 * ln(d[0]*d[0] + d[1]*d[1]);
float ang = arctan2(d[1], d[0]);
float j = power(2.71828, s[0]*lnr - s[1]*ang);
float k = s[0]*ang + s[1]*lnr;
d[0] = j * cosine(k);
d[1] = j * sine(k);
}
void IQToFreqAmp(int I,int Q,double *Frequency,int *Amp,int SampleRate)
{
double phase;
static double prev_phase = 0;
*Amp=round(sqrt( I*I + Q*Q)/sqrt(2));
//*Amp=*Amp*3; // Amp*5 pour la voix !! To be tested more
if(*Amp>32767)
{
printf("!");
*Amp=32767; //Overload
}
phase = M_PI + ((float)arctan2(I,Q)) * M_PI/180.0f;
double dp = phase - prev_phase;
if(dp < 0) dp = dp + 2*M_PI;
*Frequency = dp*SampleRate/(2.0f*M_PI);
prev_phase = phase;
//printf("I=%d Q=%d Amp=%d Freq=%d\n",I,Q,*Amp,*Frequency);
}
//--------------------------------------------------------------------
//[LEG INVERSE KINEMATICS] Calculates the angles of the coxa, femur and tibia for the given position of the feet
//IKFeetPosX - Input position of the Feet X
//IKFeetPosY - Input position of the Feet Y
//IKFeetPosZ - Input Position of the Feet Z
//IKSolution - Output TRUE if the solution is possible
//IKSolutionWarning - Output TRUE if the solution is NEARLY possible
//IKSolutionError - Output TRUE if the solution is NOT possible
//mFemurAngles - Output Angle of Femur in degrees
//mTibiaAngles - Output Angle of Tibia in degrees
//mCoxaAngles - Output Angle of Coxa in degrees
//--------------------------------------------------------------------
u8 PhoenixCore::getLegIK(u8 leg, s16 IKFeetPosX, s16 IKFeetPosY, s16 IKFeetPosZ)
{
u32 IKSW2; //Length between Shoulder and Wrist, decimals = 2
u32 IKA14; //Angle of the line S>W with respect to the ground in radians, decimals = 4
u32 IKA24; //Angle of the line S>W with respect to the femur in radians, decimals = 4
s16 IKFeetPosXZ; //Diagonal direction from Input X and Z
#if (CONFIG_DOF_PER_LEG == 4)
// these were shorts...
long TarsOffsetXZ; //Vector value \ ;
long TarsOffsetY; //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors
long TarsToGroundAngle1; //Angle between tars and ground. Note: the angle are 0 when the tars are perpendicular to the ground
long TGA_A_H4;
long TGA_B_H3;
#else
#define TarsOffsetXZ 0 // Vector value
#define TarsOffsetY 0 //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors
#endif
long Temp1;
long Temp2;
long T3;
long hyp2XY;
u8 ret;
#if (CONFIG_DOF_PER_LEG == 4)
s16 sin4;
s16 cos4;
#endif
//Calculate IKCoxaAngle and IKFeetPosXZ
s16 atan4 = arctan2 (IKFeetPosX, IKFeetPosZ, &hyp2XY);
mCoxaAngles[leg] = (((long)atan4*180) / 3141) + (s16)pgm_read_word(&TBL_COXA_ANGLE[leg]);
//Length between the Coxa and tars [foot]
IKFeetPosXZ = hyp2XY / DEC_EXP_2;
#if (CONFIG_DOF_PER_LEG == 4)
// Some legs may have the 4th DOF and some may not, so handle this here...
//Calc the TarsToGroundAngle1:
if ((u8)pgm_read_byte(&TBL_TARS_LENGTH[leg])) { // We allow mix of 3 and 4 DOF legs...
TarsToGroundAngle1 = -cTarsConst + cTarsMulti*IKFeetPosY + ((long)(IKFeetPosXZ*cTarsFactorA))/DEC_EXP_1 - ((long)(IKFeetPosXZ*IKFeetPosY)/(cTarsFactorB));
if (IKFeetPosY < 0) //Always compensate TarsToGroundAngle1 when IKFeetPosY it goes below zero
TarsToGroundAngle1 = TarsToGroundAngle1 - ((long)(IKFeetPosY*cTarsFactorC)/DEC_EXP_1); //TGA base, overall rule
if (TarsToGroundAngle1 > 400)
TGA_B_H3 = 200 + (TarsToGroundAngle1/2);
else
TGA_B_H3 = TarsToGroundAngle1;
if (TarsToGroundAngle1 > 300)
TGA_A_H4 = 240 + (TarsToGroundAngle1/5);
else
TGA_A_H4 = TarsToGroundAngle1;
if (IKFeetPosY > 0) //Only compensate the TarsToGroundAngle1 when it exceed 30 deg (A, H4 PEP note)
TarsToGroundAngle1 = TGA_A_H4;
else if (((IKFeetPosY <= 0) & (IKFeetPosY > -10))) // linear transition between case H3 and H4 (from PEP: H4-K5*(H3-H4))
TarsToGroundAngle1 = (TGA_A_H4 -(((long)IKFeetPosY*(TGA_B_H3-TGA_A_H4))/DEC_EXP_1));
else //IKFeetPosY <= -10, Only compensate TGA1 when it exceed 40 deg
TarsToGroundAngle1 = TGA_B_H3;
//Calc Tars Offsets:
sincos(TarsToGroundAngle1, &sin4, &cos4);
TarsOffsetXZ = ((long)sin4*(u8)pgm_read_byte(&TBL_TARS_LENGTH[leg]))/DEC_EXP_4;
TarsOffsetY = ((long)cos4*(u8)pgm_read_byte(&TBL_TARS_LENGTH[leg]))/DEC_EXP_4;
} else {
TarsOffsetXZ = 0;
TarsOffsetY = 0;
}
#endif
//Using GetAtan2 for solving IKA1 and IKSW
//IKA14 - Angle between SW line and the ground in radians
IKA14 = arctan2(IKFeetPosY-TarsOffsetY, IKFeetPosXZ-(u8)pgm_read_byte(&TBL_COXA_LENGTH[leg])-TarsOffsetXZ, &hyp2XY);
//IKSW2 - Length between femur axis and tars
IKSW2 = hyp2XY;
//IKA2 - Angle of the line S>W with respect to the femur in radians
Temp1 = (( ((long)(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])) -
((long)(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])*(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])) )*DEC_EXP_4 + ((long)IKSW2*IKSW2));
Temp2 = (long)(2*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg]))*DEC_EXP_2 * (u32)IKSW2;
T3 = Temp1 / (Temp2/DEC_EXP_4);
IKA24 = arccos (T3 );
//IKFemurAngle
if (mBoolUpsideDown)
mFemurAngles[leg] = (long)(IKA14 + IKA24) * 180 / 3141 - 900 + OFFSET_FEMUR_HORN(leg);//Inverted, up side down
else
mFemurAngles[leg] = -(long)(IKA14 + IKA24) * 180 / 3141 + 900 + OFFSET_FEMUR_HORN(leg);//Normal
//IKTibiaAngle
Temp1 = ((((long)(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])) +
((long)(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])*(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])))*DEC_EXP_4 - ((long)IKSW2*IKSW2));
Temp2 = (2*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])*(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg]));
long angleRad4 = arccos(Temp1 / Temp2);
#ifdef OPT_WALK_UPSIDE_DOWN
if (mBoolUpsideDown)
mTibiaAngles[leg] = (1800-(long)angleRad4*180/3141);//Full range tibia, wrong side (up side down)
else
mTibiaAngles[leg] = -(1800-(long)angleRad4*180/3141);//Full range tibia, right side (up side up)
#else
#ifdef PHANTOMX_V2 // BugBug:: cleaner way?
mTibiaAngles[leg] = -(1450-(long)angleRad4*180/3141); //!!!!!!!!!!!!145 instead of 1800
#else
mTibiaAngles[leg] = -(900-(long)angleRad4*180/3141);
#endif
#endif
#if (CONFIG_DOF_PER_LEG == 4)
//Tars angle
if ((u8)pgm_read_byte(&TBL_TARS_LENGTH[leg])) { // We allow mix of 3 and 4 DOF legs...
mTarsAngles[leg] = (TarsToGroundAngle1 + mFemurAngles[leg] - mTibiaAngles[leg]) +
OFFSET_TARS_HORN(leg);
}
#endif
//Set the Solution quality
if(IKSW2 < ((word)((u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])+(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])-30)*DEC_EXP_2))
ret = STATUS_OK;
else {
if(IKSW2 < ((word)((u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])+(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg]))*DEC_EXP_2))
ret = STATUS_WARNING;
else
ret = STATUS_ERROR;
}
return ret;
}
int main(void){
int integration[3] = {0,0,0};
char lostsignalcnt = 0;
int pry[] = {0,0,0};
int paceCounter = 0;
int pidValues13[3] = {6,20,24};
int pidValuesDen13[3] = {16,1,1};
int pidValues24[3] = {6,20,24};
int pidValuesDen24[3] = {16,1,1};
char pidRotUp[3] = {0,0,20};
char pidRotDenUp[3] = {42,1,1};
char pidRotDown[3] = {9,0,20};
char pidRotDenDown[3] = {42,1,1};
char pidRot[] = {5,0,20};
int throttledif = 0;
int throttleavr = 0;
/*counting var, for for loops*/
int i;
/*Start memory location for Accel and Gyro reads, should be moved
to gyro and accel read functions*/
uint8_t accelstartbyte = 0x30;
uint8_t gyrostartbyte = 0x1A;
/*Joystick Axis buffer
[0] - X axis tilt
[1] - Y axis tilt
[2] - Throttle
[3] - Rotation about Z axis
*/
int joyaxis[] = {0,0,0,0,0};
char joyin[] = {0,0,0,0,0};
int joytrim[] = {0,0,0,0,0};
int joydif[] = {0,0};
int joyavr[] = {0,0};
int motorSpeeds[4];
/*Var to allow increase in motor speed nonrelative to the throttle
during flight*/
int motorup = 0;
/*Vars for new input raw data (cache) and filtered data (int) from
imu*/
int gyrocache[3] = {0,0,0};
int accelcache[3] = {0,0,0};
int magcache[3] = {0,0,0};
int magfacing = 0;
int roterr = 0;
int target[] = {0,0,0};
int accelint[] = {0, 0, 0};
int gyroint[] = {0, 0, 0};
int gyrocounter[] = {0,0,0};
/*Standard values for accel and gyro (when level), set during offset*/
int accelnorm[3] = {28,-20,468};
char gyronorm[3] = {16,42,0};
/*Buffer for sending data through the xbee*/
char xbeebuffer[100];
CLK.CTRL = 0b00000011;
CLK.PSCTRL = 0b00010100;
/*Initialize PORTD to output on pins 0-3 from Timer counter pwm at
50Hz*/
PORTD.DIR = 0x2F;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCCEN_bm | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCDEN_bm;
TCD0.PER = 8000;
/*Initialize Timer counter C0 for pacing,RATE Hz*/
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 2000000 / RATE;
/*Set on board LED pins to output*/
PORTF.DIR = 0x03;
/*Set PORTC to power IMU, PIN 3 3.3V, pin 2 ground*/
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
/*Enable global interrupts of all priority levels, should be made
more relevant*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/*Set pwm duty cycle to stop motors, stop them from beeping
annoyingly*/
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
/*Set Xbee Uart transmit pin 3 to output*/
PORTE.DIR = 0x08;
/*Initialize USARTE0 as the module used by the Xbee*/
uartInitiate(&xbee, &USARTE0);
/*Initialize USARTE1 as the module used by atmega328p */
uartInitiate(&atmega328p, &USARTE1);
/*Initialize imu to use Two wire interface on portC*/
twiInitiate(&imu, &TWIC);
itg3200Init(&imu, RATE);
adxl345Init(&imu);
lsm303dlhInit(&imu);
/*Send string to indicate startup, python doesn't like return carriage
(/r) character*/
sprintf(xbeebuffer, "starting\n");
sendstring(&xbee, xbeebuffer);
/*Start of flight control state machine loop*/
while(1){
/*Check for new packet from atmega328p*/
if(IRDataAvailable)
{
sprintf(xbeebuffer, "Altitude: %d\n", (irdata[1]*256) + irdata[2]);
sendstring(&xbee, xbeebuffer);
}
/*Check for new packet from xbee each time*/
if(readdata){
readdata = 0;
lostsignalcnt = 0;
/*For Joystick packet reads*/
joytrim[2] = 0;
for(i = 0; i < 5; i++){
joyin[i] = -input[3 + i] + 126;
joyaxis[i] = joyin[i];
joyaxis[i] += joytrim[i];
}
throttleavr = ((throttleavr) + (joyaxis[2]))/2;
throttledif = joyaxis[2] - throttleavr;
joyaxis[2] += throttledif * THROTTLEJOYDIF;
for(i = 0; i < 2; i++){
joyavr[i] = (joyavr[i] + joyaxis[i])/2;
joydif[i] = joyaxis[i] - joyavr[i];
}
joyaxis[1] += joydif[1] * PRJOYDIF13;
joyaxis[0] += joydif[0] * PRJOYDIF24;
/*
yawavr = ((yawavr) + joyaxis[3])/2;
yawdif = joyaxis[3] - yawavr;
joyaxis[3] += yawdif * YAWJOYDIF;
*/
//Input 7 is the button buffer
if(input[8] == 4){
state = stopped;
//sprintf(xbeebuffer, "stopped %d\n", input[7]);
sprintf(xbeebuffer, "%4d %4d %4d %4d\n", joyaxis[0], joyaxis[1], joyaxis[2], joyaxis[3]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 0){
joytrim[0] += joyin[0];
joytrim[1] += joyin[1];
joytrim[3] += joyin[3];
}
else if(input[8] == 1){
state = running;
sprintf(xbeebuffer, "running %d\n", input[7]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 10){
state = offset;
}
else if(input[8] == 5){
//motorup += 5;
pidRot[2] ++;
sprintf(xbeebuffer, "D up %d\n", pidRot[2]);
//pidRot[2] ++;
//sprintf(xbeebuffer, "D up %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 6){
pidRot[2] --;
sprintf(xbeebuffer, "D down %d\n", pidRot[2]);
//pidRot[2] --;
//sprintf(xbeebuffer, "D down %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
//motorup -= 5;
}
else if(input[8] == 7){
pidRot[0] ++;
sprintf(xbeebuffer, "P up %d\n", pidRot[0]);
//pidRot[0] ++;
//sprintf(xbeebuffer, "P up %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 8){
pidRot[0] --;
sprintf(xbeebuffer, "P down %d\n", pidRot[0]);
//pidRot[0] --;
//sprintf(xbeebuffer, "P down %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
/*
getmag(magcache, &imu);
sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sendstring(&xbee, xbeebuffer);
*/
}
else if(input[8] == 2){
sprintf(xbeebuffer, "descending\n");
sendstring(&xbee, xbeebuffer);
motorup = -50;
}
xbeecounter = 0;
for(i=0;i<3;i++){
pidRotUp[i] = pidRot[i] * 3/4;
pidRotDown[i] = pidRot[i] * 1;
}
if(state == running){
//sprintf(xbeebuffer, "%d %d\n", joyaxis[2], throttledif);
//sprintf(xbeebuffer, "%d %d %d \n", joyaxis[0], joyaxis[1], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", pry[0], pry[1], pry[2]);
//sprintf(xbeebuffer, "%3d %3d\n", gyroint[2], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n", motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", accelint[0], accelint[1], accelint[2]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sprintf(xbeebuffer, "%4d\n", roterr);
sendstring(&xbee, xbeebuffer);
}
}
switch(state){
/*Stopped state keeps motors stopped but not beeping*/
case stopped:
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
break;
/*Offset gets standard value for gyro's and accel's*/
case offset:
getgyro(gyrocache, &imu, &gyrostartbyte);
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
target[2] = arctan2(magcache[0], magcache[1]);
for(i = 0; i < 3; i ++){
gyronorm[i] = gyrocache[i];
//accelnorm[i] = accelcache[i];
accelcache[i] = 0;
gyrocache[i] = 0;
}
sprintf(xbeebuffer, "offset %d %d %d %d %d %d\n", gyronorm[0], gyronorm[1], gyronorm[2], accelnorm[0], accelnorm[1], accelnorm[2]);
sendstring(&xbee, xbeebuffer);
state = stopped;
break;
case running:
/*Ensure loop doesn't go faster than 50Hz*/
while(!(TCC0.INTFLAGS & 0x01));
TCC0.INTFLAGS = 0x01;
/*Get gyro data
substract stationary offset
filter for stability
*/
getgyro(gyrocache, &imu, &gyrostartbyte);
for(i = 0; i < 3; i ++){
gyrocache[i] -= gyronorm[i];
if((gyrocache[i] <= 1) && (gyrocache[i] >= -1)){
gyrocache[i] = 0;
}
gyrocounter[i] += gyrocache[i];
}
for(i = 0; i < 3; i += 2){
gyroint[i] = gyrocounter[i]/DEGREE;
gyrocounter[i] %= DEGREE;
pry[i] += gyroint[i];
if(pry[i] > PI){
pry[i] = -PI + (pry[i] - PI);
}
else if(pry[i] < -PI){
pry[i] = PI + (pry[i] + PI);
}
}
gyroint[1] = -(gyrocounter[1]/DEGREE);
gyrocounter[1] %= DEGREE;
pry[1] += gyroint[1];
paceCounter ++;
//Slower Operations at 50Hz
if(paceCounter == (RATE / 20)){
paceCounter = 0;
lostsignalcnt ++;
sendbyte(&atmega328p, 'r'); //ask for IR data
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
magfacing = arctan2(magcache[0], magcache[1]);
if((4900 - abs(pry[2]) - abs(target[2])) < abs(pry[2] - target[2])){
if(target > 0){
roterr = 4900 - abs(target[2]) - abs(pry[2]);
}
else{
roterr = -(4900 - abs(target[2]) - abs(pry[2]));
}
}
else{
roterr = target[2] - pry[2];
}
for(i = 0; i < 3; i ++){
accelcache[i] -= accelnorm[i];
/*
if(accelcache[i] > (accelint[i] + 40)){
accelcache[i] = accelint[i] + 40;
}
else if(accelcache[i] < (accelint[i] - 40)){
accelcache[i] = accelint[i] - 40;
}
*/
}
accelint[0] = ((ACCELINT * accelint[0]) + ((24 - ACCELINT) * accelcache[0]))/24;
accelint[1] = ((ACCELINT * accelint[1]) + ((24 - ACCELINT) * accelcache[1]))/24;
if(accelint[1] > (pry[0] + 15)){
accelint[1] = pry[0] + 15;
}
else if(accelint[1] < (pry[0] - 15)){
accelint[1] = pry[0] - 15;
}
if(accelint[0] > (pry[1] + 15)){
accelint[0] = pry[1] + 15;
}
else if(accelint[0] < (pry[1] - 15)){
accelint[0] = pry[1] - 15;
}
pry[0] = ((AWEIGHT * accelint[1]) + (GWEIGHT * pry[0])) / (AWEIGHT + GWEIGHT);
pry[1] = ((AWEIGHT * accelint[0]) + (GWEIGHT * pry[1])) / (AWEIGHT + GWEIGHT);
/*reset cache values to 0, should be made unnecessary by modding gyro and
accel read functions*/
for(i = 0; i < 3; i ++){
accelcache[i] = 0;
}
}
if(gyroint[0] > 6){
gyroint[0] = 6;
}
else if(gyroint[0] < -6){
gyroint[0] = -6;
}
if(gyroint[1] > 6){
gyroint[1] = 6;
}
else if(gyroint[1] < -6){
gyroint[1] = -6;
}
motorSpeed(pry, integration ,gyroint, joyaxis, motorSpeeds, pidValues13, pidValues24, pidValuesDen13, pidValuesDen24);
yawCorrect(motorSpeeds, gyroint, &roterr,pidRotUp,pidRotDenUp,pidRotDown,pidRotDenDown);
if(lostsignalcnt > 10){
for(i = 0; i < 4; i ++){
motorSpeeds[i] -= 50;
}
}
while(!((TCD0.CNT > 5000) || (TCD0.CNT < 2500)));
TCD0.CCA = motorSpeeds[0] + motorup;// - motordif13;
TCD0.CCC = motorSpeeds[2] + motorup;// + motordif13;
TCD0.CCB = motorSpeeds[1] + motorup;// + motordif24;
TCD0.CCD = motorSpeeds[3] + motorup;// - motordif24;
PORTD.OUT ^= 0b00100000;
}
}
}
float complex_angle(float* d)
{
return arctan2(d[1], d[0]);
}
