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