// write to scratchpad of a SINGLE sensor
void writeSP(int brdNo) {
int i;
int get[8];
ow_reset();
write_byte(0xCC);
write_byte(0x4E);
write_byte(brdNo);
write_byte(0x0);
write_byte(0x7F);
while(read_bit() == 0);
ow_reset();
write_byte(0xCC);
write_byte(0x48);
uDelay(120);
ow_reset();
write_byte(0xCC);
uDelay(120);
write_byte(0xBE);
for (i = 0 ; i < 9 ; i++){
get[i] = read_byte();
}
if(get[2] != brdNo || get[3] != 0 || get[4] != 0x7F) {
UART_printf("BAD CONFIGURATION?\n");
}
}
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Initialization
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
void
OLED_Init() {
// Ports are output
OLED_CSd = PD_OUTPUT;
OLED_CS = 1; // 1 on CS means disable
OLED_DATACOMMANDd = PD_OUTPUT;
OLED_RESETd = PD_OUTPUT; // Reset pin
OLED_ENABLEd = PD_OUTPUT; // VCC
OLED_ENABLE = 0;
OLED_RESET = 1;
// Reset sequence
for (int i = 0; i < 200; i++) {
uDelay(200);
}
unsigned char i;
OLED_RESET = 0;
for (i = 0; i < 200; i++) {
uDelay(200);
}
OLED_RESET = 1;
for (i = 0; i < 200; i++) {
uDelay(200);
}
// Enable VCC 12,5V
uDelay(100);
// Looong delay of 200uS
for (int i = 0; i < 200*20; i++) {
uDelay(200);
}
OLED_Set_Command_Lock(0x12); // Unlock Basic Commands (0x12/0x16)
OLED_Set_Display_On_Off(0x00); // Display Off (0x00/0x01)
OLED_Set_Display_Clock(0xD0); // Set Clock as 80 Frames/Sec
OLED_Set_Multiplex_Ratio(0x3F); // 1/64 Duty (0x0F~0x3F)
OLED_Set_Display_Offset(0x00); // OLED_Shift Mapping RAM Counter (0x00~0x3F)
OLED_Set_Start_Line(0x00); // Set Mapping RAM Display Start Line (0x00~0x7F)
OLED_Set_Remap_Format(0x14); // Set Horizontal Address Increment
// Column Address 0 Mapped to SEG0
// Disable Nibble Remap
// Scan from COM[N-1] to COM0
// Disable COM Split Odd Even
// Enable Dual COM Line Mode
OLED_Set_GPIO(0x00); // Disable GPIO Pins Input
OLED_Set_Function_Selection(0x01); // Enable Internal VDD Regulator
OLED_Set_Display_Enhancement_A(0xA0, 0xFD); // Enable External VSL
// Set Low Gray Scale Enhancement
OLED_Set_Contrast_Current(0xDF); // Set Segment Output Current
OLED_Set_Master_Current(OLED_Brightness); // Set Scale Factor of Segment Output Current Control
OLED_Set_Gray_Scale_Table(); // Set Pulse Width for Gray Scale Table
OLED_Set_Phase_Length(0xE8); // Set Phase 1 as 5 Clocks & Phase 2 as 14 Clocks
OLED_Set_Display_Enhancement_B(0x20); // Enhance Driving Scheme Capability (0x00/0x20)
OLED_Set_Precharge_Voltage(0x1F); // Set Pre-Charge Voltage Level as 0.60*VCC
OLED_Set_Precharge_Period(0x08); // Set Second Pre-Charge Period as 8 Clocks
OLED_Set_VCOMH(0x07); // Set Common Pins Deselect Voltage Level as 0.86*VCC
OLED_Set_Display_Mode(0x02); // Normal Display Mode (0x00/0x01/0x02/0x03)
OLED_Set_Partial_Display(0x01, 0x00, 0x00); // Disable Partial Display
OLED_Fill_RAM(0x00); // Clear Screen
OLED_Set_Display_On_Off(0x01); // Display On (0x00/0x01)
}
void
SPI3_send_cmd(unsigned char c) {
OLED_DATACOMMAND = 0;
uDelay(SPI_DELAY);
u3tb = c;
uDelay(12);
}
void
SPI0_send_data(unsigned char c) {
while (ti_u0c1 == 0) {
NOP();
}
uDelay(200);
u0tb = c;
uDelay(200);
}
/* write_bit() - writes a bit to the 1-wire bus */
void write_bit(char bit) {
// set bus to low and write bit out to bus
GPIO_SetValue(TX_EN_PRT, TX_EN_PIN, 1);
GPIO_SetValue(TX_PRT, TX_PIN, 0);
uDelay(1);
GPIO_SetValue(TX_PRT, TX_PIN, bit);
uDelay(100);
GPIO_SetValue(TX_PRT, TX_PIN, 1);
}
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Fade In (Full Screen)
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
void
OLED_Fade_In() {
unsigned char i;
OLED_Set_Display_On_Off(0x01);
for (i = 0; i < (OLED_Brightness + 1); i++) {
OLED_Set_Master_Current(i);
uDelay(200);
uDelay(200);
uDelay(200);
}
}
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Fade Out (Full Screen)
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
void
OLED_Fade_Out() {
unsigned char i;
for (i = (OLED_Brightness + 1); i > 0; i--) {
OLED_Set_Master_Current(i - 1);
uDelay(200);
uDelay(200);
uDelay(200);
}
OLED_Set_Display_On_Off(0x00);
}
/* tempSense() - returns temperature (in celcius) of device ROM
* to specify which device is read, pass in the index of the device (ROM)
* in the foundROM array
*
* FINDING THE BOARD NUMBER OF A TEMPERATURE READ
* The board number is returned as the 16 MSB of tempSense()'s return
* value
* i.e. for temperature = tempSense(x) | 0x7F
* i.e. for board number = tempSense(x) >> 16
*/
int tempSense(int ROM) {
char get[10];
char temp_lsb, temp_msb;
int k, i;
ow_reset();
write_byte(0x55);
uDelay(10);
for(i = 0 ; i < 8 ; i++) {
write_byte(FoundROM[ROM][i]);
}
write_byte(0x44); // Start Conversion
uDelay(102);
ow_reset();
write_byte(0x55);
uDelay(10);
for(i = 0 ; i < 8 ; i++) {
write_byte(FoundROM[ROM][i]);
}
// Read Scratch Pad
write_byte(0xBE);
for (k=0;k<9;k++){
get[k]=read_byte();
}
temp_msb = get[1]; // Sign byte + lsbit
temp_lsb = get[0]; // Temp data plus lsb
// shift to get whole degree
if (temp_msb <= 0x80)
temp_lsb = (temp_lsb/2);
// mask all but the sign bit
temp_msb = temp_msb & 0x80;
// twos complement
if (temp_msb >= 0x80)
temp_lsb = (~temp_lsb)+1;
// shift to get whole degree
if (temp_msb >= 0x80)
temp_lsb = (temp_lsb/2);
// add sign bit
if (temp_msb >= 0x80)
temp_lsb = ((-1)*temp_lsb);
return (int)temp_lsb | get[2] << 16;
}
short unsigned
SPI6_receive(void) {
short unsigned r;
SPI6_send(0xFF);
uDelay(SPI_DELAY);
uDelay(SPI_DELAY);
while (ri_u6c1 == 0) {
NOP();
}
r=u6rb;
ri_u6c1=0;
return r;
}
unsigned short
SPI0_receive(void) {
unsigned short r;
uDelay(200);
while (ri_u0c1 == 0) {
NOP();
}
r=u0rb;
ri_u0c1=0;
uDelay(200);
return r;
}
/* ow_reset() - performs a 1-wire reset operation
* returns 0 if a 1-wire device is present
*/
unsigned char ow_reset() {
int8_t presence;
GPIO_SetValue(TX_EN_PRT, TX_EN_PIN, 1); // enable Tx
GPIO_SetValue(TX_PRT, TX_PIN, 0); // send value
uDelay(480);
GPIO_SetValue(TX_EN_PRT, TX_EN_PIN, 0); // disable Tx
uDelay(70);
presence = GPIO_GetValue(RX_PRT, RX_PIN);
// device is now reset
uDelay(420);
return presence;
}
void dw1000_reset () {
GPIO_InitTypeDef GPIO_InitStructure;
// Make the reset pin output
GPIO_InitStructure.GPIO_Pin = DW_RESET_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(DW_RESET_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = DW_WAKEUP_PIN;
GPIO_Init(DW_WAKEUP_PORT, &GPIO_InitStructure);
//reset logic
// Set it high
DW_RESET_PORT->BSRR = DW_RESET_PIN;
DW_RESET_PORT->BRR = DW_RESET_PIN;
// Wait for ~100ms
uDelay(100000);
DW_RESET_PORT->BSRR = DW_RESET_PIN;
//wakeup logic?
DW_WAKEUP_PORT->BSRR = DW_WAKEUP_PIN;
// Set it back to an input
/*GPIO_InitStructure.GPIO_Pin = DW_RESET_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(DW_RESET_PORT, &GPIO_InitStructure);*/
}
void
SPI7_send(unsigned short c) {
while (ti_u7c1 == 0)
NOP();
uDelay(SPI_DELAY);
ti_u7c1=0;
u7tb = c;
}
void
Delay(unsigned char n)
{
unsigned char i, j, k;
for (k = 0; k < n; k++)
for (i = 0; i < 100; i++)
for (j = 0; j < 100; j++)
uDelay(100);
}
/* write_byte() - writes multiple bytes to 1-wire bus */
void write_byte(char val) {
int i = 0;
for(i = 0 ; i < 8 ; i++) {
write_bit((val >> i) & 0x01);
}
uDelay(120);
}
void
SPI6_send(unsigned short c) {
while (ti_u6c1 == 0) {
NOP();
}
uDelay(SPI_DELAY);
ti_u6c1=0;
u6tb = c;
}
/* read_bit() - reads a bit from the 1-wire bus
* returns the bit read
*/
unsigned char read_bit() {
// enable Tx
GPIO_SetValue(TX_EN_PRT, TX_EN_PIN, 1);
// pull low and let device pull up/down
GPIO_SetValue(TX_PRT, TX_PIN, 0);
GPIO_SetValue(TX_EN_PRT, TX_EN_PIN, 0);
uDelay(15);
return GPIO_GetValue(RX_PRT, RX_PIN);
}
int DelayInit()
{
unsigned long ticks, loopBit;
int precision = 8;
sti();
KePrint(KERN_INFO "CPU: calibrating delay loop\n");
uDelayLoops = (1 << 12);
while ((uDelayLoops <<= 1))
{
ticks = currTicks;
while (ticks == currTicks);
ticks = currTicks;
uDelay(uDelayLoops);
ticks = currTicks - ticks;
if (ticks)
break;
}
uDelayLoops >>= 1;
loopBit = uDelayLoops;
while (precision-- && (loopBit >>= 1))
{
uDelayLoops |= loopBit;
ticks = currTicks;
while (ticks == currTicks);
ticks = currTicks;
uDelay(uDelayLoops);
if (ticks != currTicks)
/* Longer than one tick? */
uDelayLoops &= ~loopBit;
}
return 0;
}
/* read_byte() - reads a byte off the 1-wire bus */
int8_t read_byte() {
int retVal = 0;
int i;
for(i = 0 ; i < 8 ; i++) {
if(read_bit()) {
retVal |= 0x01 << i;
}
uDelay(120);
}
return retVal;
}
void fillScreen(uint16_t color)
{
goHome();
uint32_t i;
i = 320;
i *= 240;
while (i--)
{
uDelay(0);
writeData(color);
}
}
//갯수에 맞게 받는 이유 : 통신에러가 나오면 Length가 틀릴 가능성이 무척 높기 때문
byte rx_Packet(byte bRxLength){
unsigned long ulCounter, ulTimeLimit;
byte bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255) ulTimeLimit = RX_TIMEOUT_COUNT1;
else ulTimeLimit = RX_TIMEOUT_COUNT2;
for(bCount = 0; bCount < bRxLength; bCount++)
{
ulCounter = 0;
while(gbDXLReadPointer == gbDXLWritePointer)
{
nDelay(NANO_TIME_DELAY); // porting ydh
if(ulCounter++ > ulTimeLimit)
{
bTimeout = 1;
break;
}
uDelay(0); //porting ydh added
}
if(bTimeout) break;
gbpRxBuffer[bCount] = gbpDXLDataBuffer[gbDXLReadPointer++];
//TxDStringC("gbpRxBuffer = ");TxDHex8C(gbpRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( gbpTxBuffer[2] != BROADCAST_ID )
{
if(bTimeout && bRxLength != 255)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDString("Rx Timeout");
TxDByte(bLength);
#endif
clearBuffer256();
//return 0;
}
if(bLength > 3) //checking available length.
{
if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
clearBuffer256();
return 0;
}
if(gbpRxBuffer[2] != gbpTxBuffer[2] )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
clearBuffer256();
return 0;
}
if(gbpRxBuffer[3] != bLength-4)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
clearBuffer256();
return 0;
}
for(bCount = 2; bCount < bLength; bCount++) bChecksum += gbpRxBuffer[bCount];
if(bChecksum != 0xff)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[RxChksum Error]");
#endif
clearBuffer256();
return 0;
}
}
}
return bLength;
}
byte Dynamixel::rxPacket(byte bRxLength){
unsigned long ulCounter, ulTimeLimit;
byte bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255)
ulTimeLimit = RX_TIMEOUT_COUNT1;
else
ulTimeLimit = RX_TIMEOUT_COUNT2;
for(bCount = 0; bCount < bRxLength; bCount++)
{
ulCounter = 0;
while(mDxlDevice->read_pointer == mDxlDevice->write_pointer)
{
nDelay(NANO_TIME_DELAY); //[ROBOTIS] porting ydh
if(ulCounter++ > ulTimeLimit)
{
bTimeout = 1;
//TxDStringC("Timeout\r\n");
break;
}
uDelay(0); //[ROBOTIS] porting ydh added
}
if(bTimeout) break;
mRxBuffer[bCount] = mDxlDevice->data_buffer[mDxlDevice->read_pointer++]; // get packet data from USART device
//TxDStringC("mRxBuffer = ");TxDHex8C(mRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( mTxBuffer[2] != BROADCAST_ID )
{
if(bTimeout && bRxLength != 255)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Rx Timeout");
TxDByteC(bLength);
#endif
mDXLtxrxStatus |= (1<<COMM_RXTIMEOUT);
clearBuffer();
//TxDStringC("Rx Timeout");
return 0;
}
if(bLength > 3) //checking available length.
{
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
clearBuffer();
return 0;
}
if(mRxBuffer[2] != mTxBuffer[2] )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
clearBuffer();
return 0;
}
if(mRxBuffer[3] != bLength-4)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
clearBuffer();
return 0;
}
for(bCount = 2; bCount < bLength; bCount++){
bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare
}
if(bChecksum != 0xff)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[RxChksum Error]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
clearBuffer();
return 0;
}
}
}
return bLength;
}
byte Dynamixel::rxPacket(int bRxLength) {
unsigned long ulCounter, ulTimeLimit;
word bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255 || bRxLength == 0xffff) //2014-04-03
ulTimeLimit = RX_TIMEOUT_COUNT1;
else
ulTimeLimit = RX_TIMEOUT_COUNT2;
for(bCount = 0; bCount < bRxLength; bCount++)
{
ulCounter = 0;
while(mDxlDevice->read_pointer == mDxlDevice->write_pointer)
{
nDelay(NANO_TIME_DELAY); //[ROBOTIS] porting ydh
if(ulCounter++ > ulTimeLimit)
{
bTimeout = 1;
//TxDStringC("Timeout\r\n");
break;
}
uDelay(0); //[ROBOTIS] porting ydh added //if exist DXL 1.0 -> ok DXL 2.0 -> ok, if not exist 1.0 not ok, 2.0 ok
}
if(bTimeout) break;
mRxBuffer[bCount] = mDxlDevice->data_buffer[mDxlDevice->read_pointer++ & DXL_RX_BUF_SIZE]; // get packet data from USART device
//TxDStringC("mRxBuffer = ");TxDHex8C(mRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( mTxBuffer[mPktIdIndex] != BROADCAST_ID )
{
if(bTimeout && bRxLength != 255)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Rx Timeout");
TxDByteC(bLength);
#endif
mDXLtxrxStatus |= (1<<COMM_RXTIMEOUT);
clearBuffer();
//TxDStringC("Rx Timeout");
return 0;
}
if(bLength > 3) //checking available length.
{
/*if(mPacketType == 1){
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
else{
for(bCount = 2; bCount < bLength; bCount++){
bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare
}
if(bChecksum != 0xff) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
return 0;
}
}else{
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
else{
bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]);
if(update_crc(0, mRxBuffer, bRxLength-2) != bChecksum) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
return 0;
}
}
*/
if(mPacketType == 1) { //Dxl 1.0 header check
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) {
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
clearBuffer();
return 0;
}
} else { // Dxl 2.0 header check
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
clearBuffer();
return 0;
}
}
if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) //id check
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
clearBuffer();
return 0;
}
if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) // status packet length check
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
clearBuffer();
return 0;
}
if(mPacketType == 1 && mRxBuffer[mPktErrorIndex] != 0) { //140512 shin
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
for(bTryCount = 0; bTryCount<= 6; bTryCount++) {
if((mRxBuffer[mPktErrorIndex] & (1<<bTryCount)) == TRUE) {
switch(bTryCount) {
case 0:
TxDStringC("InputVoltage Error");
break;
case 1:
TxDStringC("Angle Limit Error");
break;
case 2:
TxDStringC("Overheating Error");
break;
case 3:
TxDStringC("Range Error");
break;
case 4:
TxDStringC("Checksum Error");
break;
case 5:
TxDStringC("Overload Error");
break;
case 6:
TxDStringC("Instruction Error");
break;
}
}
}
#endif
} else { //140512 shin
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
for(bTryCount = 1; bTryCount<= 7; bTryCount++) {
if((mRxBuffer[mPktErrorIndex]) == bTryCount) {
switch(bTryCount) {
case 1:
TxDStringC("Result Fail");
break;
case 2:
TxDStringC("Instruction Error");
break;
case 3:
TxDStringC("CRC Error");
break;
case 4:
TxDStringC("DataRange Error");
break;
case 5:
TxDStringC("DataLength Error");
break;
case 6:
TxDStringC("DataLimit Error");
break;
case 7:
TxDStringC("Accrss Error");
break;
}
}
}
#endif
}
if(mPacketType == 1) { // Dxl 1.0 checksum
for(bCount = 2; bCount < bLength; bCount++) {
bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare
}
if(bChecksum != 0xff)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[RxChksum Error]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
clearBuffer();
return 0;
}
} else { // Dxl 2.0 checksum
bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]);
if(update_crc(0, mRxBuffer, bRxLength-2) == bChecksum) { // -2 : except CRC16
return bLength;
}
else {
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("CRC-16 Error\r\n");
#endif
return 0;
}
}//end of checksum
}//(bLength > 3)
}//end of Rx status packet check
return bLength;
}
// NEXT
// The Next function searches for the next device on the 1-wire bus. If
// there are no more devices on the 1-wire then false is returned.
//
unsigned char Next(void) {
unsigned char m = 1; // ROM Bit index
unsigned char n = 0; // ROM Byte index
unsigned char k = 1; // bit mask
unsigned char x = 0;
unsigned char discrepMarker = 0; // discrepancy marker
unsigned char g; // Output bit
unsigned char nxt = FALSE; // return value
int flag;
// reset the 1-wire
flag = ow_reset();
// reset the dowcrc
dowcrc = 0;
// no parts -> return false
if(flag||doneFlag) {
// reset the search
lastDiscrep = 0;
return FALSE;
}
// send SearchROM command
write_byte(0xF0);
// for all eight bytes
do {
x = 0;
if(read_bit()==1) x = 2;
uDelay(120);
if(read_bit()==1) x |= 1;
uDelay(120);
if(x ==3) break;
else {
// all devices coupled have 0 or 1
if(x>0)
// bit write value for search
g = x>>1;
else {
// if this discrepancy is before the last
// discrepancy on a previous Next then pick
// the same as last time
if(m<lastDiscrep) g = ((ROM[n]&k)>0);
else g = (m==lastDiscrep);
// if equal to last pick 1
// if not then pick 0
// if 0 was picked then record
// position with mask k
if (g==0) discrepMarker = m;
}
// isolate bit in ROM[n] with mask k
if(g==1) ROM[n] |= k;
else ROM[n] &= ~k;
// ROM search write
write_bit(g);
// increment bit counter m
m++;
// and shift the bit mask k
k = k<<1;
// if the mask is 0 then go to new ROM
if(k==0) {
// accumulate the CRC
ow_crc(ROM[n]);
// byte n and reset mask
n++;
k++;
}
}
} while(n<8);
// if search was unsuccessful then
// reset the last discrepancy to 0
if(m<65||dowcrc) lastDiscrep=0;
else {
// search was successful, so set lastDiscrep,
// lastOne, nxt;
lastDiscrep = discrepMarker;
doneFlag = (lastDiscrep==0);
// indicates search is not complete yet, more parts remain
nxt = TRUE;
}
return nxt;
}
// First (generic) init of the DW1000
dw1000_err_e dw1000_init () {
// Do the STM setup that initializes pin and peripherals and whatnot.
if (!_stm_dw1000_interface_setup) {
setup();
}
// Reset the dw1000...for some reason
dw1000_reset();
uDelay(100);
// Make sure we can talk to the DW1000
uint32_t devID;
devID = dwt_readdevid();
if (devID != DWT_DEVICE_ID) {
//if we can't talk to dw1000, return with an error
uDelay(1000);
return DW1000_COMM_ERR;
}
GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_SET);
uDelay(1000);
GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_RESET);
// Choose antenna 0 as a default
dw1000_choose_antenna(0);
// Initialize the dw1000 hardware
uint32_t err;
err = dwt_initialise(DWT_LOADUCODE |
DWT_LOADLDO |
DWT_LOADTXCONFIG |
DWT_LOADXTALTRIM);
if (err != DWT_SUCCESS) {
return DW1000_COMM_ERR;
}
// Configure interrupts and callbacks
dwt_setinterrupt(0xFFFFFFFF, 0);
dwt_setinterrupt(DWT_INT_TFRS |
DWT_INT_RFCG |
DWT_INT_RPHE |
DWT_INT_RFCE |
DWT_INT_RFSL |
DWT_INT_RFTO |
DWT_INT_RXPTO |
DWT_INT_SFDT |
DWT_INT_ARFE, 1);
dwt_setcallbacks(txcallback, rxcallback);
// Set the parameters of ranging and channel and whatnot
global_ranging_config.chan = 2;
global_ranging_config.prf = DWT_PRF_64M;
global_ranging_config.txPreambLength = DWT_PLEN_64;
global_ranging_config.rxPAC = DWT_PAC8;
global_ranging_config.txCode = 9; // preamble code
global_ranging_config.rxCode = 9; // preamble code
global_ranging_config.nsSFD = 0;
global_ranging_config.dataRate = DWT_BR_6M8;
global_ranging_config.phrMode = DWT_PHRMODE_EXT; //Enable extended PHR mode (up to 1024-byte packets)
global_ranging_config.smartPowerEn = 1;
global_ranging_config.sfdTO = 64+8+1;//(1025 + 64 - 32);
#if DW1000_USE_OTP
dwt_configure(&global_ranging_config, (DWT_LOADANTDLY | DWT_LOADXTALTRIM));
#else
dwt_configure(&global_ranging_config, 0);
#endif
dwt_setsmarttxpower(global_ranging_config.smartPowerEn);
// Configure TX power based on the channel used
global_tx_config.PGdly = pgDelay[global_ranging_config.chan];
global_tx_config.power = txPower[global_ranging_config.chan];
dwt_configuretxrf(&global_tx_config);
// Need to set some radio properties. Ideally these would come from the
// OTP memory on the DW1000
#if DW1000_USE_OTP == 0
// This defaults to 8. Don't know why.
dwt_xtaltrim(8);
// Antenna delay we don't really care about so we just use 0
dwt_setrxantennadelay(DW1000_ANTENNA_DELAY_RX);
dwt_settxantennadelay(DW1000_ANTENNA_DELAY_TX);
#endif
return DW1000_NO_ERR;
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
uint8_t currentLED=0,idx;
uint16_t xx,yy;
uint16_t testWord;
uint8_t shifter=0;
/* Initialize LEDs on STM32F4-Discovery --------------------*/
__IO uint32_t i = 0;
uint8_t buf[255];
uint8_t len;
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED3);
// You can use these for LEDs if you use the upper 8 bits of the 16b FSMC bus to talk to the ILI9238.
// STM_EVAL_LEDInit(LED5); //GPIOD 14 -> FSMC D0. Do not use for LED, we need them to talk to the ILI9238
// STM_EVAL_LEDInit(LED6); //GPIOD 14 -> FSMC D1.
// flash the LEDs in a circle to test delayMillis(uint32_t timedelay)
STM_EVAL_LEDToggle(currentLED);
for (idx=0;idx<8;idx++)
{
STM_EVAL_LEDToggle(currentLED);
currentLED=(currentLED+1)%2;
STM_EVAL_LEDToggle(currentLED);
delayMillis(250);
}
//
// USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
// init the printf
init_printf(0,tft_putc);
// init_tft_printf(NULL,tft_putc);
// Get the 32F4 ready to talk to the TFTLCD using FSMC
initGPIO();
initFSMC();
uDelay(1000); // probably don't need this
reset();
initDisplay();
// ** Do the adafruit Demo **
// fillScreen(BLACK);
// setCursor(0, 0);
// setTextColor(CYAN);
// setTextSize(1);
// setRotation(1);
// tft_printf("Please connect to virtual COM port...");
delayMillis(2000);
testlines(CYAN);
delayMillis(2500);
testfastlines(RED, BLUE);
delayMillis(2500);
testdrawrects(GREEN);
delayMillis(2500);
testfillrects(YELLOW, MAGENTA);
delayMillis(2500);
fillScreen(BLACK);
testfillcircles(10, MAGENTA);
testdrawcircles(10, WHITE);
delayMillis(2500);
testtriangles();
delayMillis(2500);
testfilltriangles();
delayMillis(2500);
testRoundRect();
delayMillis(2500);
testFillRoundRect();
delayMillis(2500);
fillScreen(GREEN);
delayMillis(2500);
// fsmcData = 0x60020000; // sets a16
// fsmcRegister = 0x60000000; // clears a16
// printf("fsmcData:\t0x%08X\r\n",fsmcData);
// printf("fsmcRegister:\t0x%08X\r\n",fsmcRegister);
// fillScreen(BLACK);
// setCursor(0, 20);
// setTextColor(color);
// setTextSize(1);
// write("Hello World!");
// setTextSize(2);
// write(1234.56);
// setTextSize(3);
//
// println(0xDEADBEEF, HEX);
// printf("0xFF00>>8 0x%04X\r\n",0xff00>>8);
// VCP_send_str(&buf[0]);
// printf("SysTick_Config(SystemCoreClock/1000)\t %d\r\n",SysTick_Config(SystemCoreClock/1000));
// millisecondCounter=0;
// for (idx=0;idx<100;idx++)
// {
// printf("millisecondCounter:\t%d",millisecondCounter);
// }
// void delayMillis(uint32_t millis)
// {
// uint32_t target;
//
// target=millisecondCounter+millis;
// while(millisecondCounter<target);
// }
// From stm32f4_discovery.h:
// typedef enum
// {
// LED4 = 0,
// LED3 = 1,
// LED5 = 2,
// LED6 = 3
// } Led_TypeDef;
while(1)
{
for (idx=0;idx<8;idx++)
{
setRotation(idx%4);
testtext(RED);
delayMillis(1500);
}
}
}
void usleep (uint32_t u) {
uDelay(u);
}
void mDelay (const UINT32 msec)
{
uDelay(msec * 1000);
}
void writeRegister(uint16_t addr, uint16_t data)
{
writeCommand(addr);
uDelay(10);
writeData(data);
}
void delayMillis(uint32_t millis)
{
uDelay(millis * 1000);
}
