Hello,
Now I'm testing DWM1000 module with wav play from SD card.
Library for DWM1000 is Decaduino,
https://www.irit.fr/~Adrien.Van-Den-Bossche/decaduino/
Wav play and Decaduino ranging, each works fine but when I combine the code, it crashes.
Hardware connection is
Dwm1000 SCK --> teensy3.2 pin14
Dwm1000 MOSI --> teensy3.2 pin14
Dwm1000 MISO --> teensy3.2 pin12
Dwm1000 CS --> teensy3.2 pin15
Dwm1000 IRQ --> teensy3.2 pin16
Dwm1000 RST--> teensy3.2 pin3
Here's complete code that I'm working on.
Any help regarding this issue will be highly appreciated since it's urgent project!
Best jun
Now I'm testing DWM1000 module with wav play from SD card.
Library for DWM1000 is Decaduino,
https://www.irit.fr/~Adrien.Van-Den-Bossche/decaduino/
Wav play and Decaduino ranging, each works fine but when I combine the code, it crashes.
Hardware connection is
Dwm1000 SCK --> teensy3.2 pin14
Dwm1000 MOSI --> teensy3.2 pin14
Dwm1000 MISO --> teensy3.2 pin12
Dwm1000 CS --> teensy3.2 pin15
Dwm1000 IRQ --> teensy3.2 pin16
Dwm1000 RST--> teensy3.2 pin3
Here's complete code that I'm working on.
Code:
/*Developing to Tag
Audio file play and check noise
Read 10times and get average
Desitination Address(anchor_adrees[]), my_short_address
anchor_address[4] = { 100, 101, 102, 103}
*/
#include <Audio.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
#include <Wire.h>
#include <DecaDuino.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
AudioPlaySdWav playWav1;
// Use one of these 3 output types: Digital I2S, Digital S/PDIF, or Analog DAC
AudioOutputI2S audioOutput;
//AudioOutputSPDIF audioOutput;
//AudioOutputAnalog audioOutput;
AudioConnection patchCord1(playWav1, 0, audioOutput, 0);
AudioConnection patchCord2(playWav1, 1, audioOutput, 1);
AudioControlSGTL5000 sgtl5000_1;
// Use these with the Teensy Audio Shield
#define SDCARD_CS_PIN 10
#define SDCARD_MOSI_PIN 7
#define SDCARD_SCK_PIN 14
#define my_pan_id 0
#define bat_pin 17
#define TIMEOUT 5 // ms
#define total_anchor_num 2 // for test
#define total_check_time 10 // number of ranging with anchor for averaging
#define bat_check_interval 1000
#define range_interval 1000
byte anchor_address[4] = { 100, 101, 102, 103};
byte anchor_number = 0;
byte channel = 1;
byte check_time = 0;
float max_distance[4] = {10.0, 10.0, 10.0, 10.0}; //maximum distance from anchor 1, 2, 3, 4
float min_distance[4] = {0, 0, 0, 0}; //minimum distance from anchor 1, 2, 3, 4
float average[4] = {0, 0, 0, 0};
float sum;
#define my_short_address 0
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
#define AIR_SPEED_OF_LIGHT 229702547.0
#define DW1000_TIMEBASE 15.65E-12
#define COEFF AIR_SPEED_OF_LIGHT*DW1000_TIMEBASE
#define X_CORRECTION 1.0000000
//#define Y_CORRECTION 0.230000000
#define Y_CORRECTION 0.000000000
#define TWR_ENGINE_STATE_INIT 1
#define TWR_ENGINE_STATE_WAIT_NEW_CYCLE 2
#define TWR_ENGINE_STATE_SEND_START 3
#define TWR_ENGINE_STATE_WAIT_SEND_START 4
#define TWR_ENGINE_STATE_MEMORISE_T1 5
#define TWR_ENGINE_STATE_WATCHDOG_FOR_ACK 6
#define TWR_ENGINE_STATE_RX_ON_FOR_ACK 7
#define TWR_ENGINE_STATE_WAIT_ACK 8
#define TWR_ENGINE_STATE_MEMORISE_T4 9
#define TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY 10
#define TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY 11
#define TWR_ENGINE_STATE_WAIT_DATA_REPLY 12
#define TWR_ENGINE_STATE_EXTRACT_T2_T3 13
#define TWR_ENGINE_STATE_DISPLAY 14
#define TWR_ENGINE_STATE_GET_SUM 15
#define TWR_ENGINE_STATE_CHECK_NUM_ERROR 16
#define TWR_ENGINE_STATE_DISPLAY 17
#define TWR_ENGINE_STATE_FINISH 18
#define TWR_ENGINE_STATE_UNABLE_TO_REACH_ANCHOR 19
#define TWR_ENGINE_STATE_CHECK_ONE_CYCLE 20
#define TWR_MSG_TYPE_UNKNOWN 0
#define TWR_MSG_TYPE_START 1
#define TWR_MSG_TYPE_ACK 2
#define TWR_MSG_TYPE_DATA_REPLY 3
int i;
int rxFrames;
uint64_t t1, t2, t3, t4;
int32_t tof;
DecaDuino decaduino;
uint8_t txData[128];
uint8_t rxData[128];
uint16_t rxLen;
int state;
int timeout;
float distance[4];
byte division_factor;
float bat_voltage;
int reading;
boolean ranging_flag = false;
byte num_ranging_error = 0; // how many errors amongst 10 readings
boolean ranging_error[4] = {false, false, false, false};
unsigned long last_time_check_range, last_time_check_bat, audio_timer= 0;
#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif
void setup() {
pinMode(13, OUTPUT);
pinMode(A3, INPUT);
// Audio connections require memory to work. For more
// detailed information, see the MemoryAndCpuUsage example
AudioMemory(40);
sgtl5000_1.enable();
sgtl5000_1.volume(0.5);
// use altenate pins for SPI
//https://www.pjrc.com/teensy/td_libs_SPI.html#altpins
SPI.setMOSI(SDCARD_MOSI_PIN);
SPI.setSCK(SDCARD_SCK_PIN);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE);
display.setCursor(0, 0);
if (!(SD.begin(SDCARD_CS_PIN))) {
// stop here, but print a message repetitively
while (1) {
display.println("Unable to access the SD card");
display.display();
delay(500);
}
}
if ( !decaduino.init() ) {
display.println("decaduino init failed");
display.print("TAG ");
display.print(my_short_address);
display.display();
while (1) {
digitalWrite(13, HIGH);
delay(50);
digitalWrite(13, LOW);
delay(50);
}
}
decaduino.setPanId(my_pan_id);
decaduino.setShortAddress(my_short_address);
decaduino.setChannel(channel);
display.println("decaduino A initialized");
display.print("PanID: ");
display.println(decaduino.getPanId()); // FFFF 65535
display.print("Short Address: ");
display.println(decaduino.getShortAddress());
display.print("Channel: ");
display.println(decaduino.getChannel());
display.display();
// Set RX buffer
decaduino.setRxBuffer(rxData, &rxLen);
state = TWR_ENGINE_STATE_INIT;
delay(2000);
}
void loop() {
byte from_address, destination_adress;
if (millis() > last_time_check_bat + bat_check_interval) {
reading = analogRead(A3);
//bat_voltage = 3.3 * reading / 1024.0;
//bat_voltage = 3.3 * 2 * reading / 1280.0;
bat_voltage = 3.13 * 2 * reading / 1024.0;
last_time_check_bat = millis();
}
// time to check all the anchors
if (ranging_flag == false && millis() > last_time_check_range + range_interval) {
anchor_number = 0; // anchor1
check_time = 0;
num_ranging_error = 0;
sum = 0;
state = TWR_ENGINE_STATE_INIT;
ranging_error[0] = false;
ranging_error[1] = false;
ranging_error[2] = false;
ranging_error[3] = false;
division_factor = total_check_time;
ranging_flag = true;
last_time_check_range = millis();
// blinkLED(1);
}
AudioNoInterrupts();
// check all the anchors and the the average
if (ranging_flag == true) {
// check Range with all the anchors
from_address = anchor_address[anchor_number]; // when receive
destination_adress = anchor_address[anchor_number]; // when send
getRangeFromAnchor();
}
AudioInterrupts();
// check the distance and play audio
average[0] = 4;
int playduration = 5000;
if( average[0] > 3 && millis() > (audio_timer + playduration)){
playFile("SDTEST1.WAV");
blinkLED(1);
audio_timer = millis();
}else if(average[0] <= 3 && millis()> (audio_timer + playduration)){
playFile("SDTEST2.WAV");
blinkLED(1);
audio_timer = millis();
}
}
void blinkLED(int times) {
for (int i = 0; i < times; i++) {
digitalWrite(13, HIGH);
delay(10);
digitalWrite(13, LOW);
delay(10);
}
}
void playFile(const char *filename)
{
//Serial.print("Playing file: ");
//Serial.println(filename);
// Start playing the file. This sketch continues to
// run while the file plays.
playWav1.play(filename);
// A brief delay for the library read WAV info
//delay(5);
/*
// Simply wait for the file to finish playing.
while (playWav1.isPlaying()) {
// uncomment these lines if you audio shield
// has the optional volume pot soldered
//float vol = analogRead(15);
//vol = vol / 1024;
// sgtl5000_1.volume(vol);
}
*/
}
void getRangeFromAnchor() {
switch (state) {
case TWR_ENGINE_STATE_INIT:
decaduino.plmeRxDisableRequest();
state = TWR_ENGINE_STATE_WAIT_NEW_CYCLE;
break;
case TWR_ENGINE_STATE_WAIT_NEW_CYCLE:
state = TWR_ENGINE_STATE_SEND_START;
break;
case TWR_ENGINE_STATE_SEND_START:
// destination_address, my_address,
txData[0] = anchor_address[anchor_number];
txData[1] = my_short_address;
txData[2] = TWR_MSG_TYPE_START; // 1
decaduino.pdDataRequest(txData, 3);
state = TWR_ENGINE_STATE_WAIT_SEND_START;
break;
case TWR_ENGINE_STATE_WAIT_SEND_START:
if ( decaduino.hasTxSucceeded() ) { ////////////////// sent data
state = TWR_ENGINE_STATE_MEMORISE_T1;
}
break;
case TWR_ENGINE_STATE_MEMORISE_T1:
t1 = decaduino.lastTxTimestamp;
state = TWR_ENGINE_STATE_WATCHDOG_FOR_ACK;
break;
case TWR_ENGINE_STATE_WATCHDOG_FOR_ACK:
timeout = millis() + TIMEOUT; // 20ms is the timeout
state = TWR_ENGINE_STATE_RX_ON_FOR_ACK;
break;
case TWR_ENGINE_STATE_RX_ON_FOR_ACK:
decaduino.plmeRxEnableRequest(); //Sets transceiver mode to receive mode.
state = TWR_ENGINE_STATE_WAIT_ACK;
break;
case TWR_ENGINE_STATE_WAIT_ACK:
if ( millis() > timeout ) { ///////////////////////////////// all anchors are off
// state = TWR_ENGINE_STATE_INIT;
///////////////////////////////////////////////////// timeout! go to the beginning.
if (num_ranging_error < 2) {
//blinkLED(1);
num_ranging_error++;
state = TWR_ENGINE_STATE_INIT;
} else { ////////////////////////////////////////////// if it fails 3times
state = TWR_ENGINE_STATE_UNABLE_TO_REACH_ANCHOR;
}
///////////////////////////////////////////////////////////// // we got Ack, one or more anchors are On
} else {
//blinkLED(1);
if ( decaduino.rxFrameAvailable() ) {
/////////////////////////////// check if it's the one from the anchor we want
if ( rxData[0] == my_short_address && rxData[1] == anchor_address[anchor_number] && rxData[2] == TWR_MSG_TYPE_ACK ) {
//
state = TWR_ENGINE_STATE_MEMORISE_T4;
} else {
//////////////////////////////// it's not the one from the anchor we want
// go back and wait for another one
state = TWR_ENGINE_STATE_RX_ON_FOR_ACK;
}
}
}
break;
case TWR_ENGINE_STATE_UNABLE_TO_REACH_ANCHOR:
// blinkLED(1);
ranging_error[anchor_number] = true;
state = TWR_ENGINE_STATE_CHECK_ONE_CYCLE;
break;
case TWR_ENGINE_STATE_MEMORISE_T4:
t4 = decaduino.lastRxTimestamp;
state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY;
break;
case TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY:
timeout = millis() + TIMEOUT;
state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY;
break;
case TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY:
decaduino.plmeRxEnableRequest(); //Sets transceiver mode to receive mode.
state = TWR_ENGINE_STATE_WAIT_DATA_REPLY;
break;
case TWR_ENGINE_STATE_WAIT_DATA_REPLY:
if ( millis() > timeout ) {
state = TWR_ENGINE_STATE_INIT;
} else {
if ( decaduino.rxFrameAvailable() ) {
if ( rxData[0] == my_short_address && rxData[1] == anchor_address[anchor_number] && rxData[2] == TWR_MSG_TYPE_DATA_REPLY ) {
state = TWR_ENGINE_STATE_EXTRACT_T2_T3;
} else state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY;
}
}
break;
case TWR_ENGINE_STATE_EXTRACT_T2_T3:
t2 = decaduino.decodeUint64(&rxData[3]);
t3 = decaduino.decodeUint64(&rxData[9]);
tof = (t4 - t1 - (t3 - t2)) / 2;
distance[anchor_number] = tof * COEFF * X_CORRECTION + Y_CORRECTION;
// if the distance is out of range then ignore this reading by making it 0
if (distance[anchor_number] > max_distance[anchor_number] || distance[anchor_number] < min_distance[anchor_number]) {
distance[anchor_number] = 0;
num_ranging_error++;
}
state = TWR_ENGINE_STATE_GET_SUM;
break;
case TWR_ENGINE_STATE_GET_SUM:
// check if it's time to get the sum
// check_time 0~9 total_check_time = 10
if (check_time < (total_check_time - 1)) { // it did't reach 10 readings
// still need to ranging the anchor
sum += distance[anchor_number];
check_time++;
state = TWR_ENGINE_STATE_INIT;
} else { // it reached 10 readings
state = TWR_ENGINE_STATE_CHECK_NUM_ERROR;
}
break;
case TWR_ENGINE_STATE_CHECK_NUM_ERROR:
// it reached 10times. check if the ranging_error > 5. it means we got more than 5 success.
if ( num_ranging_error >= 4) {
ranging_error[anchor_number] = true;
} else {
///////////////////////////////////////////////////////////////////// get the average
average[anchor_number] = sum / (division_factor - num_ranging_error);
ranging_error[anchor_number] = false;
}
state = TWR_ENGINE_STATE_CHECK_ONE_CYCLE;
break;
case TWR_ENGINE_STATE_CHECK_ONE_CYCLE:
// anchor_number 0~3
if (anchor_number < (total_anchor_num - 1)) {
///////////////////////////////////////////// move on to the next anchor
anchor_number++;
check_time = 0;
num_ranging_error = 0;
sum = 0;
state = TWR_ENGINE_STATE_INIT;
} else {
//////////////////////////////////////////// we finished whole(4) anchors reading
state = TWR_ENGINE_STATE_DISPLAY;
}
break;
case TWR_ENGINE_STATE_DISPLAY:
// We checked 1 anchor and averaged the value
// Let's display the result and then move on to the next anchor
// we got all the average and print them
display.clearDisplay();
display.setCursor(0, 0);
display.print("Bat : ");
display.print(bat_voltage);
//display.print(reading);
display.println("V");
display.println();
for (int i = 0; i < total_anchor_num; i++) {
display.print("anchor");
display.print(i + 1);
display.print(": ");
if (ranging_error[i] == true) {
display.println("Error");
} else {
display.print(average[i]);
display.println("m");
}
}
//t4 - t1
display.print("one cycle time : ");
display.print( millis() - last_time_check_range);
display.display();
state = TWR_ENGINE_STATE_FINISH;
break;
case TWR_ENGINE_STATE_FINISH:
////////////////////////////////////////////////////////////// we checked all the sensor so change the flag and get ready
ranging_flag = false;
break;
default:
state = TWR_ENGINE_STATE_INIT;
break;
}
}
Any help regarding this issue will be highly appreciated since it's urgent project!
Best jun