예제 #1
0
err_t adxl345GetXYZ(int16_t *x, int16_t *y, int16_t *z)
{
  int32_t i2cState;

  if (!_adxl345Initialised)
  {
    ASSERT_STATUS(adxl345Init());
  }

  I2CWriteLength = 2;
  I2CReadLength = 6;
  I2CMasterBuffer[0] = ADXL345_ADDRESS;
  I2CMasterBuffer[1] = ADXL345_REG_DATAX0;
  I2CMasterBuffer[2] = ADXL345_ADDRESS | ADXL345_READBIT;
  i2cState = i2cEngine();

  /* Check if we got an ACK or TIMEOUT error */
  ASSERT_I2C_STATUS(i2cState);

  /* Shift values to create properly formed integer */
  *x = (I2CSlaveBuffer[1] << 8) | (I2CSlaveBuffer[0]);
  *y = (I2CSlaveBuffer[3] << 8) | (I2CSlaveBuffer[2]);
  *z = (I2CSlaveBuffer[5] << 8) | (I2CSlaveBuffer[4]);

  return ERROR_NONE;
}
예제 #2
0
int main(int argc, char** argv) {

    /*Configuring POSC with PLL, with goal FOSC = 80 MHZ */
    // Configure PLL prescaler, PLL postscaler, PLL divisor
    // Fin = 8 Mhz, 8  * (40/2/2) = 80
    PLLFBD = 18; // M=40          // change to 38 for POSC 80 Mhz - this worked only on a single MCU for uknown reason
    CLKDIVbits.PLLPOST = 0; // N2=2
    CLKDIVbits.PLLPRE = 0; // N1=2

    // Initiate Clock Switch to Primary Oscillator with PLL (NOSC=0b011)
    //__builtin_write_OSCCONH(0x03);

    // tune FRC
    OSCTUN = 23;  // 23 * 0.375 = 8.625 % -> 7.37 Mhz * 1.08625 = 8.005Mhz
    // Initiate Clock Switch to external oscillator NOSC=0b011 (alternative use FRC with PLL (NOSC=0b01)
    __builtin_write_OSCCONH(0b011);
    __builtin_write_OSCCONL(OSCCON | 0x01);

    // Wait for Clock switch to occur
    while (OSCCONbits.COSC!= 0b011);
    // Wait for PLL to lock
    while (OSCCONbits.LOCK!= 1);

     // local variables in main function
    int status = 0;
    int i = 0;
    int ax = 0, ay = 0, az = 0;
    int statusProxi[8];
    int slowLoopControl = 0;
    UINT16 timerVal = 0;
    float timeElapsed = 0.0;
    //extern UINT8 pwmMotor;
    extern UINT16 speakerAmp_ref;
    extern UINT16 speakerFreq_ref;
    extern UINT8 proxyStandby;
    UINT16 dummy = 0x0000;

    setUpPorts();
    delay_t1(50);

    PWMInit();
    delay_t1(50);

    ctlPeltier = 0;
    PeltierVoltageSet(ctlPeltier);
    FanCooler(0);
    diagLED_r[0] = 100;
    diagLED_r[1] = 0;
    diagLED_r[2] = 0;
    LedUser(diagLED_r[0], diagLED_r[1],diagLED_r[2]);

    // Speaker initialization - set to 0,1
    spi1Init(2, 0);
    speakerAmp_ref = 0;
    speakerAmp_ref_old = 10;
    speakerFreq_ref = 1;
    speakerFreq_ref_old = 10;
    int count = 0;
    UINT16 inBuff[2] = {0};
    UINT16 outBuff[2] = {0};

    while (speakerAmp_ref != speakerAmp_ref_old) {
        if (count > 5 ) {
            // Error !
            //LedUser(100, 0, 0);
            break;
        }

        inBuff[0] = (speakerAmp_ref & 0x0FFF) | 0x1000;

        chipSelect(slaveVib);
        status = spi1TransferWord(inBuff[0], outBuff);
        chipDeselect(slaveVib);

        chipSelect(slaveVib);
        status = spi1TransferWord(inBuff[0], &speakerAmp_ref_old);
        chipDeselect(slaveVib);

        count++;
    }

    count = 0;

    while (speakerFreq_ref != speakerFreq_ref_old) {
        if (count > 5 ) {
            // Error !
            //LedUser(0, 100, 0);
            break;
        }

        inBuff[0] = (speakerFreq_ref & 0x0FFF) | 0x2000;

        chipSelect(slaveVib);
        status = spi1TransferWord(inBuff[0], outBuff);
        chipDeselect(slaveVib);

        chipSelect(slaveVib);
        status = spi1TransferWord(inBuff[0], &speakerFreq_ref_old);
        chipDeselect(slaveVib);

        count++;
    }

    accPin = aSlaveR;
    accPeriod = 1.0 / ACC_RATE * 1000000.0;  // in us; for ACC_RATE = 3200 Hz it should equal 312.5 us
    status = adxl345Init(accPin);
    ax = status;
    delay_t1(5);

    /* Init FFT coefficients */
    TwidFactorInit(LOG2_FFT_BUFF, &Twiddles_array[0],0);
    delta_freq = (float)ACC_RATE / FFT_BUFF;

    // read 100 values to calculate bias
    int m;
    int n = 0;
    for (m = 0; m < 100; m++) {

        status = readAccXYZ(accPin, &ax, &ay, &az);
        if (status <= 0) {
            //
        }
        else {
            ax_b_l += ax;
            ay_b_l += ay;
            az_b_l += az;
            n++;
        }
        delay_t1(1);
    }

    ax_b_l /= n;
    ay_b_l /= n;
    az_b_l /= n;

    _SI2C2IE = 0;
    _SI2C2IF = 0;

    // Proximity sensors initalization
    I2C1MasterInit();
    status = VCNL4000Init();

    // Cooler temperature sensors initalization
    status = adt7420Init(0, ADT74_I2C_ADD_mainBoard);
    delay_t1(1);
    muxCh = I2C1ChSelect(1, 6);
    status = adt7420Init(0, ADT74_I2C_ADD_flexPCB);

    // Temperature sensors initialization
    statusTemp[0] = adt7320Init(tSlaveF, ADT_CONT_MODE | ADT_16_BIT);
    delay_t1(5);
    statusTemp[1] = adt7320Init(tSlaveR, ADT_CONT_MODE | ADT_16_BIT);
    delay_t1(5);
    statusTemp[2] = adt7320Init(tSlaveB, ADT_CONT_MODE | ADT_16_BIT);
    delay_t1(5);
    statusTemp[3] = adt7320Init(tSlaveL, ADT_CONT_MODE | ADT_16_BIT);
    delay_t1(5);

    // Temperature estimation initialization
    for (i = 0; i < 50; i++) {
        adt7320ReadTemp(tSlaveF, &temp_f);
        delay_t1(1);
        adt7320ReadTemp(tSlaveL, &temp_l);
        delay_t1(1);
        adt7320ReadTemp(tSlaveB, &temp_b);
        delay_t1(1);
        adt7320ReadTemp(tSlaveR, &temp_r);
        delay_t1(1);
    }

    tempBridge[0] = temp_f;
    tempBridge[1] = temp_r;
    tempBridge[2] = temp_b;
    tempBridge[3] = temp_l;

    if (statusTemp[0] != 1)
        temp_f = -1;
    if (statusTemp[1] != 1)
        temp_r = -1;
    if (statusTemp[2] != 1)
        temp_b = -1;
    if (statusTemp[3] != 1)
        temp_l = -1;

    // CASU ring average temperature
    temp_casu = 0;
    tempNum = 0;
    tempSensors = 0;

    for (i = 0; i < 4; i++) {
        if (statusTemp[i] == 1 && tempBridge[i] > 20 && tempBridge[i] < 60) {
            tempNum++;
            temp_casu += tempBridge[i];
            tempSensors++;
        }
    }

    if (tempNum > 0)
        temp_casu /= tempNum;
    else
        temp_casu = -1;

    temp_casu1 = temp_casu;
    temp_wax = temp_casu;
    temp_wax1 = temp_casu;
    temp_model = temp_wax;

    temp_old[0] = temp_f;
    temp_old[1] = temp_r;
    temp_old[2] = temp_b;
    temp_old[3] = temp_l;
    temp_old[4] = temp_flexPCB;
    temp_old[5] = temp_pcb;
    temp_old[6] = temp_casu;
    temp_old[7] = temp_wax;

    for (i = 0; i < 4; i++) {
        uref_m[i] = temp_wax;
    }

    // Configure i2c2 as a slave device and interrupt priority 5
    I2C2SlaveInit(I2C2_CASU_ADD, BB_I2C_INT_PRIORITY);

    // delay for 2 sec
    for(i = 0; i < 4; i ++) {
        delay_t1(500);
        ClrWdt();
    }

    while (i2cStarted == 0) {
        delay_t1(200);
        ClrWdt();
    }

    dma0Init();
    dma1Init();

    CloseTimer4();
    ConfigIntTimer4(T4_INT_ON | TEMP_LOOP_PRIORITY);
    OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(2000, 256));

    CloseTimer5();
    ConfigIntTimer5(T5_INT_ON | FFT_LOOP_PRIORITY);
    OpenTimer5(T5_ON | T5_PS_1_256, ticks_from_ms(1000, 256));

    diagLED_r[0] = 0;
    diagLED_r[1] = 0;
    diagLED_r[2] = 0;
    LedUser(diagLED_r[0], diagLED_r[1],diagLED_r[2]);

    start_acc_acquisition();

    while(1) {

        ConfigIntTimer2(T2_INT_OFF);    // Disable timer interrupt
        IFS0bits.T2IF = 0;              // Clear interrupt flag
        OpenTimer2(T2_ON | T2_PS_1_256, 65535); // Configure timer

        if (!proxyStandby) {
            statusProxi[0] = I2C1ChSelect(1, 2);            // Front
            proxy_f = VCNL4000ReadProxi();
            delay_t1(1);
            statusProxi[1] = I2C1ChSelect(1, 4);            // Back right
            proxy_br = VCNL4000ReadProxi();
            delay_t1(1);
            statusProxi[2] = I2C1ChSelect(1, 3);            // Front right
            proxy_fr = VCNL4000ReadProxi();
            delay_t1(1);
            statusProxi[3] = I2C1ChSelect(1, 5);            // Back
            proxy_b = VCNL4000ReadProxi();
            delay_t1(1);
            statusProxi[4] = I2C1ChSelect(1, 0);            // Back left
            proxy_bl = VCNL4000ReadProxi();
            delay_t1(1);
            statusProxi[5] = I2C1ChSelect(1, 1);            // Front left
            proxy_fl = VCNL4000ReadProxi();
            delay_t1(1);
        }
        else {
            proxy_f = 0;            // Front
            proxy_br = 0;            // Back right
            proxy_fr = 0;            // Front right
            proxy_b = 0;            // Back
            proxy_bl = 0;            // Back left
            proxy_fl = 0;            // Front left
        }

        if (timer4_flag == 1) {
            // every 2 seconds
            CloseTimer4();
            ConfigIntTimer4(T4_INT_ON | TEMP_LOOP_PRIORITY);
            timer4_flag = 0;

            if (dma_spi2_started == 0) {
                OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(2000, 256));
                skip_temp_filter++;
                tempLoop();
            }
            else {
                OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(50, 256));
            }
        }

        if (dma_spi2_done == 1) {
            fftLoop();
            dma_spi2_done = 0;
        }
        if ((timer5_flag == 1) || (new_vibration_reference == 1)) {
            // every 1 seconds
            CloseTimer5();
            ConfigIntTimer5(T5_INT_ON | FFT_LOOP_PRIORITY);
            OpenTimer5(T5_ON | T5_PS_1_256, ticks_from_ms(1000, 256));

            timer5_flag = 0;
            if (new_vibration_reference == 1) {
            //if(1){
                CloseTimer3();
                dma0Stop();
                dma1Stop();
                spi2Init(2, 0);
                dma0Init();
                dma1Init();
                chipDeselect(aSlaveR);
                IFS0bits.DMA0IF = 0;
                delay_t1(30); // transient response
            }
            new_vibration_reference = 0;

            start_acc_acquisition();
        }

        // Cooler fan control
        if (fanCtlOn == 1) {
            if (temp_pcb >= 25 && fanCooler == FAN_COOLER_OFF)
                fanCooler = FAN_COOLER_ON;
            else if (temp_pcb <= 24 && fanCooler == FAN_COOLER_ON)
                fanCooler = FAN_COOLER_OFF;
            // In case of I2C1 fail turn on the fan
            if ((proxy_f == 0xFFFF) && (proxy_fr == 0xFFFF) && (proxy_br == 0xFFFF) && (proxy_b == 0xFFFF) && (proxy_bl == 0xFFFF) && (proxy_fl == 0xFFFF))
                fanCooler = FAN_COOLER_ON;
        }
        else if (fanCtlOn == 2)
            fanCooler = FAN_COOLER_ON;
        else
            fanCooler = FAN_COOLER_OFF;

        //TEST
