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;
}
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);
}
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;
}
}
}