int ReadSR( void)
{
// Check the Serial EEPROM status register
int i;
CSEE = 0; // select the Serial EEPROM
WriteSPI2( SEE_RDSR); // send a READ STATUS COMMAND
i = WriteSPI2( 0); // send/receive
CSEE = 1; // deselect to terminate command
return i;
} //ReadSR
void ByteWriteSPI2(unsigned char Add, unsigned char Data )
{
DS_CS_2 = 0; // Select Device
WriteSPI2 ( Add ); // write address byte to EEPROM
WriteSPI2 ( Data ); // Write Byte to device
DS_CS_2 = 1; // Deselect device and initiate Write
SPI2STATbits.SPITBF = 0; // Transmit started, SPIxTXB is empty
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
void WriteEnable( void)
{
// send a Write Enable command
CSEE = 0; // select the Serial EEPROM
WriteSPI2( SEE_WEN); // write enable command
CSEE = 1; // deselect to complete the command
}//WriteEnable
void iWriteSEE( long address, int data)
{ // write a 16-bit value starting at an even address
// wait until any work in progress is completed
while ( ReadSR() & 0x1); // check the WIP flag
// Set the Write Enable Latch
WriteEnable();
// perform a 16-bit write sequence (2 byte page write)
CSEE = 0; // select the Serial EEPROM
WriteSPI2( SEE_WRITE); // write command
WriteSPI2( address>>8); // address MSB first
WriteSPI2( address & 0xfe); // address LSB (word aligned)
WriteSPI2( data >>8); // send msb
WriteSPI2( data & 0xff); // send lsb
CSEE = 1;
}//iWriteSEE
int iReadSEE( long address)
{ // read a 16-bit value starting at an even address
int lsb, msb;
// wait until any work in progress is completed
while ( ReadSR() & 0x1); // check the WIP flag
// perform a 16-bit read sequence (two byte sequential read)
CSEE = 0; // select the Serial EEPROM
WriteSPI2( SEE_READ); // read command
WriteSPI2( address>>8); // address MSB first
WriteSPI2( address & 0xfe); // address LSB (word aligned)
msb = WriteSPI2( 0); // send dummy, read msb
lsb = WriteSPI2( 0); // send dummy, read lsb
CSEE = 1;
return ( (msb<<8)+ lsb);
}//iReadSEE
void ByteReadSPI2(unsigned char Add, unsigned char *rdptr, unsigned char length )
{
DS_CS_2 = 0; // Select Device
WriteSPI2( Add ); // WRITE word address to EEPROM
getsSPI2( rdptr, length ); // read in multiple bytes
DS_CS_2 = 1; // Deselect Device
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
void BurstWriteSPI2(unsigned char Address, unsigned char data0, unsigned char data1,
unsigned char data2, unsigned char data3,
unsigned char data4, unsigned char data5,
unsigned char data6, unsigned char data7)
{
DS_CS_2 = 0; // Select Device
WriteSPI2 ( Address ); // write address byte to EEPROM
WriteSPI2 ( data0 ); // Write Byte to device
WriteSPI2 ( data1 ); // Write Byte to device
WriteSPI2 ( data2 ); // Write Byte to device
WriteSPI2 ( data3 ); // Write Byte to device
WriteSPI2 ( data4 ); // Write Byte to device
WriteSPI2 ( data5 ); // Write Byte to device
WriteSPI2 ( data6 ); // Write Byte to device
WriteSPI2 ( data7 ); // Write Byte to device
DS_CS_2 = 1; // Deselect device and initiate Write
SPI2STATbits.SPITBF = 0;
__delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select
//@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles)
}
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
unsigned int *chptr; // For sending a float value
unsigned int command; // For sending commands to Axis Converter
double gyroXrate, gyroYrate;
char nRFstatus;
unsigned char rfCommands[32]; // Commands received from remote
unsigned char nRFregisters[10];
int iii;
long long xSpeedFilterSum = 0;
long long ySpeedFilterSum = 0;
// Toggle LED
_LATA4 = 1;
/** Read sensors ******************************************************/
readSensorData();
__delay32(1000); // Without this delay, the I2C command acts funny...
accXangle = ((atan2(accel[0], accel[2]))*RAD_TO_DEG)-AngleOffset[0];
accYangle = ((atan2(accel[1], accel[2]))*RAD_TO_DEG)-AngleOffset[1];
gyroXrate = ((double)gyro[0]-(double)GyroOffset[0])/131;
gyroYrate = -(((double)gyro[1]-(double)GyroOffset[1])/131);
/** Feedback Loop *****************************************************/
compAngleX = (compFilterGyro*(compAngleX+(gyroYrate*(double)SAMPLE_TIME)))+(compFilterAccel*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (compFilterGyro*(compAngleY+(gyroXrate*(double)SAMPLE_TIME)))+(compFilterAccel*accYangle);
if (enableSteppers == 1)
{
iCompAngleY += compAngleY;
iCompAngleX += compAngleX;
}
else
{
iCompAngleY = 0.0;
iCompAngleX = 0.0;
}
ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN+gyroXrate*(double)iGAIN);
xSpeed = -((compAngleX-sGAIN)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN+gyroYrate*(double)iGAIN);
xSpeedFilter[SpeedFilterCounter] = xSpeed;
ySpeedFilter[SpeedFilterCounter] = ySpeed;
for(iii = 0; iii<=SpeedFilterLength;iii++)
{
xSpeedFilterSum += xSpeedFilter[iii];
ySpeedFilterSum += ySpeedFilter[iii];
}
xSpeed = (long long)xSpeedFilterSum/(SpeedFilterLength+1);
ySpeed = (long long)ySpeedFilterSum/(SpeedFilterLength+1);
// ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN);
// xSpeed = -((compAngleX)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN);
// if (xSpeed<0)
// xSpeed = -(xSpeed*xSpeed);
// else
// xSpeed = xSpeed*xSpeed;
// if (ySpeed<0)
// ySpeed = -(ySpeed*ySpeed);
// else
// ySpeed = ySpeed*ySpeed;
compAngleXlast = compAngleX;
compAngleYlast = compAngleY;
// xSpeed = gyroXrate *1000;
// ySpeed = gyroYrate *1000;
/** Build command string **********************************************/
command = 0b1000000000000000;
if (enableSteppers == 1)
command = command | 0b0100000000000000;
/* Send data to Axis Controller ***************************************/
chptr = (unsigned char *) ∠ // For sending a float value
CS_Axis = 0;
/* Command: MSB
* 15: 0 = NO Speed Values
* 14: 1 = Enable Stepper Driver
* 13:
*/
WriteSPI2(command); // Command: MSB
WriteSPI2(xSpeed); // Send x-velocity vector
WriteSPI2(ySpeed); // Send y-velocity vector
WriteSPI2(*chptr++); // Sending first part of float value
WriteSPI2(*chptr); // Sending second part of float value
CS_Axis = 1;
/** Prepare telemetry *************************************************/
setRawData2string();
/**** Telemetry string format for command=0x42 ************************/
/** command, axh, axl, ayh, ayl, azh, azl, th, tl, 9byte **/
/** gxh, gxl, gyh, gyl, gzh, gzl, cxh, cxl, cyh, cyl, czh, czl,12byte**/
/** motorStatus 1byte**/
/** --Total 22byte **/
serString[16] = xSpeed & 0xFF; // Temporary sending additional telemetry
serString[15] = xSpeed >> 8;
serString[18] = ySpeed & 0xFF;
serString[17] = ySpeed >> 8;
// serString[18] = iax & 0xFF;
// serString[17] = iax >> 8;
// serString[20] = igy & 0xFF;
// serString[19] = igy >> 8;
serString[21] = enableSteppers;
SpeedFilterCounter++;
if (SpeedFilterCounter> SpeedFilterLength)
SpeedFilterCounter = 0;
/** Receive RF commands if any ********************************************/
// LDByteWriteSPI(0x20, 0b00001010);
nRFstatus = LDBytePollnRF();
for(iii=0; iii<10;iii++) // For debug
{
nRFregisters[iii] = readnRFbyte(iii);
}
if (nRFstatus & 0b01000000) // Data Ready RX FIFO
{
// _LATB4 = 1;
//// Delay10TCYx(100);
LDByteReadSPI(0x61, rfCommands, 21); // Read RX FIFO
LDByteWriteSPI(0x27 , 0b01000000); // Clear RX FIFO interrupt bit
//ByteWriteSPI(0xE2 , 0xFF); //Flush RX FIFO
if(rfCommands[0] == 0x82)
{
if(enableSteppers)
{
enableSteppers = 0;
buttonState = 0;
//_LATB4 = 0;
} else {
enableSteppers = 1;
buttonState = 2;
//_LATB4 = 1;
}
} else if (rfCommands[0] == 0x83)
{
_LATB4 = 1;
// Set the received values to variables
for(iii=1;iii<19;iii += 3){
if(rfCommands[iii]<9)
setValueCode[rfCommands[iii]] = (rfCommands[iii+1]<<8) + rfCommands[iii+2];
}
pGAIN = setValueCode[1];
dGAIN = setValueCode[2];
compFilterGyro = (float)setValueCode[4]/(float)1000;
compFilterAccel = 1.0 - compFilterGyro;
iGAIN = (float)setValueCode[3]/100.0;
//angle = (setValueCode[6]-180)* PI /180.0;
//SpeedFilterLength = setValueCode[5];
LDByteWriteI2C(0x00D0, 0x1A, setValueCode[5]); // 0.0 ms and disable FSYNC
sGAIN = (float)(setValueCode[6]-1000)/100.0;
}
// //LATDbits.LATD3 = 0;
//
}
// LDByteWriteSPI(0x20, 0b00001010); //
// LDByteWriteSPI(0x26, 0b00000010); // Set RC_CH to reset retransmit counter
LDCommandWriteSPI(0xE2); // Flush RX buffer (not during ACK)
/** Send telemetry ********************************************************/
// _LATB15 = 0; // Set CE low to stop receiving data
// LDByteWriteSPI(0x20 , 0b00001010); // Set to send mode
// __delay32(100);
LDByteWriteSPI(0x27, 0b00010000); // clear MAX_RT (max retries)
LDCommandWriteSPI(0xE1); // Flush TX buffer (tx buffer contains last failed transmission)
LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
sendnRFstring( serString, 22);
/** Set nRF to recive mode ************************************************/
// __delay32(1000);
// LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
// LDByteWriteSPI(0x20 , 0b00001011); // Set to receive mode
// _LATB15 = 1; // Set CE high to start receiving data
/* Done with interrupt ****************************************************/
_LATA4 = 0;
_T1IF = 0; // Clear Timer 1 interrupt flag
}
/*****************************************************************************
Function:
void DRV_SPI_TxRx(void)
Summary:
Transmits and receives SPI bytes
Description:
Transmits and receives N bytes of SPI data.
Precondition:
None
Parameters:
pTxBuf - pointer to SPI tx data
txLen - number of bytes to Tx
pRxBuf - pointer to where SPI rx data will be stored
rxLen - number of SPI rx bytes caller wants copied to p_rxBuf
Returns:
None
Remarks:
Will clock out the larger of txLen or rxLen, and pad if necessary.
*****************************************************************************/
void DRV_SPI_TxRx(SpiChannel chn,
uint8_t *pTxBuf,
uint16_t txLen,
uint8_t *pRxBuf,
uint16_t rxLen)
{
uint16_t byteCount;
uint16_t i;
uint8_t rxTrash;
/* total number of byte to clock is whichever is larger, txLen or rxLen */
byteCount = (txLen >= rxLen)?txLen:rxLen;
for (i = 0; i < byteCount; ++i)
{
/* if still have bytes to transmit from tx buffer */
if ((txLen > 0) && (pTxBuf != 0))
{
#if defined (__C32__)
SpiChnWriteC(chn, *pTxBuf++);
#elif defined (__C30__)
switch(chn)
{
case 1:
WriteSPI1(*pTxBuf++);
break;
case 2:
WriteSPI2(*pTxBuf++);
break;
}
#endif
--txLen;
}
/* else done writing bytes out from tx buffer */
else
{
#if defined (__C32__)
SpiChnWriteC(chn, 0); /* clock out a "don't care" byte */
#elif defined (__C30__)
switch(chn)
{
case 1:
WriteSPI1(0x00);
break;
case 2:
WriteSPI2(0x00);
break;
}
#endif
}
/* wait until tx/rx byte to completely clock out */
WaitForDataByte(chn);
/* if still have bytes to read into rx buffer */
if ((rxLen > 0) && (pRxBuf != 0))
{
#if defined (__C32__)
*pRxBuf++ = SpiChnReadC(chn);
#elif defined (__C30__)
switch(chn)
{
case 1:
*pRxBuf++ = ReadSPI1();
break;
case 2:
*pRxBuf++ = ReadSPI2();
break;
}
#endif
--rxLen;
}
/* else done reading bytes into rx buffer */
else
{
#if defined (__C32__)
rxTrash = SpiChnReadC(chn); /* read and throw away byte */
#elif defined (__C30__)
switch(chn)
{
case 1:
rxTrash = ReadSPI1();
break;
case 2:
rxTrash = ReadSPI2();
break;
}
#endif
}
} /* end for loop */
}
