//-------------------------------------------------------------------- //[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]); }