#include <htc.h> #include "pic16f690.h" #include <stdio.h> #include "bitdefs.h" __CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS); #define FALSE 0 #define TRUE 1 #define ON 1 #define OFF 0 #define _MS *10/8 //timer overflows every 0.8 ms... #define TimeoutTime 10_MS #define ACV_MSB 0x21 #define ACV_LSB 0x86 #define Green 0x01 #define Red 0xFE #define GreenServo 1 #define RedServo 0 /**************************************************************************************/ //Module types typedef enum{ WaitingDelimiter, WaitingLengthMSB, WaitingLengthLSB, WaitingData, WaitingCheckSum, ErrorInRX } RXStates_t; typedef enum{ WaitingToSend, SendingDelimiter, SendingLengthMSB, SendingLengthLSB, SendingAPIID, SendingFrameID, SendingDestAddMSB, SendingDestAddLSB, SendingOptByte, SendingDataBytes, CalculatingCheckSum, SendingCheckSum } XmitStates_t; typedef enum{ NoColor, GreenTeam, RedTeam } AtollStates_t; typedef enum{ TC_Green, TC_PendingGreen, TC_None, TC_PendingRed, TC_Red } TeamColor_t; //For finding team color typedef enum{ IO_RunAD, IO_CalcPower, IO_CalcDirection, IO_CalcMapping, IO_CalcPowerBoost, IO_ResetDriveArray } CheckIOStates_t; //For CheckIO's /**************************************************************************************/ //Function prototypes void Initialization(void); void CheckTimer(void); void XmitStateMachine(void); void CheckIO(void); void RXStateMachine(void); void Response_RXGoodMessage(void); void UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber); void SetLEDs(void); void wait_us(unsigned long microsecs); signed int SQRT(signed int a); signed int CalculatePower(signed int y_accel, signed int x_accel); signed int CalculateDirection(signed int Power, signed int x_accel); void CalibrationCheck(void); void RunAD(char Chan0, char Chan1, char Chan2, char Chan3); void UpdateTeamColor(unsigned char OurTeamColor); /**************************************************************************************/ //Module Variables static unsigned char Xmit_DestAddMSB; static unsigned char Xmit_DestAddLSB; static unsigned char Xmit_DataLength; static unsigned char Drive_DataByteArray[10]; static unsigned char Servo_DataByteArray[10] = 0; static unsigned char TimerOverflowFlag = FALSE; static unsigned char TimerOverflowFlag2 = FALSE; static XmitStates_t CurrentXmitState = WaitingToSend; static unsigned char InitiateXmitIOFlag = FALSE; static unsigned char InitiateXmitServosFlag = FALSE; static unsigned int CurrentTimerTick = 0; static unsigned char IncomingData[15]; static unsigned char IncomingDataLength; static unsigned char RXGoodMessageFlag = FALSE; static AtollStates_t Atoll[] = {NoColor,NoColor,NoColor,NoColor,NoColor,NoColor,NoColor,NoColor}; static char ServoStates[]={OFF,OFF,OFF,OFF,OFF,OFF,OFF,OFF}; static unsigned char ServoOnOffByte = 0; // Byte 1 that I send to servos static unsigned char ServoColorByte = 0; // Byte 2 that I send to servos static unsigned char TeamColorByte = 0x88; // Byte 3 that I send to servos, initialize to no color static unsigned char UpdateColorsFlag = FALSE; static signed char Output_LeftMotorSpeed; static signed char Output_RightMotorSpeed; static CheckIOStates_t CurrentIOState = IO_RunAD; /**************************************************************************************/ /* Function: main * Usage: main() * -------------- * This initializes and then in an infinite loop looks at the timer (to check overflows), * calls the Inputs state machine, transmit state machine, and receive transmission. If * a good message was received it calls the appropriate function to handle the message. */ void main(void){ Initialization(); while(1){ CheckTimer(); if((InitiateXmitIOFlag == FALSE) && (InitiateXmitServosFlag == FALSE)){ CheckIO(); } XmitStateMachine(); RXStateMachine(); if(RXGoodMessageFlag == TRUE){ Response_RXGoodMessage(); } } } /**************************************************************************************/ /* Function: Initialization * Usage: Initialization() * ------------------------------- * This initializes all input ports, as well as the A/D conversion, EUART, and TIMER0. */ void Initialization(void){ //Clear ANSEL registers, except AN0, AN4, AN5 ANSEL = 0; ANSELH = 0; ANS4 = 1; //x-axis ANS5 = 1; //y-axis ANS0 = 1; // Power boost //Output for swap motor input button TRISC4 = 1; //TRIS registers for EUSART TRISB |= BIT5HI; //RX input TRISB &= BIT7LO; //TX output //Inputs for for accelerometer TRISC0 = 1; //x-axis TRISC1 = 1; //y-axus //Debug LEDs TRISA1 = 0; TRISA2 = 0; RA1 = 1; RA2 = 1; //AD Conversion //AD conversion Fosc/32 ADCS0 = 0; ADCS1 = 1; ADCS2 = 0; VCFG = 0; //Use Vdd as voltage reference //Select AN4 for now CHS0 = 0; CHS1 = 0; CHS2 = 1; CHS3 = 0; ADFM = 0; //Left justified ADON = 1; //Enable ADC //EUSART init SPBRG = 129; // For 9.6kBaud TXSTA |= BIT2HI; //BRGH; //asynch mode: //TXSTA &= ~SYNC; // defaults to asynchronous already RCSTA |= BIT7HI; //SPEN; //enable tx and rx: TXSTA |= BIT5HI; //TXEN; CREN = 1; // Initialize Timer0 T0CS = 0; // Use internal Fosc/4 PSA = 0; PS0 = 1; // All of this is /16... about 0.8ms per overflow PS1 = 1; PS2 = 0; //Timer 0 is default enabled... //Initialize Servos (scoreboard and team color) Servo_DataByteArray[0] = 0xB9; Servo_DataByteArray[1] = 0b00000000; Servo_DataByteArray[2] = 0b00000000; Servo_DataByteArray[3] = 0x88; } /**************************************************************************************/ /* Function: XmitStateMachine * Usage: XmitStateMachine() * --------------------------- * This state machine is used to transmit over EUART motor commands to the ACV (through * the xbee), as well as transmitting servo states for the servos that indicate the * score and team color on our CVC. */ void XmitStateMachine(void){ static unsigned char DelimiterByte = 0x7E; static unsigned char LengthMSB = 0x00; static unsigned char LengthLSB = 0; //Will be updated later static unsigned char APIID = 0x01; //Indicates sending packet of data static unsigned char DestAddMSB; static unsigned char DestAddLSB; static unsigned char OptByte = 0x00; //No options static unsigned char DataByteArray[10]; //Chose 10 randomly as an upper bound static unsigned char AccumulatingCheckSum = 0; static unsigned char DataByteIndex; static unsigned char DataLength; //Dictated by "Global" input static unsigned char FrameID = 0; unsigned char j = 0; DestAddMSB = ACV_MSB; DestAddLSB = ACV_LSB; DataLength = 4; // Anything I have to xmit is only 4 bytes long LengthLSB = DataLength + 5; // 5 is for APIID, FrameID, DestAddMSB, DestAddLSB, Options //The first data byte is sent during the SendingOptByte state, so at that state //we check to see if the current xmit is either sending a motor command or update //servos color command. Depending on which we extract the appropriate data byte array //to send. if(CurrentXmitState == SendingOptByte){ if(InitiateXmitIOFlag == TRUE){ for(j=0; j<10;j++){ DataByteArray[j] = Drive_DataByteArray[j]; } }else if(InitiateXmitServosFlag == TRUE){ for(j=0; j<10;j++){ DataByteArray[j] = Servo_DataByteArray[j]; } } } //Here begins the actual state machine switch(CurrentXmitState){ case WaitingToSend: if(TXIF == 1){ if(TimerOverflowFlag == TRUE){ TimerOverflowFlag = FALSE; //Fires every 200ms InitiateXmitIOFlag = TRUE; TXREG = DelimiterByte; CurrentXmitState = SendingDelimiter; }else if(TimerOverflowFlag2 == TRUE){ TimerOverflowFlag2 = FALSE; //Fires between every 200ms flag InitiateXmitServosFlag = TRUE; CurrentXmitState = SendingOptByte; } } break; case SendingDelimiter: if(TXIF == 1){ TXREG = LengthMSB; CurrentXmitState = SendingLengthMSB; } break; case SendingLengthMSB: if(TXIF == 1){ TXREG = LengthLSB; CurrentXmitState = SendingLengthLSB; } break; case SendingLengthLSB: if(TXIF == 1){ TXREG = APIID; CurrentXmitState= SendingAPIID; AccumulatingCheckSum = 0; AccumulatingCheckSum += APIID; //start the checksum } break; case SendingAPIID: if(TXIF == 1){ TXREG = FrameID; //Don't need to check frame ID since I am always sending CurrentXmitState = SendingFrameID; AccumulatingCheckSum += FrameID; } break; case SendingFrameID: if(TXIF == 1){ TXREG = DestAddMSB; CurrentXmitState = SendingDestAddMSB; AccumulatingCheckSum += DestAddMSB; } break; case SendingDestAddMSB: if(TXIF == 1){ TXREG = DestAddLSB; CurrentXmitState = SendingDestAddLSB; AccumulatingCheckSum += DestAddLSB; } break; case SendingDestAddLSB: if(TXIF == 1){ TXREG = OptByte; CurrentXmitState = SendingOptByte; AccumulatingCheckSum += OptByte; } break; case SendingOptByte: if(TXIF == 1){ DataByteIndex = 0; // Initialize index TXREG = DataByteArray[DataByteIndex]; CurrentXmitState = SendingDataBytes; AccumulatingCheckSum += DataByteArray[0]; //Begin sending data } break; case SendingDataBytes: if(TXIF == 1){ DataByteIndex++; //This works because DataLength starts at 1, while index starts at 0 if(DataByteIndex >= DataLength){ //if finished sending all data, go to next state CurrentXmitState = CalculatingCheckSum; if(InitiateXmitServosFlag == TRUE){ InitiateXmitServosFlag = FALSE; CurrentXmitState = WaitingToSend; } }else{ TXREG = DataByteArray[DataByteIndex]; AccumulatingCheckSum += DataByteArray[DataByteIndex]; } } break; case CalculatingCheckSum: if(TXIF == 1){ TXREG = 0xFF - AccumulatingCheckSum; CurrentXmitState = SendingCheckSum; AccumulatingCheckSum = 0; } break; case SendingCheckSum: if(TXIF == 1){ CurrentXmitState = WaitingToSend; InitiateXmitIOFlag = FALSE; //Reset both flags no matter what at the end InitiateXmitServosFlag = FALSE; } break; } } /**************************************************************************************/ /* Function: RXStateMachine * Usage: RXStateMachine() * ------------------------ * This state machine is used to receive data from the Xbee. There are timeouts * implemented at each state, and if a valid message was received (checksum was correct) * then a flag is set to be true to indicate that a good message was received (RXGoodMessageFlag). */ void RXStateMachine(void){ static RXStates_t CurrentRXState = WaitingDelimiter; static unsigned char IncomingDataIndex; static unsigned int LastTimerTick; //For the timout static unsigned char AccumulatingCheckSum; unsigned IncomingCheckSum; //Here begins actual state machine switch(CurrentRXState){ case WaitingDelimiter: if(RCIF == TRUE){ if(RCREG == 0x7E){ CurrentRXState = WaitingLengthMSB; LastTimerTick = CurrentTimerTick; }else{ CurrentRXState = ErrorInRX; } } break; case WaitingLengthMSB: if(RCIF == TRUE){ if(RCREG == 0x00){ //MSB should be 0, we don't have very long data CurrentRXState = WaitingLengthLSB; LastTimerTick = CurrentTimerTick; }else{ CurrentRXState = ErrorInRX; } }else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout CurrentRXState = ErrorInRX; } break; case WaitingLengthLSB: if(RCIF == TRUE){ IncomingDataLength = RCREG; CurrentRXState = WaitingData; LastTimerTick = CurrentTimerTick; IncomingDataIndex = 0; // Initialize the index here, where it starts AccumulatingCheckSum = 0; }else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout CurrentRXState = ErrorInRX; } break; case WaitingData: if(RCIF == TRUE){ IncomingData[IncomingDataIndex] = RCREG; AccumulatingCheckSum += IncomingData[IncomingDataIndex]; IncomingDataIndex++; LastTimerTick = CurrentTimerTick; if(IncomingDataIndex >= IncomingDataLength){ //If all data has come in, go to next state CurrentRXState = WaitingCheckSum; } }else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout CurrentRXState = ErrorInRX; } break; case WaitingCheckSum: if(RCIF == TRUE){ IncomingCheckSum = RCREG; if((IncomingCheckSum + AccumulatingCheckSum) != 0xFF){ //Check to see if checksum valid CurrentRXState = ErrorInRX; }else{ RXGoodMessageFlag = TRUE; //Trigger good message flag CurrentRXState = WaitingDelimiter; } }else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout CurrentRXState = ErrorInRX; } break; case ErrorInRX: //Error just returns you to first state CurrentRXState = WaitingDelimiter; break; } } /**************************************************************************************/ /* Function: Response_RXGoodMessage * Usage: Response_RXGoodMessage() * ------------------------------- * This response routine extracts the actual command data, then checks to see if it was * either a command to change the score servos or the team color servo, and calls the * the appropriate function to do one or the other. */ void Response_RXGoodMessage(void){ unsigned char RXCommandDataLength; unsigned char RXFrameID; unsigned char RXSourceAddMSB; unsigned char RXSourceAddLSB; unsigned char RXIncomingDataOptions; unsigned char RXCommandData[10]; //Arbritrary for now unsigned char i; RXGoodMessageFlag = FALSE; if(IncomingData[0] == 0x81){ RXCommandDataLength = IncomingDataLength - 5; // 5 = APIID, SourceMSB, SourceLSB, RSSI, Options RXSourceAddMSB = IncomingData[1]; RXSourceAddLSB = IncomingData[2]; //IncomingData[3]; This is signal strength; ignore RXIncomingDataOptions = IncomingData[4]; for(i = 0; i < RXCommandDataLength; i++){ RXCommandData[i] = IncomingData[5+i]; } if((RXSourceAddMSB == ACV_MSB) && (RXSourceAddLSB == ACV_LSB) && ((RXIncomingDataOptions & BIT1HI) != BIT1HI)){ //ACV is sending me teamcolor if(RXCommandDataLength == 2){ if(RXCommandData[0] == 0xEF){ UpdateTeamColor(RXCommandData[1]); RA1^=1; } } //else it is a broadcast to update atoll colors }else if((RXIncomingDataOptions & BIT1HI) == BIT1HI){ if(RXCommandDataLength == 3){ if(RXCommandData[0] == 0xAB){ //This means that an Atoll was captured UpdateAtollColors(RXCommandData[1],RXCommandData[2]); } } } } } /**************************************************************************************/ /* Function: UpdateTeamColor * Usage: UpdateTeamColor(unsigned char OurTeamColor) * -------------------------------------------------- * This updates the appropriate servo data byte (Servo_DataByteArray[3]) with the input OurTeamColor. * It just passes on this input to the servo PIC. */ void UpdateTeamColor(unsigned char OurTeamColor){ TeamColorByte = OurTeamColor; if(TeamColorByte == 0x88){ // If white key fob is swiped ServoOnOffByte = 0; //Reset the atoll color scores ServoColorByte = 0; } Servo_DataByteArray[0] = 0xB9; Servo_DataByteArray[3] = TeamColorByte; //Just pass on the team color byte to servo PIC UpdateColorsFlag = TRUE; //Don't use anymore, but there just in case we need it. } /**************************************************************************************/ /* Function: UpdateAtollColors * Usage: UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber) * ---------------------------------------------------------------------------- * This is called when an atoll is captured using the color of the team that captured * as one input and the atoll number that was captured as the other input. * It updates the two appropriate bytes associated with this: Servo_DataByteArray[1] and * Servo_DataByteArray[2]. The first byte is just an On/Off byte to indicate whether * a servo should be on and not on the neutral position. The second byte inciates color, * 0 for red and 1 for green. */ void UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber){ unsigned char i; //Update array of Atoll colors and whether the servo should be on if(TeamColor == Green){ Atoll[AtollNumber-1] = GreenTeam;// -1 because index starts at 0 ServoStates[AtollNumber-1] = ON; }else if(TeamColor == Red){ Atoll[AtollNumber-1] = RedTeam; ServoStates[AtollNumber-1] = ON; }else if(TeamColor == NoColor){ Atoll[AtollNumber-1] = NoColor; ServoStates[AtollNumber-1] = OFF; } ServoColorByte = 0; ServoOnOffByte = 0; //Takes the two arrays above and extracts it into two single bytes for(i = 0; i < 8; i++){ if(Atoll[7-i] == GreenTeam){ ServoColorByte = (ServoColorByte*2) + 1; }else if(Atoll[7-i] == RedTeam){ ServoColorByte = (ServoColorByte*2); }else if(Atoll[7-i] == NoColor){ ServoColorByte = (ServoColorByte*2); } } for(i = 0; i < 8; i++){ if(ServoStates[7-i] == OFF){ ServoOnOffByte = (ServoOnOffByte*2); }else{ //Servo is probably on ServoOnOffByte = (ServoOnOffByte*2) + 1; } } Servo_DataByteArray[1] = ServoOnOffByte; Servo_DataByteArray[2] = ServoColorByte; UpdateColorsFlag = TRUE; //Don't use anymore, but there just in case we need it. return; } /**************************************************************************************/ /* Function: RunAD * Usage: RunAD(char Chan3, char Chan2, char Chan1, char Chan0) * ------------------------------------------------------------- * This just takes the four bits it takes to set a port to run an A/D convresion. * It does the necessary minimal blocking to get this process done. After, this function * is run, you can check ADRESH for the A/D value. */ void RunAD(char Chan3, char Chan2, char Chan1, char Chan0){ unsigned char i; CHS0 = Chan0; CHS1 = Chan1; CHS2 = Chan2; CHS3 = Chan3; ADIF = 0; for(i = 0; i<50; i++){} //Dead for loop for acquisition ~20us GO_DONE = 1; //Starts Conversion process while(ADIF == 0){} return; } /**************************************************************************************/ /* Function: CheckIO * Usage: CheckIO() * ------------------ * This is actually a state machine because some parts of the code take so long to run * that it was appropriate to do so. It checks the accelerometer outputs to do appropriate * left/right motor mapping. It checks the FSR output to see how much power boost there * should be. And it checks the RC4 button to see if we want to swap the motors. */ void CheckIO(void){ static signed int X_ADConversion; static signed int Y_ADConversion; static signed int X_Coordinate; static signed int Y_Coordinate; static signed int Direction; static signed int Power; static signed int absDirection; static signed int absLeftMotorSpeed; static signed int absRightMotorSpeed; static signed int LeftMotorSpeed; static signed int RightMotorSpeed; static signed int ChangeDirections; //Power boost variables static signed int FSR_ADConversion; //Data bytes to transmit static unsigned char DataByte1 = 0; static unsigned char DataByte2 = 0; static unsigned char DataByte3 = 0; switch(CurrentIOState){ case IO_RunAD: //Select AN4 RunAD(0,1,0,0); X_ADConversion = ADRESH; //Select AN5 RunAD(0,1,0,1); Y_ADConversion = ADRESH; //Select AN0 for Left Power Boost RunAD(0,0,0,0); FSR_ADConversion = ADRESH; //Check change directions button (it's really a flip in that left becomes right, and vice versa) ChangeDirections = RC4; CurrentIOState = IO_CalcPower; break; case IO_CalcPower: //Make x and y go from -128 to 127 X_Coordinate = (X_ADConversion - 1) - 127; Y_Coordinate = (Y_ADConversion - 1) - 127; Power = CalculatePower(Y_Coordinate, X_Coordinate); CurrentIOState = IO_CalcDirection; break; case IO_CalcDirection: Direction = CalculateDirection(Power, X_Coordinate); CurrentIOState = IO_CalcMapping; break; case IO_CalcMapping: //Does our motor mapping as well as makes them absolutes absDirection = (Direction >= 0) ? Direction : -Direction; LeftMotorSpeed = (((Power*Direction) >= 0) ? Power : ((127 - 2*absDirection)*Power/127)); RightMotorSpeed = (((Power*Direction) <= 0) ? Power : ((127 - 2*absDirection)*Power/127)); absLeftMotorSpeed = ((LeftMotorSpeed >= 0) ? LeftMotorSpeed : -LeftMotorSpeed); absRightMotorSpeed = ((RightMotorSpeed >= 0) ? RightMotorSpeed : -RightMotorSpeed); //Assigns appropriate bits for direction, 0 means reverse, 1 means forward DataByte1 = 0; if(RightMotorSpeed > 0){ if(ChangeDirections == OFF){ DataByte1 |= BIT1HI; }else{ DataByte1 |= BIT0HI; } } if(LeftMotorSpeed > 0){ if(ChangeDirections == OFF){ DataByte1 |= BIT0HI; }else{ DataByte1 |= BIT1HI; } } CurrentIOState = IO_CalcPowerBoost; break; case IO_CalcPowerBoost: //Power Boost functionality, scales appropriately the power. if((FSR_ADConversion >= 26) && (FSR_ADConversion <= 164)){ absLeftMotorSpeed = (absLeftMotorSpeed*(FSR_ADConversion + 36))/200; absRightMotorSpeed = (absRightMotorSpeed*(FSR_ADConversion + 36))/200; }else if(FSR_ADConversion > 164){ absLeftMotorSpeed = (absLeftMotorSpeed); absRightMotorSpeed = (absRightMotorSpeed); }else if(FSR_ADConversion < 26){ absLeftMotorSpeed = 0; absRightMotorSpeed = 0; } CurrentIOState = IO_ResetDriveArray; break; case IO_ResetDriveArray: if(ChangeDirections == OFF){ DataByte1 |= BIT7HI; // For RFID Spoof activation } Drive_DataByteArray[0] = 0xCD; Drive_DataByteArray[1] = DataByte1; //Flip the motors if button was pressed if(ChangeDirections == OFF){ Drive_DataByteArray[2] = 2*((unsigned char)absLeftMotorSpeed); Drive_DataByteArray[3] = 2*((unsigned char)absRightMotorSpeed); }else{ Drive_DataByteArray[3] = 2*((unsigned char)absLeftMotorSpeed); Drive_DataByteArray[2] = 2*((unsigned char)absRightMotorSpeed); } CurrentIOState = IO_RunAD; break; } } /**************************************************************************************/ /* Function: CheckTimer * Usage: CheckTimer() * -------------------- * This timer updates for two flags. TimerOverflowFlag is one that fires every 200ms. * TimerOverflowFlag2 fires between every one of those 200ms flags. */ // This is a timer check that amounts to 5Hz using overflows void CheckTimer(void){ static unsigned char i=0; if(T0IF == 1){ i++; CurrentTimerTick++; T0IF = 0; if(i == 120){ TimerOverflowFlag2 = TRUE; } if(i>=245){ i=0; TimerOverflowFlag = TRUE; } } } /**************************************************************************************/ /* Function: CalculatePower * Usage: CalculatePower(signed int y_accel, signed int x_accel) * ------------------------------------------------------------- * We calculate power by squaring x and y, and taking the square root. This function just * calls the square root function. */ signed int CalculatePower(signed int y_accel, signed int x_accel){ signed int R_squared; signed int Power; signed int abs_x_accel_temp; signed int abs_y_accel_temp; signed int abs_x_accel; signed int abs_y_accel; abs_x_accel_temp = ((x_accel > 0) ? x_accel : - x_accel); abs_y_accel_temp = ((y_accel > 0) ? y_accel : - y_accel); abs_x_accel = abs_x_accel_temp; abs_y_accel = abs_y_accel_temp; R_squared = (abs_x_accel*abs_x_accel) + (abs_y_accel*abs_y_accel); Power = SQRT(R_squared); //make sure power >= x_accel Power = ((Power > abs_x_accel) ? Power : abs_x_accel); //Check to see if one of accel directions is negative, if so make power negative if((y_accel < 0)){ Power = -Power; } return Power; } /**************************************************************************************/ /* Function: SQRT * Usage: SQRT(signed int a) * ------------------------- * Calculates a square root with approximation. */ signed int SQRT(signed int a){ signed int SQRT_Result; if(a <= 1000){ SQRT_Result = (a*23)/1000; } else if( (a > 1000) && (a <= 17000) ){ SQRT_Result = 75 + (a/300) - (46875)/(((a/225) + 25) * ((a/225) + 25)); } else if( (a > 17000) ){ SQRT_Result = 127; } return(SQRT_Result); } /**************************************************************************************/ /* Function: CalculateDirection * Usage: CalculateDirection(signed int Power, signed int x_accel) * --------------------------------------------------------------- * Takes newly found power and x value, and just takes their quotient (x/power) to get * an approximate direction. */ signed int CalculateDirection(signed int Power, signed int x_accel){ signed int Direction; Power = ((Power > 0) ? Power : - Power); if(Power == 0){ Direction = 0; }else{ Direction = ((127*x_accel)/Power); } return(Direction); } /**************************************************************************************/