/* Rocket Inertial Guidance System by Marc Udoff Shawn Liang This program uses 3, 8g accelerometers, and 1 gyroscope to determine position. After the rocket reaches the apogee it fires a parachute. Next it guides the rocket back to where it came During flight, data is recorded for post flight analysis. NOTE: THIS CODE MAY HAVE CHANGES FROM THE FINAL VERSION WITHOUT SD. SD HAS NOT BEEN TESTED AND JUST FOR AN EXAMPLE OF HOW WE WOULD/PLANNED TO IMPLEMENT IT */ #include #include // sprintf #include "SD_SPI.c" #include //SIN TABLE #define SIN_SIZE 91 //intialize sin table in flash flash float sinTable[SIN_SIZE]= {0.0, 0.017452406922891652, 0.034899497673273466, 0.05233595637526092, 0.06975647568212136, 0.08715574648633526, 0.10452846353119914, 0.12186935072360491, 0.13917310480770934, 0.15643446543283304, 0.17364818505881505, 0.19080900972237513, 0.20791169133617238, 0.224951061786508, 0.2419219099084592, 0.2588190457424402, 0.27563736328688093, 0.2923717189519147, 0.30901699513102976, 0.32556816192997995, 0.3420201574321773, 0.3583679701963357, 0.37460662051626625, 0.3907311229216636, 0.4067366440441487, 0.4226182691437928, 0.4383711605198014, 0.453990519684951, 0.4694715888272564, 0.48480961518803195, 0.5000000011474721, 0.515038082138895, 0.5299192774134222, 0.5446390540111233, 0.5591929281418008, 0.5735764665507969, 0.5877852535787972, 0.6018150300989029, 0.6156614877816062, 0.6293204088583276, 0.6427876326860014, 0.6560590250256865, 0.6691306392359612, 0.6819983666179156, 0.6946584125096927, 0.7071067975774541, 0.7193397919354646, 0.7313537271239196, 0.743144826895937, 0.7547096140845521, 0.7660444536200831, 0.7771460028856121, 0.7880107724174332, 0.798635507204653, 0.8090170206903871, 0.8191520497341803, 0.8290376055403433, 0.8386705808589446, 0.84804809002692, 0.8571673202360011, 0.8660254051094254, 0.8746197324192683, 0.8829476007774165, 0.8910065543155204, 0.8987940599253441, 0.9063077852644216, 0.9135454760679717, 0.9205048573688913, 0.9271838768623725, 0.9335804351841238, 0.9396926460044798, 0.9455185881197143, 0.9510565172778177, 0.9563047713633961, 0.9612617007335291, 0.9659258436016673, 0.97029573392403, 0.9743700639611371, 0.9781476102624994, 0.9816271854613449, 0.984807763439316, 0.9876883444700546, 0.9902680672792955, 0.9925461563926496, 0.9945219046170538, 0.9961946953404873, 0.9975640515103347, 0.998629538283101, 0.9993908310993529, 0.9998476951017048, 1.0}; enum rocketState{WAITING, FLYING, FALLING} state; //state of rocket //motor burn time is 3.1 seconds #define minTimeTillChute 3500 //3500ms=3.5s minimum before parachute opens after launch #define maxTimeTillChute 7000 //7000ms=7s maximum before parachute opens after launch unsigned int timerChute; //timer till chute can come out #define HEARTFAST 100 //5 times a second #define HEARTMEDIUM 500 //blink once a second #define HEARTSLOW 1000 //blink every other second int timerHeart; #define TIMEDATA 100 //100ms signed int timerData; //get new data from sensors when this expires #define TIMEMOTOR 300 //300ms unsigned int timerMotor; //adjust motor when this expires #define STEPPERTIME 20 //1ms unsigned int stepperTimer; //how long to wait between cycles of stepper motor #define PULLDELAY 2000 //wait 2 second before pulling in chute a little after falling int pullinDelay; //timer for pulling in chute char pulledinFlag; //flag to say the chute was pulled in char motorState1, motorState2; //M1 and M2's state in the magnetic sequence #define dt 0.001*TIMEDATA //how often we got new data (0.001 is because we have our timer in ms) /* MMA1220D: X AXIS SENSITIVITY MICROMACHINEDACCELEROMETER ±8g http://www.freescale.com/files/sensors/doc/data_sheet/MMA1220D.pdf */ //voltage to acceleration (1 axis) #define VtoG8 1/0.256 //1g per 256mV #define ADCtoV 5.0/256.0 //volts per step #define ADCtoG ADCtoV*VtoG8 //ADC to g #define GtoFt 32.174 //32.147 ft/s^2 per g //Chip select for SPI #define SD_CS PORTB.4 #define ADC1_CS PORTB.0 #define ADC2_CS PORTB.1 //phyics a-acceleration, v-velocity, d-displacement float a_x, a_y, a_z; float v_x, v_y, v_z; float d_x, d_y, d_z; //used for updating calculated data float aXLast,aYLast, aZLast; //new data is ready from sensors char updateDataFlag; #define VtoD 1/0.0125 //1(degree/second)/12.5mV for 150 gyro #define ADCtoD ADCtoV*VtoD //ADC to degrees per second float theta; //calibrated manually with using a level as a refernece (Vcc/2 is 0g=128 steps) float zeroOffset[4]={126.75, 125.75, 127.5, 125}; //The internal clock unsigned int time_ms; unsigned int time_sec; //1000 ms/sec up to 65536 seconds=~18 hours of on time before overflow /*********************************** Data Stuff Redundant data system ***********************************/ #define float2fix(a) ((int)((a)*16.0)) //Convert float to fix. a is a float 12 bits of digit, 4 of decimal #define fix2float(a) (((float)a)/16.0)) //Convert a fixed back to float #define STORE_CRIT_SIZE 4 #define STORE_DATA_SIZE 512-STORE_CRIT_SIZE #define HSRATE 125 //high sample rate - every 125ms (8 samples a second) #define LSRATE 1500 //low sample rate - every 1000ms=1s (1 sample every 1.5 seconds) #define maxHS 64 //max number of high sample rate samples to take maxHS*HSRATE=8 seconds of high samples eeprom int flightData[STORE_DATA_SIZE]; //store some flight data unsigned int flightDataIndex; //important times to keep track of - when we think we lifted off, when we triggered the chute, when we pulled in parachute enum CritalTimes{FLY_T=0, FALL_T=1, PULL_T=2, IDKK=3}; eeprom int critTime[STORE_CRIT_SIZE]; unsigned int timerSample; //timer to get new data //SD STUFF //write 512 bytes at a time #define SD_BLOCK_SIZE 512 /* DATA FORMAT: 16 bytes: 0-3: sec, ms 4-7: a_x 8-11: a_y 12-15: a_y */ #define SD_DATASIZE 16 char sdFlag; //is SD in use? - needed for interupts trying to use spi char sdBuffer[SD_BLOCK_SIZE]; //buffer for sd data - must send 512 bytes at a time unsigned int sdBufferIndex; //current index of buffer unsigned long sdAddr; //current address to write to //Easily break up floats into bytes union float2Char{ float f; char c[4]; } f2cTemp; union int2Char{ int i; char c[2]; } i2cTemp; /*************************/ void intialize(void); //sets up the rocket or dumps data to hyperterm void motorControl(void); //does the logic to turn motors and adjust chutes void recordEEPROMData(void); //record a data point in eeprom void rotateM1(char); //rotate motor1 7.5degrees void rotateM2(char); //rotate motor2 7.5degrees void updateData(); //updates v and d from a void rotateCycle(char); //rotates one direction then back void rotateFull(char, char); //does full rotation in a given direction of a motor float getSin(signed long); //sine of any long in degrees #define getCos(angle) (getSin(90+angle)) //cosine of any long in degrees /* Main function called on startup and does: 1. Sets up the rocket 2. Updates Data 3. Records Data 4. Sets of chute and pulls it taught 5. Motor Control */ void main(void) { intialize(); //setup while (1) { //if we have data ready to do float mults do the mult //this must take minTimeTillChute)&&(v_z<0)) || (timerChute>maxTimeTillChute)) { PORTD.3=1; critTime[FALL_T]=time_sec; timerChute=0; state = FALLING; } //end start falling mode //if chute not pulled in a little already, and enough time has passed since it has opened //pull in the chute 2 revolutions if( (pulledinFlag==0) && (pullinDelay==0)) { char rev; critTime[PULL_T]=time_sec; //pull parachute in 2 rev for(rev=0; rev<2; rev++) { //rotate both CW (aka pull in) rotateFull(3,1); } PORTD.3=0; //turn off charge since it must have fired to not kill the battery pulledinFlag=1; } //pull in chute //if we are falling, chute taught, and motor timer has expired, adjust the motors if(FALLING && pulledinFlag==1 && timerMotor==0) motorControl(); } //end if } //end while } /* timer 0 compare ISR (every 1ms) 1. Update all timers and the internal clock 2. Get new data every TIMEDATA and sum theta and convert data to stationary axis data Then set a flag so the data can be integerated (prevents the interupt from being too long) */ interrupt [TIM0_COMP] void timer0_compare(void) { char junk; //update all timers every ms if(state==FALLING && pullinDelay>0) pullinDelay--; //time till next motor adjustment if(timerMotor>0) timerMotor--; //time between steps of the stepper motor if(stepperTimer>0) stepperTimer--; //time till next sample for eeprom if(timerSample>0) timerSample--; //how long we have been flying for (for chute redudant system) if(state==FLYING) timerChute++; if(--timerHeart<=0) { PORTD.2=~PORTD.2; //blink LED if(state==FLYING) timerHeart=HEARTMEDIUM; else if(state==FALLING) timerHeart=HEARTFAST; else timerHeart=HEARTSLOW; } //every ms check and update time if(++time_ms==1000){ time_ms=0; ++time_sec; } //if timer has expired and sd is not writing (SPI in use) and not updating data still if(updateDataFlag==0 && sdFlag==0 && --timerData<=0) { float dTheta; float x, y; timerData=TIMEDATA; //set up SPI for ADC - 500kHz (bigger than data sheet suggests but tested to be accurate by bruce) //Needed since SD will modify spi SPCR = 0b01011110 ; SPSR = 1; //Reading ADC's is a modified version of Bruce Land's Code: http://instruct1.cit.cornell.edu/courses/ee476/SPI/ADCspi2channel.c //8g 1 axis - x ADC1_CS = 0; //chip select low begins conversion SPDR=0b00001100; //set single-ended, channel 0 while (SPSR.7==0); junk = SPDR; SPDR=0x00; //get channel 0 while (SPSR.7==0); x = ((float)SPDR); ADC1_CS = 1; //chip select high ends conversion //8g 1 axis - y ADC1_CS = 0; //chip select low begins conversion SPDR=0b00001110; //set single-ended, channel 1 while (SPSR.7==0); junk = SPDR; SPDR=0x00; //get channel 1 while (SPSR.7==0); y = ((float)SPDR); ADC1_CS = 1; //chip select high ends conversion //8g 1 axis - z ADC2_CS = 0; //chip select low begins conversion SPDR=0b00001100; //set single-ended, channel 0 while (SPSR.7==0); junk = SPDR; SPDR=0x00; //get channel 1 while (SPSR.7==0); a_z = ((float)SPDR); ADC2_CS = 1; //chip select high ends conversion //gyro for x/y axis ADC2_CS = 0; //chip select low begins conversion SPDR=0b00001110; //set single-ended, channel 1 while (SPSR.7==0); junk = SPDR; SPDR=0x00; //get channel 1 while (SPSR.7==0); dTheta = ((float)SPDR); ADC2_CS = 1; //chip select high ends conversion //get change in angle (anglular velocity*time) dTheta = (dTheta-zeroOffset[3])*(ADCtoD*dt); //sum change to keep track of change from launch theta += dTheta; //offset all raw data from sensors x=(x-zeroOffset[0]); y=(y-zeroOffset[1]); a_z=(a_z-zeroOffset[2]); //get true a_x and a_y (round to nearest degree for look up) //casting without rounding since adding floats is slow (to round) a_x=getCos(((signed long)theta))*x-getCos(90-((signed long)theta))*y; a_y=getSin(((signed long)theta))*x+getSin(90-((signed long)theta))*y; updateDataFlag=1; } //end if } //end interrupt /* updateData 1. Saves current acceleration vector 2. Sets the update data flag to 0 so we can get more data 3. Determines if we are flying (we overcame gravity) 4. If we are in the air integrate velocity and displacement via the verlet algorithm 5. Store Data on SD card */ void updateData(void) { float AX, AY, AZ; //store before they are overwritten AX=a_x; AY=a_y; AZ=a_z; //we are updating the data - can get more data updateDataFlag=0; AX=AX*(ADCtoG*GtoFt); //convert from ADC to ft/s^2 AY=AY*(ADCtoG*GtoFt); //convert from ADC to ft/s^2 AZ=AZ*(ADCtoG*GtoFt); //convert from ADC to ft/s^2 //if we are waiting for liftoff and we go more than 0g (overcame gravity) we are flying if((state==WAITING) && (AZ>0.0)) { state=FLYING; critTime[FLY_T]=time_sec; } //are we in the air? if(state!= WAITING) { //update x,y,z displacement and velocity //Verlet integration algorithm //From: http://instruct1.cit.cornell.edu/courses/cs417-land/SECTIONS/dynamics.html //Xn=Xn-1+Vn-1*dt+1/2*An-1*dt^2 //Vn=Vn-1+1/2(An+An-1)*dt d_x = d_x+v_x*dt+aXLast*(dt*dt/2.0); v_x = v_x + (AX+aXLast)*dt/2.0; d_y = d_y+v_y*dt+aYLast*dt*dt/2.0; v_y = v_y + (AY+aYLast)*dt/2.0; d_z = d_z+v_z*dt+aZLast*dt*dt/2.0; v_z = v_z + (AZ+aZLast)*dt/2.0; //update last accelaterion for verlet algorithm aXLast=AX; aYLast=AY; aZLast=AZ; } //end state != waiting /* Store data on the SD card. Only acceleration needed as everything else can be derived DATA FORMAT: --16 bytes-- 0-3: sec, ms 4-7: a_x 8-11: a_y 12-15: a_y */ //if there room is room the sd buffer, write data if((sdBufferIndex+SD_DATASIZE)=SD_BLOCK_SIZE) { //write 0 for all unused bytes in block for(sdBufferIndex; sdBufferIndex 0 SE CW II SW CCW NW CCW X+Y < 0 NW CW X+Y > 0 NE CW III SE CCW SW CCW X-Y > 0 SW CW X-Y < 0 NW CW IV NE CCW SE CCW X+Y > 0 SE CW X+Y < 0 SW CW ************************/ void motorControl(void) { char ccw = 1; char cw =2; if(d_x > 0 && d_y > 0) { // First Quadrant if(a_x < 0 && a_y > 0) // NW rotateCycle(ccw); else if (a_x > 0 && a_y > 0) // NE if(a_x - a_y < 0) rotateCycle(ccw); else rotateCycle(cw); else if (a_x < 0 && a_y < 0) // SE rotateCycle(cw); } if(d_x < 0 && d_y > 0) { // Second Quadrant if(a_x < 0 && a_y < 0) // SW rotateCycle(ccw); else if (a_x < 0 && a_y > 0) // NW if(a_x + a_y < 0) rotateCycle(ccw); else rotateCycle(cw); else if (a_x > 0 && a_y > 0) // NE rotateCycle(cw); } if(d_x < 0 && d_y < 0) { // Third Quadrant if(a_x > 0 && a_y < 0) // SE rotateCycle(ccw); else if (a_x < 0 && a_y < 0) // SW if(a_x - a_y > 0) rotateCycle(ccw); else rotateCycle(cw); else if (a_x < 0 && a_y > 0) // NW rotateCycle(cw); } if(d_x > 0 && d_y < 0) { // Fourth Quadrant if(a_x > 0 && a_y > 0) // NE rotateCycle(ccw); else if (a_x > 0 && a_y < 0) // SE if(a_x - a_y > 0) rotateCycle(ccw); else rotateCycle(cw); else if (a_x < 0 && a_y < 0) // SW rotateCycle(cw); } } /* rotateCycle rotate 5 rotations CW then CCW for a specific motor (motor=3 does both) Used by motorControl PULL ON M1 to turn rocket CCW PULL ON M2 to turn rocket CW */ void rotateCycle(char motor) { int i; for(i=0; i<5; i++) { rotateFull(motor,1); } for(i=0; i<5; i++) { rotateFull(motor,0); } } /* rotateFull Do one full revolution CW 1 is motor1, 2 is motor2, 3 is both dir is the either 1 (CW motor rotation) or 2 (CW motor rotation) */ void rotateFull(char motor, char dir) { char i; for(i=0; i<48; i++) { if(motor&0x1) rotateM1(dir); if(motor&0x2) rotateM2(dir); //delay stepperTimer=STEPPERTIME; //since this stepper will take a long time, check for crital processes waiting if(updateDataFlag) updateData(); if(timerSample==0) recordEEPROMData(); while(stepperTimer!=0); } } /* rotateM2 - Rotate Motor 2 PULL with M2 to turn rocket CW cw pulls in the parachute Uses a state machine to keep track of current state to rotate motor correctly */ void rotateM2(char cw){ if(cw==1){ if(++motorState2>4) motorState2=1; } else if(--motorState2<1) motorState2=4; switch(motorState2) { case(1): { PORTA.4=0; //Yellow PORTA.5=1; //Orange PORTA.6=1; //Black PORTA.7=0; //Brown break; } case(2): { PORTA.4=0; //Yellow PORTA.5=1; //Orange PORTA.6=0; //Black PORTA.7=1; //Brown break; } case(3): { PORTA.4=1; //Yellow PORTA.5=0; //Orange PORTA.6=0; //Black PORTA.7=1; //Brown break; } case(4): { PORTA.4=1; //Yellow PORTA.5=0; //Orange PORTA.6=1; //Black PORTA.7=0; //Brown break; } } } /* rotateM1 - Rotate Motor 2 PULL with M1 to turn rocket CCW cw pulls in the parachute Uses a state machine to keep track of current state to rotate motor correctly */ void rotateM1(char cw){ if(cw==1){ if(++motorState1>4) motorState1=1; } else if(--motorState1<1) motorState1=4; switch(motorState1) { case(1): { PORTA.0=0; //Yellow PORTA.1=1; //Orange PORTA.2=1; //Black PORTA.3=0; //Brown break; } case(2): { PORTA.0=0; //Yellow PORTA.1=1; //Orange PORTA.2=0; //Black PORTA.3=1; //Brown break; } case(3): { PORTA.0=1; //Yellow PORTA.1=0; //Orange PORTA.2=0; //Black PORTA.3=1; //Brown break; } case(4): { PORTA.0=1; //Yellow PORTA.1=0; //Orange PORTA.2=1; //Black PORTA.3=0; //Brown break; } } } /* getSin(angle) where angle is in degrees (-32768, +32767) Takes a long, centers it between 0 and 360, and looks up angle in a lookup table Note: cos=sin(90+x) */ float getSin(signed long angle) { //get angle from 0-360 while(!(0<=angle && angle<360)) { if(angle>=360) angle -= 360; else angle += 360; } //first quadrant I/II if(angle<=90) return sinTable[angle]; else if(angle<=180) return sinTable[180-angle]; else if(angle<=270) return 0-sinTable[angle-180]; else return 0-sinTable[360-angle]; } /* intialize 1. Sets up UART 2. Sets up ports 3. Sets up timer0 compare interupt 4. Dump data if D.5 is grounded (run program at Vcc) 5. Set up all variables for flight to start 6. Turn on interupts */ void intialize(void) { int i; //init the UART (D.0 D.1) UCSRB = 0x18; UBRRL = 103; printf(" starting...\n\r"); //set up SPI //bit 7 SPIE=0 no ISR //bit 6 SPE=1 enable spi //bit 5 DORD=0 msb first //bit 4 MSTR=1 Mega32 is spi master //bit 3 CPLO=1 clock polarity //bit 2 CPHA=1 clock phase //bit 1,0 rate sel=10 along with SPRC=1 sets clk to f/32 = 500 kHz SPCR = 0b01011110; SPSR = 1; //set up i/o data direction DDRB.0 = 1; //output chip select for ADC with x and y DDRB.1 = 1; //output chip select for ADC with z and gyro DDRB.4 = 1; //CS for SD card DDRB.5 = 1; //output MOSI DDRB.6 = 0; //input MISO DDRB.7 = 1; //output SCLK PORTB.0=1; PORTB.1=1; PORTB.4=1; DDRD.2=1; //output - life LED PORTD.2=1; //this led will flash DDRD.3=1; //output - launch parachute PORTD.3=0; //keep parachute in //USED FOR EEPROM DATA DUMP //D.5==1 -> run program, D.5==0 -->dump data DDRD.5=0; //input DDRA=0b11111111; //output for motor PORTA=0; //set up timer 0 TIMSK=2; //turn on timer 0 cmp match ISR OCR0 = 250; //set the compare re to 250 time ticks - 1ms //prescalar to 64 and turn on clear-on-match TCCR0=0b00001011; //dump flight data D.5 is grounded if(PIND.5==0) { int j; //ALL DATA NEEDS TO BE DIVIDED BY 16!!!! printf("t = ["); for(j=0; j