void LightDown (void){ uint8_t i; for (i=PWMGet(); i>0; i--){ PWMSet (i); if (i<PWMStepOver) Wait (PWMSlowDelay); else Wait (PWMQuickDelay); } PWMStop(); }
void LightUp (void){ uint8_t i; PWMStart(0); // Enable shutdown at Light-up for (i=0; i<=250; i++){ PWMSet (i); EnableInput (); if (i<PWMStepOver) Wait (PWMSlowDelay); else Wait (PWMQuickDelay); DisableInput (); if (Valto != None) { EventHandler (); // if key pressed then shutdown if (Valto==KeyPressed) break; } } }
/********************************************************************************************** * Main *********************************************************************************************/ void main(void) { //After tomorrow's test flight. FOR THE LOVE OF GOD MOVE THESE INITALIZATIONS TO FUNCTIONS /********************************************************************************************** * Local Variables *********************************************************************************************/ unsigned long ultrasonic = 0; // Enable lazy stacking for interrupt handlers. This allows floating-point // instructions to be used within interrupt handlers, but at the expense of // extra stack usage. FPULazyStackingEnable(); //Set the clock speed to 80MHz aka max speed SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ); /*unsigned long test[2]; test[0] = 180; test[1] = 10; short bob[1]; bob[0] = ((char)test[0]<<8)|(char)test[1]; float jimmy = (short)(((char)test[0]<<8)|(char)test[1]); jimmy /= 26;*/ /********************************************************************************************** * Peripheral Initialization Awake *********************************************************************************************/ SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); //Turn on GPIO communication on F pins for switches SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); //Turn on GPIO for ADC SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Turn on GPIO for the PWM comms SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); //Turn on GPIO for LED test SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); //Turn on GPIO for UART SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0); //Turn on Timer for PWM SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1); //Turn on Timer for PWM SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2); //Turn on Timer for PWM SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER3); //Turn on Timer for PWM SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C1); //Turn on I2C communication I2C slot 0 SysCtlPeripheralEnable(SYSCTL_PERIPH_UART2); //Turn on the UART com SysCtlPeripheralEnable(SYSCTL_PERIPH_WDOG); //Turn on the watchdog timer. This is a risky idea but I think it's for the best. /********************************************************************************************** * Peripheral Initialization Sleep *********************************************************************************************/ /*SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOF); //This sets what peripherals are still enabled in sleep mode while UART would be nice, it would require the clock operate at full speed which is :P SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_GPIOB); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_GPIOA); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_TIMER0); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_TIMER1); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_TIMER2); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_TIMER3); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_SSI0); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_I2C1); SysCtlPeripheralSleepDisable(SYSCTL_PERIPH_ADC); SysCtlPeripheralClockGating(true); //I'm not sure about this one maybe remove it */ /********************************************************************************************** * PWM Initialization *********************************************************************************************/ SysCtlDelay((SysCtlClockGet() / (1000 * 3))*100); //This shouldn't be needed will test to remove //PWM pin Setup //PWM 0 on GPIO PB6, PWM 1 on pin 4... etc GPIOPinConfigure(GPIO_PB6_T0CCP0); //Pitch - yaw + GPIOPinConfigure(GPIO_PB4_T1CCP0); //Pitch + yaw + GPIOPinConfigure(GPIO_PB0_T2CCP0); //Roll - yaw - GPIOPinConfigure(GPIO_PB2_T3CCP0); //Roll + yaw - GPIOPinTypeTimer(GPIO_PORTB_BASE, (GPIO_PIN_6|GPIO_PIN_4|GPIO_PIN_0|GPIO_PIN_2)); //Prescale the timers so they are slow enough to work with the ESC TimerPrescaleSet(TIMER0_BASE,TIMER_A,2); TimerPrescaleSet(TIMER1_BASE,TIMER_A,2); TimerPrescaleSet(TIMER2_BASE,TIMER_A,2); TimerPrescaleSet(TIMER3_BASE,TIMER_A,2); //Basic LED Out Test Not sure why this is here look into This just turns on an LED that I don't have plugged in. should remove later GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_3); GPIOPinWrite(GPIO_PORTA_BASE,GPIO_PIN_3,0xFF); //GPIOPinTypeGPIOOutputOD(GPIO_PORTB_BASE,GPIO_PIN_0); //Timers Setup for PWM and the load for the countdown TimerConfigure(TIMER0_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM); TimerLoadSet(TIMER0_BASE, TIMER_A, ulPeriod -1); TimerConfigure(TIMER1_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM); TimerLoadSet(TIMER1_BASE, TIMER_A, ulPeriod -1); TimerConfigure(TIMER2_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM); TimerLoadSet(TIMER2_BASE, TIMER_A, (ulPeriod -1)); TimerConfigure(TIMER3_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM); TimerLoadSet(TIMER3_BASE, TIMER_A, ulPeriod -1); //TimerPrescaleSet(TIMER2_BASE, TIMER_A, extender1); //TimerLoadSet(TIMER2_BASE, TIMER_A, period1); //Set the match which is when the thing will pull high TimerMatchSet(TIMER0_BASE, TIMER_A, 254); //Duty cycle = (1-%desired)*1000 note this means this number is percent low not percent high TimerMatchSet(TIMER1_BASE, TIMER_A, 254); TimerMatchSet(TIMER2_BASE, TIMER_A, 254); TimerMatchSet(TIMER3_BASE, TIMER_A, 254); //TimerPrescaleMatchSet(TIMER2_BASE, TIMER_A, extender2); //TimerMatchSet(TIMER2_BASE, TIMER_A, period2); //Enable the timers TimerEnable(TIMER0_BASE, TIMER_A); TimerEnable(TIMER1_BASE, TIMER_A); TimerEnable(TIMER2_BASE, TIMER_A); TimerEnable(TIMER3_BASE, TIMER_A); /********************************************************************************************** * onboard Chip interrupt Initialization *********************************************************************************************/ //These two buttons are used to reset the bluetooth module in case of disconnection GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3); //RGB LED's GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x00); HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY_DD; //Sw1 (PF4) is unaviable unless you make it only a GPIOF input via these commands HWREG(GPIO_PORTF_BASE + GPIO_O_CR) = 0x1; GPIOPinTypeGPIOInput(GPIO_PORTF_BASE,GPIO_PIN_0|GPIO_PIN_4); //Onboard buttons (PF0=Sw2,PF4=Sw1 GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0|GPIO_PIN_4, GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU); //This will make the buttons falling edge (a press pulls them low) //void (*functionPtr)(void) = &onBoardInteruptHandle; GPIOPortIntRegister(GPIO_PORTF_BASE, onBoardInteruptHandle); //set function to handle interupt GPIOIntTypeSet(GPIO_PORTF_BASE,GPIO_PIN_0|GPIO_PIN_4,GPIO_FALLING_EDGE); //Set the interrupt as falling edge GPIOPinIntEnable(GPIO_PORTF_BASE,GPIO_PIN_0|GPIO_PIN_4); //Enable the interrupt //IntMasterEnable(); IntEnable(INT_GPIOF); /********************************************************************************************** * UART Initialization *********************************************************************************************/ //Unlock PD7 HWREG(GPIO_PORTD_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY_DD; HWREG(GPIO_PORTD_BASE + GPIO_O_CR) = 0x80; GPIOPinConfigure(GPIO_PD7_U2TX); //Set PD7 as TX GPIOPinTypeUART(GPIO_PORTD_BASE, GPIO_PIN_7); GPIOPinConfigure(GPIO_PD6_U2RX); //Set PD6 as RX GPIOPinTypeUART(GPIO_PORTD_BASE, GPIO_PIN_6); UARTConfigSetExpClk(UART2_BASE,SysCtlClockGet(),115200,UART_CONFIG_WLEN_8|UART_CONFIG_STOP_ONE|UART_CONFIG_PAR_NONE); //I believe the Xbee defaults to no parity I do know it's 9600 baud though, changed to 115200 for bluetooth reasons UARTFIFOLevelSet(UART2_BASE,UART_FIFO_TX1_8,UART_FIFO_RX1_8); //Set's how big the fifo needs to be in order to call the interrupt handler, 2byte UARTIntRegister(UART2_BASE,Uart2IntHandler); //Regiester the interrupt handler UARTIntClear(UART2_BASE, UART_INT_TX | UART_INT_RX); //Clear the interrupt UARTIntEnable(UART2_BASE, UART_INT_TX | UART_INT_RX); //Enable the interrupt to trigger on both TX and RX event's. Could possibly remove TX UARTEnable(UART2_BASE); //Enable UART IntEnable(INT_UART2); //Second way to enable handler not sure if needed using anyway /********************************************************************************************** * I2C Initialization *********************************************************************************************/ //Serious credit to the man who made the Arduino version of this. he gave me addresses and equations. Sadly Arduino obfuscates what really is happening //Link posted on blog page //gyro address = 0x68 not 0x69 GPIOPinConfigure(GPIO_PA7_I2C1SDA); GPIOPinTypeI2C(GPIO_PORTA_BASE, GPIO_PIN_7); //Set GPA7 as SDA GPIOPinConfigure(GPIO_PA6_I2C1SCL); GPIOPinTypeI2CSCL(GPIO_PORTA_BASE, GPIO_PIN_6); //Set GPA6 as SCL I2CMasterInitExpClk(I2C1_MASTER_BASE,SysCtlClockGet(),false); //I think it operates at 100kbps I2CMasterEnable(I2C1_MASTER_BASE); //Initalize the accelerometer Address = 0x53 GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x02); UARTSend(0xAB); I2CTransmit(0x53,0x2D,0x00); I2CTransmit(0x53,0x2D,0x10); I2CTransmit(0x53,0x2D,0x08); //Initalize the gyroscope Address = 0x68 I2CTransmit(0x68,0x3E,0x00); I2CTransmit(0x68,0x15,0x07); I2CTransmit(0x68,0x16,0x1E); I2CTransmit(0x68,0x17,0x00); UARTSend(0xAC); /********************************************************************************************** * SysTick Initialization *********************************************************************************************/ SysTickIntRegister(SysTickIntHandler); SysTickIntEnable(); SysTickPeriodSet((SysCtlClockGet() / (1000 * 3))*timeBetweenCalculations); //This sets the period for the delay. the last num is the num of milliseconds SysTickEnable(); /********************************************************************************************** * Watchdog Initialization *********************************************************************************************/ WatchdogReloadSet(WATCHDOG_BASE, 0xFEEFEEFF); //Set the timer for a reset WatchdogIntRegister(WATCHDOG_BASE,WatchdogIntHandler); //Enable interrupt WatchdogIntClear(WATCHDOG_BASE); WatchdogIntEnable(WATCHDOG_BASE); WatchdogEnable(WATCHDOG_BASE); //Enable the actual timer IntEnable(INT_WATCHDOG); /********************************************************************************************** * Preflight motor inialization maybe not necessary not going to test *********************************************************************************************/ PWMSet(TIMER0_BASE,998); PWMSet(TIMER1_BASE,998); PWMSet(TIMER2_BASE,998); PWMSet(TIMER3_BASE,998); recievedCommands[0]=253; SysCtlDelay((SysCtlClockGet() / (1000 * 3))*100); //Very important to ensure motor see's a start high (998 makes 0 sense but it works so shhhh don't tell anyone) GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x06); while(1){ WatchdogReloadSet(WATCHDOG_BASE, 0xFEEFEEFF); //Feed the dog a new time //UARTSend(recievedCommands[0]); //SysCtlDelay(50000); //Set 4 PWM Outputs //Get Acc data I2CRead(0x53,0x32,6,quadAcc); //Address blah blah 2 for each axis rawAccToG(quadAcc,RwAcc); /*GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x04); //Blue //Get Gyro data /************************************** Gyro ITG-3200 I2C registers: temp MSB = 1B, temp LSB = 1C x axis MSB = 1D, x axis LSB = 1E y axis MSB = 1F, y axis LSB = 20 z axis MSB = 21, z axis LSB = 22 *************************************/ I2CRead(0x68,0x1B,8,quadGyro); //Address blah blah 2 for each axis + 2 for temperature. why. because why not rawGyroToDegsec(quadGyro,Gyro_ds); //GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x02); //Red //Get the actual angles in XYZ. Store them in RwEst //getInclination(RwAcc, RwEst, RwGyro, Gyro_ds, Awz); //After this function is called RwEst will hold the roll pitch and yaw //RwEst will be returned in PITCH, ROLL, YAW 0, 1, 2 remember this order very important. Little obvious but yaw is worthless /*if(RwEst[1]>0.5){ GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x06); //Red Blue, Correct data read in }else{ GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x0A); //Red Green, The correct data is not there }*/ /*GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x06); //Red Blue, Correct data read in float test=RwAcc[0]*100; //These two commands work char temp = (char)test; //UARTSend((char)(RwAcc[0])*100); //This one does not UARTSend(temp); //UARTSend((char)(RwAcc[1])*100); UARTSend(0xAA); SysCtlDelay((SysCtlClockGet() / (1000 * 3))*1); */ } }
/********************************************************************************************** * SysTick Functions *********************************************************************************************/ void SysTickIntHandler(void){ // UARTSend(0xAA); //Note going for float to char does not work, well it actually may work. It seems that the debugger will show char's as well characters so trying to use it as an int8 is ok but hard to debug. REALLY NEED TO FIND COMPILER'S DATA TYPE SIZES GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x08); //RBG is the order 2 4 8. display green when updating float quadPitch = 0; float quadRoll = 0; short androidPitch = 0; short androidRoll = 0; float pitchOffset = 0; float rollOffset = 0; unsigned long temp = 0; //Handles if the Stellaris UART is no longer listening to the Xbee and such.Should handle it anyway. No promises. /*if(UARTCount == previousCount){ SysCtlPeripheralReset(SYSCTL_PERIPH_UART2); UARTCount += 1; }*/ /*I2CRead(0x53,0x32,6,quadAcc); //Address blah blah 2 for each axis rawAccToG(quadAcc,RwAcc); //Get Gyro data //Gyro ITG-3200 I2C //registers: //temp MSB = 1B, temp LSB = 1C //x axis MSB = 1D, x axis LSB = 1E //y axis MSB = 1F, y axis LSB = 20 //z axis MSB = 21, z axis LSB = 22` I2CRead(0x68,0x1B,8,quadGyro); //Address blah blah 2 for each axis + 2 for temperature. why. because why not rawGyroToDegsec(quadGyro,Gyro_ds); //Get the actual angles in XYZ. Store them in RwEst getInclination(RwAcc, RwEst, RwGyro, Gyro_ds, Awz);*/ //After this function is called RwEst will hold the roll pitch and yaw //RwEst will be returned in YAW, PITCH, ROLL 0, 1, 2 remember this order very important. Little obvious but yaw is worthless //rotateX(HALF_PI * -RwEst[0]); //rotateZ(HALF_PI * RwEst[1]); //quadPitch = abs(atan2(RwEst[2],RwEst[0]) * 180 / PI); //quadRoll = abs(atan2(RwEst[2],RwEst[1]) * 180 / PI); /*quadPitch = fabs(RwEst[0]) * 90; quadRoll = fabs(RwEst[1]) * 90;*/ /*recievedCommands[0] = 90; recievedCommands[1] = 90; recievedCommands[2] = 50; recievedCommands[3] = 0xA0; recievedCommands[4] = 2;*/ quadPitch = RwEst[0] * 90; quadRoll = RwEst[1] * 90; // UARTSend(fabs(quadPitch)); // UARTSend(fabs(quadRoll)); //Note yaw should be tossed because it's garbage. just figuring out what yaw is. androidRoll = recievedCommands[0] - 90; androidPitch = recievedCommands[1] - 90; /*UARTSend(fabs(RwAcc[0]*100)); UARTSend(fabs(RwAcc[1]*100)); UARTSend(fabs(RwAcc[2]*100)); UARTSend(fabs(Gyro_ds[0])); UARTSend(fabs(Gyro_ds[1])); UARTSend(fabs(Gyro_ds[2]));*/ //UARTSend(atan2f((float).3,(float).6)*180/PI); pitchOffset = quadPitch - androidPitch; rollOffset = quadRoll - androidRoll; SysTickPeriodSet((SysCtlClockGet() / (1000 * 3))*timeBetweenCalculations); //The time isn't set instantly but should be delayed till later in the code. Also this is not doing ms effects //char recievedPitchVal=0; These are the chars in the packet order expected to be stored in the array //char recievedRollVal=0; //char recievedYawVal=0; //char recievedThrottle //char recievedSpecialCommand=0; //commandAddress = (commandAddress>4) ? -2 : commandAddress; ? syntax //PWMSet(TIMER0_BASE,recievedCommands[3] - pitchOffset); if(recievedCommands[4] == 15){ //If in manual mode change it to a 2 i put it at 3 to disable it basically //Note 207 is off im trying 190 to make sure the motors don't bug out temp = ((recievedCommands[3] - pitchOffset + (recievedCommands[2]-50))>190) ? 190 : (floor(recievedCommands[3] - pitchOffset + (recievedCommands[2]-50))); temp = (temp < 2) ? 2 : (floor(temp)); UARTSend(temp); PWMSet(TIMER0_BASE,temp); temp = ((recievedCommands[3] + pitchOffset + (recievedCommands[2]-50))>190) ? 190 : (floor(recievedCommands[3] - pitchOffset + (recievedCommands[2]-50))); temp = (temp < 2) ? 2 : (floor(temp)); UARTSend(temp); PWMSet(TIMER1_BASE,temp); temp = ((recievedCommands[3] - rollOffset - (recievedCommands[2]-50))>190) ? 190 : (floor(recievedCommands[3] - rollOffset - (recievedCommands[2]-50))); temp = (temp < 2) ? 2 : (floor(temp)); UARTSend(temp); PWMSet(TIMER2_BASE,temp); temp = ((recievedCommands[3] + rollOffset - (recievedCommands[2]-50))>190) ? 190 : (floor(recievedCommands[3] - rollOffset - (recievedCommands[2]-50))); temp = (temp < 2) ? 2 : (floor(temp)); UARTSend(temp); PWMSet(TIMER3_BASE,temp); }else if(recievedCommands[4]==1){ //Hover mode //float temp = (raw[5]<<8)|(raw[4]); //temp/=256; //if(temp>1){ //} }else if(recievedCommands[4]==0){ PWMSet(TIMER0_BASE,0xCF); PWMSet(TIMER1_BASE,0xCF); PWMSet(TIMER2_BASE,0xCF); PWMSet(TIMER3_BASE,0xCF); }else if(recievedCommands[4]==2){ PWMSet(TIMER0_BASE,recievedCommands[3]); PWMSet(TIMER1_BASE,recievedCommands[3]); PWMSet(TIMER2_BASE,recievedCommands[3]); PWMSet(TIMER3_BASE,recievedCommands[3]); } GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,0x04); //Display blue when flying // UARTSend(0x0D); }