//        temp_f = temp_model;
//        if (temp_ref < 30) {
//            temp_r = smc_parameters[0] * 10;
//        }
//        else {
//            temp_r = smc_parameters[0] / 2.0 * 10.0;
//        }
//        temp_r = alpha*10;
//        temp_b = sigma_m * 10;
//        temp_l = sigma * 10;
        //temp_flexPCB = temp_ref_ramp;
/*
        proxy_f = dma_spi2_started;
        proxy_fl = dma_spi2_done;
        proxy_bl = new_vibration_reference;
        proxy_b = timer5_flag;
        proxy_br = timer4_flag;
*/
        int dummy_filt = 0;
        for (i = 0; i < 8; i++) {
            if (index_filter[i] > 0){
                dummy_filt++;
            }
        }

        if (dummy_filt > 0) {
            filtered_glitch = dummy_filt;
            //for (i = 0; i< 8; index_filter[i++] = 0);
        }
        else {
            filtered_glitch = 0;
        }

        updateMeasurements();

        timerVal = ReadTimer2();
        CloseTimer2();
        timeElapsed = ms_from_ticks(timerVal, 256);
        //if (timeElapsed < MAIN_LOOP_DUR)
        //    delay_t1(MAIN_LOOP_DUR - timeElapsed);

        ClrWdt(); //Clear watchdog timer

    } // end while(1)
    return (EXIT_SUCCESS);
}
예제 #3
0
파일: buildapril.c 프로젝트: OpenGelo/iarc
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;	


		}

	}
}