Juan Salazar
All mbed code for control over dive planes, pump motor, valve motor, BCUs, UART interface, etc.
Dependencies: mbed ESC mbed MODDMA
Revision 0:c3a329a5b05d, committed 2020-01-14
- Comitter:
- juansal12
- Date:
- Tue Jan 14 19:17:05 2020 +0000
- Commit message:
- Sofi7 mbed code;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/.gitignore Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,1 @@ +/.settings/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/.hgignore Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,22 @@ +syntax: regexp +\.hgignore$ +\.git$ +\.svn$ +\.orig$ +\.msub$ +\.meta$ +\.ctags +\.uvproj$ +\.uvopt$ +\.project$ +\.cproject$ +\.launch$ +\.project$ +\.cproject$ +\.launch$ +Makefile$ +\.ewp$ +\.eww$ +\.htm$ +Debug$ +.settings$
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/AcousticControl/AcousticController.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,905 @@
+/*
+ * Author: Joseph DelPreto
+ */
+
+#include "AcousticController.h"
+
+#ifdef acousticControl
+
+// The static instance
+AcousticController acousticController;
+
+// Map received state to fish values
+const float pitchLookupAcoustic[] = {0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; // [0.2 - 0.8]
+const float yawLookupAcoustic[] = {-1, -0.7, -0.5, 0, 0.5, 0.7, 1}; // [-1, 1]
+const float thrustLookupAcoustic[] = {0, 0.25, 0.50, 0.75};
+const float frequencyLookupAcoustic[] = {0.0000009, 0.0000012, 0.0000014, 0.0000016}; // cycles/us // NOTE also update periodHalfLookup if you update these values
+const float periodHalfLookupAcoustic[] = {555555, 416666, 357142, 312500}; // 1/(2*frequencyLookup) -> us
+
+// AGC definition
+const uint8_t agcGains[] = {1, 1, 2, 5, 10, 20, 50, 100}; // the possible gains of the AGC
+const uint8_t agcGainsLength = sizeof(agcGains)/sizeof(agcGains[0]);
+const uint16_t signalLevelBufferLength = (agcUpdateWindow)/(sampleWindow*sampleInterval); // first constant is window length for updating agc in microseconds
+const int32_t signalLevelSumDesired = ((uint32_t)500)*((uint32_t)signalLevelBufferLength); // desire about 2 VPP, so signal minus offset should have 1V amplitude, so say mean a bit smaller, about 0.4V ~= 500
+
+// Fish timeout
+const uint16_t fishTimeoutAcousticBuffers = fishTimeoutAcousticWindow/(sampleWindow*sampleInterval); // first constant is timeout in microseconds
+
+// Apparently it's hard to get a proper function pointer to a member function
+// so this dummy method will be set as the toneDetector callback function
+void toneDetectorCallback(int32_t* newTonePowers, uint32_t signalLevel)
+{
+ acousticController.processTonePowers(newTonePowers, signalLevel);
+}
+
+// Constructor
+AcousticController::AcousticController(Serial* usbSerialObject /* = NULL */) :
+ // Initialize variables
+ bufferCount(0),
+ lastDataWord(0),
+ timeSinceGoodWord(0),
+ streamCurFishStateAcoustic(0),
+ streamCurFishStateEventAcoustic(0)
+{
+ // AGC Pins
+ gain0 = new DigitalOut(agcPin0);
+ gain1 = new DigitalOut(agcPin1);
+ gain2 = new DigitalOut(agcPin2);
+ agc[0] = gain0;
+ agc[1] = gain1;
+ agc[2] = gain2;
+ #ifdef AGCLeds
+ agcLED0 = new DigitalOut(LED1);
+ agcLED1 = new DigitalOut(LED2);
+ agcLED2 = new DigitalOut(LED3);
+ agcLEDs[0] = agcLED0;
+ agcLEDs[1] = agcLED1;
+ agcLEDs[2] = agcLED2;
+ #endif
+
+ // Low battery
+ lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
+
+ // Misc
+ #ifdef artificialPowers
+ FILE* finPowers; = fopen("/local/powers.wp", "r");
+ #endif
+
+ // Complete initialization
+ init(usbSerialObject);
+}
+
+// Initialization
+void AcousticController::init(Serial* usbSerialObject /* = NULL */)
+{
+ // Create usb serial object or use provided one
+ if(usbSerialObject == NULL)
+ {
+ usbSerialObject = new Serial(USBTX, USBRX);
+ usbSerialObject->baud(serialDefaultBaudUSB);
+ }
+ usbSerial = usbSerialObject;
+ // Miscellaneous
+ bufferCount = 0;
+ lastDataWord = 0;
+ timeSinceGoodWord = 0;
+
+ // Will check if battery is low in every buffer callback
+ lowBatteryVoltageInput->mode(PullUp);
+ //lowBatteryInterrupt.fall(&lowBatteryCallback);
+
+ // TODO remove this?
+ wait(1.5);
+
+ // Configure the tone detector
+ toneDetector.setCallback(&toneDetectorCallback);
+ toneDetector.init();
+
+ // Clear detection arrays
+ #if defined(threshold2)
+ for(int t = 0; t < numTones; t++)
+ {
+ detectSums[t] = 0;
+ for(uint8_t p = 0; p < detectWindow; p++)
+ tonesPresent[p][t] = 0;
+ }
+ detectWindowIndex = 0;
+ readyToThreshold = false;
+ powerHistoryIndex = 0;
+ powerHistoryDetectIndex = (powerHistoryLength - 1) - (powerHistoryDetectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ powerHistorySumDetect[t] = 0;
+ powerHistorySumNoDetect[t] = 0;
+ powerHistoryMaxDetect[t] = 0;
+ powerHistoryMaxNoDetect[t] = 0;
+ for(uint16_t p = 0; p < powerHistoryLength; p++)
+ {
+ powerHistory[p][t] = 0;
+ }
+ }
+ #elif defined(threshold1)
+ for(int t = 0; t < numTones; t++)
+ {
+ detectSums[t] = 0;
+ }
+ powerHistoryIndex = 0;
+ powerHistoryDetectIndex = (powerHistoryLength - 1) - (detectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ powerHistorySum[t] = 0;
+ powerHistoryMax[t] = 0;
+ for(uint16_t p = 0; p < powerHistoryLength; p++)
+ {
+ powerHistory[p][t] = 0;
+ }
+ }
+ readyToThreshold = false;
+ #endif
+ #ifdef singleDataStream
+ #ifdef saveData
+ dataIndex = 0;
+ #endif
+ #else
+ #ifdef saveData
+ dataWordIndex = 0;
+ #endif
+ dataBitIndex = 0;
+ interWord = false;
+ #endif
+ waitingForEnd = false;
+ periodIndex = 0;
+ fskIndex = 0;
+
+ // Initialize adjustable gain control
+ signalLevelBufferIndex = 0;
+ signalLevelSum = 0;
+ currentGainIndex = 4;
+ for(uint8_t i = 0; i < sizeof(agc)/sizeof(agc[0]); i++)
+ {
+ agc[i]->write(currentGainIndex & (1 << i));
+ #ifdef AGCLeds
+ agcLEDs[i].write(currentGainIndex & (1 << i));
+ #endif
+ }
+}
+
+
+// Called by toneDetector when new tone powers are computed (once per buffer)
+void AcousticController::processTonePowers(int32_t* newTonePowers, uint32_t signalLevel)
+{
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[4] = -10;
+ #endif
+ // Check for low battery and if so terminate tone detector
+ if(lowBatteryVoltageInput == 0)
+ {
+ lowBatteryCallback();
+ return;
+ }
+ // See if we're done and if so terminate the tone detector
+ if(bufferCount == loopCount)
+ {
+ #ifndef infiniteLoopAcoustic
+ toneDetector.stop();
+ return;
+ #else
+ //bufferCount = 0;
+ #endif
+ }
+ bufferCount++;
+ // See if we're in autonomous mode and if so just let it be
+ if(fishController.autoMode)
+ return;
+
+ periodIndex++;
+ timeSinceGoodWord++;
+
+ #if defined(threshold2)
+ // Update threshold window state for each tone
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ // Update our history of powers for this tone
+ powerHistory[powerHistoryIndex][t] = (newTonePowers[t] >> 5);
+ // Compute max/sum of the current window (only for frequencies we currently anticipate)
+ if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
+ {
+ powerHistoryMaxDetect[t] = 0;
+ powerHistoryMaxNoDetect[t] = 0;
+ powerHistorySumDetect[t] = 0;
+ powerHistorySumNoDetect[t] = 0;
+ // Look at non-detection zone
+ for(uint16_t p = ((powerHistoryIndex+1)%powerHistoryLength); p != powerHistoryDetectIndex; p=(p+1)%powerHistoryLength)
+ {
+ powerHistorySumNoDetect[t] += powerHistory[p][t];
+ if(powerHistory[p][t] > powerHistoryMaxNoDetect[t])
+ powerHistoryMaxNoDetect[t] = powerHistory[p][t];
+ }
+ // Look at detection zone
+ for(uint16_t p = powerHistoryDetectIndex; p != ((powerHistoryIndex+1)%powerHistoryLength); p=((p+1)%powerHistoryLength))
+ {
+ powerHistorySumDetect[t] += powerHistory[p][t];
+ if(powerHistory[p][t] > powerHistoryMaxDetect[t])
+ powerHistoryMaxDetect[t] = powerHistory[p][t];
+ }
+ }
+ }
+ // Advance our power history index (circular buffer)
+ powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
+ powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
+ readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
+ // If not waiting out silence until next pulse is expected, see if a tone is present
+ if(!waitingForEnd && readyToThreshold)
+ {
+ // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
+ for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
+ {
+ detectSums[t] -= tonesPresent[detectWindowIndex][t];
+ if(
+ ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[t])
+ && (powerHistoryMaxDetect[t] > powerHistoryMaxNoDetect[t])
+ && ((powerHistorySumDetect[t] - (powerHistorySumDetect[t] >> 3)) > powerHistorySumDetect[fskIndex*2+((t+1)%2)])
+ && ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[fskIndex*2+((t+1)%2)])
+ && ((powerHistoryMaxDetect[t] << 4) > powerHistorySumNoDetect[t])
+ && (powerHistoryMaxDetect[t] > 100000)
+ )
+ tonesPresent[detectWindowIndex][t] = 1;
+ else
+ tonesPresent[detectWindowIndex][t] = 0;
+ detectSums[t] += tonesPresent[detectWindowIndex][t];
+ }
+ detectWindowIndex = (detectWindowIndex+1) % detectWindow;
+ // If both are considered present (should be very rare?), choose the one with a higher mean
+ if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
+ {
+ if(powerHistorySumDetect[fskIndex*2] > powerHistorySumDetect[fskIndex*2+1])
+ detectSums[fskIndex*2+1] = 0;
+ else
+ detectSums[fskIndex*2] = 0;
+ }
+ // See if a tone is present
+ int tonePresent = -1;
+ if(detectSums[fskIndex*2] > detectThresh)
+ tonePresent = fskIndex*2;
+ else if(detectSums[fskIndex*2+1] > detectThresh)
+ tonePresent = fskIndex*2+1;
+ // Record data and update state
+ if(tonePresent > -1)
+ {
+ #ifdef singleDataStream // if we just want a stream of bits instead of segmenting into words
+ #ifdef saveData
+ data[dataIndex++] = tonePresent%2;
+ #endif
+ #ifdef streamData
+ usbSerial->printf("%ld\t%d\n", bufferCount, (bool)(tonePresent%2));
+ #endif
+ periodIndex = detectSums[tonePresent];
+ fskIndex = (fskIndex+1) % numFSKGroups;
+ #else
+ // See if it has been a long time since last pulse
+ if(periodIndex >= interWordWait)
+ {
+ // If we currently think that we're between words, then that's good so process the previous data word
+ // If we don't think we're between words, then we missed a bit so discard whatever we've collected in the word so far
+ // Either way, this is the start of a new word so reset dataBitIndex
+ if(interWord && dataBitIndex == dataWordLength)
+ {
+ timeSinceGoodWord = 0;
+ streamCurFishStateEventAcoustic = 5;
+ // Decode last word and then send it out
+ #ifdef saveData
+ decodeAcousticWord(data[dataWordIndex]);
+ dataWordIndex++;
+ #else
+ decodeAcousticWord(dataWord);
+ #endif
+ dataBitIndex = 0;
+ interWord = false;
+ }
+ else if(!interWord) // missed a bit
+ {
+ #ifdef streamData
+ usbSerial->printf("%ld\t0\t-1\n", bufferCount);
+ #endif
+ #ifdef saveData
+ for(int dbi = 0; dbi < dataWordLength; dbi++)
+ data[dataWordIndex][dbi] = 0;
+ data[dataWordIndex][dataWordLength-1] = 1;
+ dataWordIndex++;
+ #endif
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[4] = -1;
+ streamCurFishStateEventAcoustic = 1;
+ #endif
+ lastDataWord = 0;
+ }
+ // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
+ if(interWordWait > 1)
+ {
+ dataBitIndex = 0;
+ interWord = false;
+ }
+ }
+ else if(interWord)
+ {
+ // It has not been a long time since the last pulse, yet we thought it should be
+ // Seems like we erroneously detected a bit that shouldn't have existed
+ // Discard current word as garbage and start again
+ dataBitIndex = 0;
+ #ifdef streamData
+ usbSerial->printf("%ld\t0\t-2\n", bufferCount);
+ #endif
+ #ifdef saveData
+ for(int dbi = 0; dbi < dataWordLength; dbi++)
+ data[dataWordIndex][dbi] = 0;
+ data[dataWordIndex][dataWordLength-2] = 1;
+ dataWordIndex++;
+ #endif
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[4] = -2;
+ streamCurFishStateEventAcoustic = 2;
+ #endif
+ lastDataWord = 0;
+ }
+ // If we're not between words (either normally or because we were just reset above), store the new bit
+ if(!interWord)
+ {
+ #ifdef saveData
+ data[dataWordIndex][dataBitIndex++] = tonePresent%2;
+ #else
+ dataWord[dataBitIndex++] = tonePresent%2;
+ #endif
+ // Rotate through which FSK frequency we expect for the next pulse
+ fskIndex = (fskIndex+1) % numFSKGroups;
+ // If we've finished a word, say we're waiting between words
+ // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
+ if(dataBitIndex == dataWordLength)
+ {
+ interWord = true;
+ fskIndex = 0;
+ }
+ }
+ periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
+ #endif
+ // Wait out reflections until next pulse
+ waitingForEnd = true;
+ }
+ }
+ else if(periodIndex > period) // done waiting for next bit (waiting out reflections)
+ {
+ waitingForEnd = false;
+ // Reset the sums and indicators
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ detectSums[t] = 0;
+ for(uint8_t p = 0; p < detectWindow; p++)
+ tonesPresent[p][t] = 0;
+ }
+ }
+ #elif defined(threshold1)
+ // Update threshold window state for each tone
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ // Update our history of powers for this tone
+ powerHistorySum[t] -= powerHistory[powerHistoryIndex][t];
+ powerHistory[powerHistoryIndex][t] = newTonePowers[t];
+ powerHistorySum[t] += newTonePowers[t];
+ // Compute max of the current window (only for frequencies we currently anticipate)
+ if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
+ {
+ powerHistoryMax[t] = 0;
+ for(uint16_t p = 0; p < powerHistoryLength; p++)
+ {
+ if(powerHistory[p][t] > powerHistoryMax[t])
+ powerHistoryMax[t] = powerHistory[p][t];
+ }
+ }
+ }
+ // Advance our power history index (circular buffer)
+ powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
+ readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
+ // If not waiting until next pulse is expected, see if a tone is present
+ if(!waitingForEnd && readyToThreshold)
+ {
+ // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
+ for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
+ {
+ detectSums[t] = 0;
+ for(uint16_t p = powerHistoryDetectIndex; p != powerHistoryIndex; p = (p+1) % powerHistoryLength)
+ {
+ if((powerHistory[p][t] > (powerHistorySum[fskIndex*2+((t+1)%2)] >> 2)) // power greater than 12.5 times mean of other channel
+ && (powerHistory[p][t] > (powerHistoryMax[t] >> 2)) // power greater than 1/4 of the max on this channel
+ && (powerHistory[p][t] > 1000) // power greater than a fixed threshold
+ && powerHistoryMax[t] > (powerHistorySum[t] >> 3)) // max on this channel is 6.25 times greater than the mean on this channel (in case this channel noise is just must higher than other channel)
+ detectSums[t]++;
+ }
+ }
+ // If both are considered present (should be very rare?), choose the one with a higher mean
+ if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
+ {
+ if(powerHistorySum[fskIndex*2] > powerHistorySum[fskIndex*2+1])
+ detectSums[fskIndex*2+1] = 0;
+ else
+ detectSums[fskIndex*2] = 0;
+ }
+ // See if a tone is present
+ int tonePresent = -1;
+ if(detectSums[fskIndex*2] > detectThresh)
+ tonePresent = fskIndex*2;
+ else if(detectSums[fskIndex*2+1] > detectThresh)
+ tonePresent = fskIndex*2+1;
+ // Record data and update state
+ if(tonePresent > -1)
+ {
+ #ifdef singleDataStream
+ #ifdef saveData
+ data[dataIndex++] = tonePresent%2;
+ #endif
+ #ifdef streamData
+ usbSerial->printf("%d", (bool)(tonePresent%2));
+ #endif
+ periodIndex = detectSums[tonePresent];
+ fskIndex = (fskIndex+1) % numFSKGroups;
+ #else
+ // See if it has been a long time since last pulse
+ if(periodIndex >= interWordWait)
+ {
+ // If we currently think that we're between words, then that's good so process the previous data word
+ // If we don't think we're between words, then we missed a bit so discard whatever we've collected in the word so far
+ // Either way, this is the start of a new word so reset dataBitIndex
+ if(interWord && dataBitIndex == dataWordLength)
+ {
+ // Decode last word and then send it out
+ #ifdef saveData
+ decodeAcousticWord(data[dataWordIndex]);
+ dataWordIndex++;
+ #else
+ decodeAcousticWord(dataWord);
+ #endif
+ dataBitIndex = 0;
+ interWord = false;
+
+ timeSinceGoodWord = 0;
+ }
+ else if(interWord)
+ {
+ #ifdef streamData
+ usbSerial->printf("0\t-1\n");
+ #endif
+ lastDataWord = 0;
+ }
+ // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
+ if(interWordWait > 1)
+ {
+ dataBitIndex = 0;
+ interWord = false;
+ }
+ }
+ else if(interWord)
+ {
+ // It has not been a long time since the last pulse, yet we thought it should be
+ // Seems like we erroneously detected a bit that shouldn't have existed
+ // Discard current word as garbage and start again
+ dataBitIndex = 0;
+ #ifdef streamData
+ usbSerial->printf("0\t-2\n");
+ #endif
+ lastDataWord = 0;
+ }
+ // If we're not between words (either normally or because we were just reset above), store the new bit
+ if(!interWord)
+ {
+ #ifdef saveData
+ data[dataWordIndex][dataBitIndex++] = tonePresent%2;
+ #else
+ dataWord[dataBitIndex++] = tonePresent%2;
+ #endif
+ fskIndex = (fskIndex+1) % numFSKGroups;
+ periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
+ // If we've finished a word, say we're waiting between words
+ // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
+ if(dataBitIndex == dataWordLength)
+ {
+ interWord = true;
+ fskIndex = 0;
+ }
+ }
+ #endif
+ // Wait out reflections until next pulse
+ waitingForEnd = true;
+ }
+ }
+ else if(periodIndex > period) // done waiting for next bit (waiting out reflections)?
+ {
+ waitingForEnd = false;
+ }
+ // Advance our power history start detect index (circular buffer) to stay detectWindow elements behind main index
+ powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
+ #endif // end switch between threshold1 or threshold2
+
+ // Update signal level history
+ signalLevelSum += signalLevel;
+ signalLevelBufferIndex = (signalLevelBufferIndex+1) % signalLevelBufferLength;
+ #ifdef streamSignalLevel
+ //usbSerial->printf("%ld\t%d\t%d\n", signalLevel, currentGainIndex, lastDataWord);
+ usbSerial->printf("%ld\t%d\n", signalLevel, currentGainIndex);
+ #endif
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[3] = currentGainIndex;
+ #endif
+ // If have seen enough readings, choose a new gain
+ if(signalLevelBufferIndex == 0)
+ {
+ // Calculate ideal gain for signalLevelSum to become signalLevelSumDesired
+ int32_t newGain = ((int64_t)signalLevelSumDesired * (int64_t)agcGains[currentGainIndex]) / (int64_t)signalLevelSum;
+ // See which available gain is closest
+ uint8_t bestIndex = currentGainIndex;
+ int32_t minDiff = 10000;
+ for(uint8_t g = 0; g < agcGainsLength; g++)
+ {
+ int32_t diff = (int32_t)agcGains[g] - newGain;
+ if(diff > 0 && diff < minDiff)
+ {
+ minDiff = diff;
+ bestIndex = g;
+ }
+ else if(diff < 0 && -1*diff < minDiff)
+ {
+ minDiff = -1*diff;
+ bestIndex = g;
+ }
+ }
+ // Set the gain
+ currentGainIndex = bestIndex;
+ for(uint8_t i = 0; i < 3; i++)
+ {
+ #ifdef useAGC
+ agc[i]->write(currentGainIndex & (1 << i));
+ #endif
+ #ifdef AGCLeds
+ agcLEDs[i].write(currentGainIndex & (1 << i));
+ #endif
+ }
+ // Reset sum
+ signalLevelSum = 0;
+ }
+
+ #ifdef artificialPowers
+ usbSerial->printf("%ld \t%ld \t%ld \t%d \t%ld \t%ld \t%ld \t%ld \t%d \t%d \t%d \n", newTonePowers[0], newTonePowers[1], detectSums[0], detectSums[1], powerHistorySum[0], powerHistorySum[1], powerHistoryMax[0], powerHistoryMax[1], fskIndex, waitingForEnd, periodIndex);
+ #endif
+
+ // Check for timeout
+ #ifdef fishTimeoutAcoustic
+ if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
+ {
+ #ifndef singleDataStream
+ if(timeSinceGoodWord == fishTimeoutAcousticBuffers)
+ {
+ bool neutralWord[] = {0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0}; // Decodes to state 54, which is neutral state
+ decodeAcousticWord(neutralWord);
+ streamCurFishStateEventAcoustic = 4;
+ }
+ #endif
+ timeSinceGoodWord = fishTimeoutAcousticBuffers+1; // make sure we won't constantly send a neutral command (only timeout once)
+ }
+ #endif
+
+ // Stream data
+ #ifdef streamAcousticControlLog
+ //usbSerial->printf("%ld %ld %ld %ld %ld %ld\n", bufferCount-1, acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
+ //usbSerial->printf("%ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2]);
+ //usbSerial->printf("%ld %ld %ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
+ //FunctionPointer fp = &tempMethod;
+ //acousticControlLogToStream[0] = 6100;
+ //acousticControlLogToStream[1] = 62;
+ //acousticControlLogToStream[2] = 63;
+ //acousticControlLogToStream[3] = 64;
+ //acousticControlLogToStream[4] = -10;
+ uint32_t streamGoertzel1 = acousticControlLogToStream[0];// >> 8; // second Fiji day included >> 8 to reduce bits
+ uint32_t streamGoertzel2 = acousticControlLogToStream[1];// >> 8; // second Fiji day included >> 8 to reduce bits
+ uint16_t streamSignalLevel = ((acousticControlLogToStream[2] >> 2) & ((uint16_t)2047)) + (((uint16_t)21) << 11); // first Fiji day was just acousticControlLogToStream[2], second day included code word
+ uint8_t streamGain = acousticControlLogToStream[3] + 168; // only used in first day
+ //int16_t streamWord = acousticControlLogToStream[4];
+ // Decide the event to transmit, giving priority to acoustic events over button board events
+ uint16_t streamFishStateEvent = fishController.streamFishStateEventController;
+ if(streamCurFishStateEventAcoustic > 0)
+ streamFishStateEvent = streamCurFishStateEventAcoustic;
+ uint16_t streamWord = (uint16_t)streamCurFishStateAcoustic | ((uint16_t)streamFishStateEvent << 11); // same for both Fiji days
+
+ uint8_t* bufferOut = (uint8_t*) (&streamGoertzel1);
+ usbSerial->putc(bufferOut[0]);
+ usbSerial->putc(bufferOut[1]);
+ usbSerial->putc(bufferOut[2]);
+ usbSerial->putc(bufferOut[3]); // commented out for second day
+
+ bufferOut = (uint8_t*) (&streamGoertzel2);
+ usbSerial->putc(bufferOut[0]);
+ usbSerial->putc(bufferOut[1]);
+ usbSerial->putc(bufferOut[2]);
+ usbSerial->putc(bufferOut[3]); // commented out for second day
+
+ bufferOut = (uint8_t*) (&streamSignalLevel);
+ usbSerial->putc(bufferOut[0]);
+ usbSerial->putc(bufferOut[1]);
+
+ bufferOut = (uint8_t*) (&streamGain);
+ usbSerial->putc(bufferOut[0]); // commented out for second day
+
+ bufferOut = (uint8_t*) (&streamWord);
+ usbSerial->putc(bufferOut[0]);
+ usbSerial->putc(bufferOut[1]);
+ //usbSerial->printf("%d %d\n", streamCurFishState, streamWord);
+
+ /*uint8_t* bufferOut = (uint8_t*) acousticControlLogToStream;
+ for(uint8_t i = 0; i < 20; i++)
+ usbSerial->putc(bufferOut[i]);*/
+
+ // Reset the events so we don't constantly print them
+ // TODO remove this so we do constantly print them, and only look for changes in the column in case some are dropped?
+ streamCurFishStateEventAcoustic = 0;
+ fishController.streamFishStateEventController = 0;
+ #endif
+}
+
+
+#ifndef singleDataStream
+// Use Hamming code to decode the data word and check for an error
+// TODO make constants/defines for magic numbers below that indicate length of hamming code
+void AcousticController::decodeAcousticWord(volatile bool* data)
+{
+ // Note that the below code is written such that "5" (number of parity bits) can be a variable / #define, but it's just not yet
+ // Check whether each parity bit is correct
+ bool paritySums[5] = {0};
+ for(uint8_t b = 0; b < dataWordLength-1; b++)
+ {
+ for(uint8_t p = 0; p < 5; p++)
+ {
+ if(((b+1) & (1 << p)) > 0 || p == 5-1) // check if bit belongs to parity set (overall parity at end includes everything)
+ paritySums[p] ^= data[b];
+ }
+ }
+ paritySums[5-1] ^= data[dataWordLength-1]; // overall parity bit
+ // See if we have an error to correct
+ uint8_t errorBit = 0;
+ uint8_t numParityErrors = 0;
+ for(uint8_t p = 0; p < 5-1; p++)
+ {
+ if(paritySums[p])
+ {
+ errorBit += (1 << p);
+ numParityErrors++;
+ }
+ }
+ // Flip the erroneous bit if applicable
+ if(numParityErrors > 1)
+ data[errorBit-1] = !data[errorBit-1];
+ // If using final parity bit, check that we don't detect an uncorrectable error
+ if(!(numParityErrors > 0 && !paritySums[5-1]))
+ {
+ uint16_t word = 0;
+ uint8_t i = 0;
+ uint8_t nextParity = 1;
+ for(uint8_t b = 0; b < dataWordLength; b++)
+ {
+ if(b == nextParity-1)
+ nextParity <<= 1;
+ else
+ word |= data[b] << i++;
+ }
+ // Update the fish
+ processAcousticWord(word);
+ lastDataWord = word;
+ #ifdef streamData
+ if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
+ usbSerial->printf("%ld\t%d\t-4\n", bufferCount, (int)word);
+ else
+ usbSerial->printf("%ld\t%d\t1\n", bufferCount, (int)word);
+ #endif
+ #ifdef streamAcousticControlLog
+ if(timeSinceGoodWord >= fishTimeoutAcousticBuffers) // check if we're timed out, since we may have reached this method when the timeout checker called us to reset the fish
+ acousticControlLogToStream[4] = -4;
+ else
+ acousticControlLogToStream[4] = word;
+ #endif
+ }
+ else // there was an uncorrectable error
+ {
+ #ifdef streamData
+ uint16_t word = 0;
+ for(int i = 0; i < 16; i++)
+ word += data[i] << i;
+ usbSerial->printf("%ld\t%d\t-3\n", bufferCount, word);
+ #endif
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[4] = -3;
+ streamCurFishStateEventAcoustic = 3;
+ #endif
+ lastDataWord = 0;
+ }
+}
+
+void AcousticController::processAcousticWord(uint16_t word)
+{
+ // Extract state from word
+ newSelectButtonIndex = getSelectIndexAcoustic(word);
+ newPitchIndex = getPitchIndexAcoustic(word);
+ newYawIndex = getYawIndexAcoustic(word);
+ newThrustIndex = getThrustIndexAcoustic(word);
+ newFrequencyIndex = getFrequencyIndexAcoustic(word);
+ // Log it
+ streamCurFishStateAcoustic = getCurStateWordAcoustic;
+ // Set the states
+ #ifdef acousticControllerControlFish
+ fishController.setSelectButton(newSelectButtonIndex > 0);
+ fishController.setPitch(pitchLookupAcoustic[newPitchIndex]);
+ fishController.setYaw(yawLookupAcoustic[newYawIndex]);
+ fishController.setThrust(thrustLookupAcoustic[newThrustIndex]);
+ fishController.setFrequency(frequencyLookupAcoustic[newFrequencyIndex], periodHalfLookupAcoustic[newFrequencyIndex]);
+ #endif
+}
+#endif
+
+// Stop the AcousticController
+// Will stop the tone detector and will stop the fishController
+// Note that the acoustic controller's run() method is blocking, so if you
+// want to use this stop method you should launch run() in a separate thread
+void AcousticController::stop()
+{
+ // Stop the tone detector
+ // This will end the toneDetector's run() method at the next buffer
+ // which will cause our run() method to advance and finish up
+ toneDetector.stop();
+}
+
+// Main loop
+// Is blocking, so won't return until loopCount is reached or another thread calls stop()
+void AcousticController::run()
+{
+ // Create file for logging data words
+ #ifdef saveData
+ int fileNum = -1;
+ char filename[25];
+ foutDataWords = NULL;
+ do
+ {
+ fileNum++;
+ fclose(foutDataWords);
+ sprintf(filename, "/local/%d.txt", fileNum);
+ foutDataWords = fopen(filename, "r");
+ usbSerial->printf("%d\n", fileNum);
+ } while(foutDataWords != NULL);
+ foutDataWords = fopen(filename, "w");
+ #endif
+
+ #ifdef acousticControllerControlFish
+ // Start the fish controller
+ fishController.start();
+ #endif
+
+ #ifndef artificialPowers
+ // Start listening for tones
+ programTimer.start();
+ toneDetector.run();
+ programTimer.stop();
+ toneDetector.finish(); // we won't include the time this takes to write files in the elapsed time
+ #else
+ // Read powers from file
+ usbSerial->printf("newPower[0] \tnewPower[1] \tdetectSums[0] \tdetectSums[1] \tsum[0] \tsum[1] \tmax[0] \tmax[1] \tfskIndex \twaiting \tperiodIndex \n");
+ int32_t maxSignalValTemp = 0;
+ int res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
+ while(res > 0)
+ {
+ processTonePowers(nextPowers, maxSignalValTemp);
+ res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
+ }
+ fclose(finPowers);
+ #endif
+
+ #ifdef acousticControllerControlFish
+ // Stop the fish controller
+ fishController.stop();
+ // If battery died, wait a bit for pi to clean up and shutdown and whatnot
+ if(lowBatteryVoltageInput == 0)
+ {
+ wait(90); // Give the Pi time to shutdown
+ fishController.setLEDs(255, false);
+ }
+ #endif
+
+ // Print results
+ #ifdef printBufferSummary
+ int elapsed = programTimer.read_us();
+ usbSerial->printf("\n");
+ usbSerial->printf("Buffers processed: %ld\n", bufferCount);
+ usbSerial->printf("Elapsed time : %d us\n", elapsed);
+ usbSerial->printf("Per-sample time : %f us\n", (double)elapsed/(double)bufferCount/(double)sampleWindow);
+ usbSerial->printf(" Sample frequency: %f kHz\n", (double)bufferCount*(double)sampleWindow/(double)elapsed*1000.0);
+ usbSerial->printf("Per-buffer time : %f us\n", (double)elapsed/(double)bufferCount);
+ usbSerial->printf(" Buffer-processing frequency: %f kHz\n", (double)bufferCount/(double)elapsed*1000.0);
+
+ usbSerial->printf("\nComputed powers from last buffer: \n");
+ int32_t* lastTonePowers = toneDetector.getTonePowers();
+ for(int i = 0; i < numTones; i++)
+ usbSerial->printf(" Tone %d: %f Hz -> %f\n", i, targetTones[i], toFloat(lastTonePowers[i]));
+ #endif
+ #if defined(singleDataStream) && defined(saveData)
+ usbSerial->printf("\nData received (%d bits):\n", dataIndex);
+ fprintf(foutDataWords, "\nData received (%d bits):\n", dataIndex);
+ long errors = 0;
+ for(int d = 5; d < dataIndex; d++)
+ {
+ usbSerial->printf("%d", data[d]);
+ fprintf(foutDataWords, "%d", data[d]);
+ if(d > 0 && data[d] == data[d-1])
+ errors++;
+ }
+ usbSerial->printf("\n");
+ usbSerial->printf("errors: %ld\n", errors);
+ fprintf(foutDataWords, "\n");
+ fprintf(foutDataWords, "errors: %ld\n", errors);
+ fclose(foutDataWords);
+ #ifdef debugLEDs
+ if(errors > 0)
+ led1 = 1;
+ #endif
+ #elif defined(saveData)
+ usbSerial->printf("\nData received (%d words):\n", dataWordIndex);
+ fprintf(foutDataWords, "\nData received (%d words):\n", dataWordIndex);
+ long errors = 0;
+ long badWords = 0;
+ for(int w = 0; w < dataWordIndex; w++)
+ {
+ errors = 0;
+ usbSerial->printf(" ");
+ fprintf(foutDataWords, " ");
+ for(int b = 0; b < dataWordLength; b++)
+ {
+ usbSerial->printf("%d", data[w][b]);
+ fprintf(foutDataWords, "%d", data[w][b]);
+ if(b > 0 && data[w][b-1] == data[w][b])
+ errors++;
+ }
+ if(errors > 0)
+ {
+ usbSerial->printf(" X");
+ fprintf(foutDataWords, " X");
+ badWords++;
+ }
+ usbSerial->printf("\n");
+ fprintf(foutDataWords, "\n");
+ }
+ usbSerial->printf("\nbad words: %d\n", badWords);
+ fprintf(foutDataWords, "\nbad words: %d\n", badWords);
+ fclose(foutDataWords);
+ #ifdef debugLEDs
+ if(badWords > 0)
+ led1 = 1;
+ #endif
+ #endif
+
+ // TODO remove these waits?
+ wait(1);
+ #ifdef printBufferSummary
+ usbSerial->printf("\nAcousticController Done!");
+ #endif
+ wait(3);
+ #ifdef printBufferSummary
+ usbSerial->printf("\n");
+ #endif
+}
+
+
+void AcousticController::lowBatteryCallback()
+{
+ // Stop the tone detector
+ // This will end the main call to start, causing main to terminate
+ // Main will also stop the fish controller once this method ends
+ toneDetector.stop();
+ // Also force the pin low to signal the Pi
+ // (should have already been done, but just in case)
+ // TODO check that this really forces it low after this method ends and the pin object may be deleted
+ DigitalOut simBatteryLow(lowBatteryVoltagePin);
+ simBatteryLow = 0;
+}
+
+#endif // #ifdef acousticControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/AcousticControl/AcousticController.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,256 @@
+/*
+ * AcousticController.h
+ * Author: Joseph DelPreto
+ */
+
+//#define acousticControl
+
+#ifdef acousticControl
+
+#ifndef ACOUSTICCONTROL_ACOUSTICCONTROLLER_H_
+#define ACOUSTICCONTROL_ACOUSTICCONTROLLER_H_
+
+#include "ToneDetector.h"
+#include "FishController.h"
+#include "mbed.h"
+
+// ===== INPUT/OUTPUT =====
+// NOTE: To be silent (to not use usbSerial object at all), undefine printBufferSummary, streamAcousticControlLog, streamData, and streamSignalLevel
+// To stream the log info used in Fiji, just define streamAcousticControlLog - these are not human-readable streams
+// To view the data as it's being received/decoded, define streamData (or streamSignalLevel for signal level) - these are human-readable streams
+// To view a data summary (ex. number of buffers processed, words received, etc.) define printBufferSummary
+#define serialDefaultBaudUSB 921600 // IMPORTANT: must be 921600 when using streaAcousticControlLog (to be fast enough to keep up with data stream)
+#define printBufferSummary // print a summary of how many buffers were processed, the last computed tone powers, etc. once the controller is stopped
+/* SEE ALSO: streamAcousticControlLog, defined in ToneDetector.h
+ If defined, will update the following (see end of processTonePowers for how they're packaged):
+
+ acousticControlLogToStream:
+ [0]: goertzel powers f1
+ [1]: goerztel powers f2
+ [2]: signal level
+ [3]: AGC gain
+ [4]: fish state/events over Serial
+ -1: erroneously missed a bit
+ -2: erroneously added a bit
+ -3: detected an uncorrectable error in the Hamming decoding
+ -4: fish timeout (reset to neutral)
+ >0: the data word that was successfully decoded and processed
+ -10: no event
+
+UPDATE:
+
+ acousticControlLogToStream[4] has been deprecated and replaced with streamCurFishStateEventAcoustic and streamCurFishStateAcoustic
+ streamCurFishStateEventAcoustic can have the following values:
+ 1: erroneously missed a bit
+ 2: erroneously added a bit
+ 3: detected an uncorrectable error in the Hamming decoding
+ 4: fish timeout (reset to neutral)
+ 5: successfully received a word (note streamCurFishState indicates the current fish state)
+ 6: button board yaw left
+ 7: button board yaw right
+ 8: button board faster
+ 9: button board slower
+ 10: button board pitch up
+ 11: button board pitch down
+ 12: button board shutdown pi
+ 13: button board reset mbed
+ 14: button board auto mode
+ 15: button board unknown
+*/
+
+//#define artificialPowers // will use tone powers saved in a file rather than actually sampling (expects two tab-separated columns of powers in /local/powers.wp)
+//#define singleDataStream // just read a single continuous stream of bits (as opposed to segmenting into words/decoding)
+
+//#define saveData // save data such as received words to an array, then save it to a file at the end - faster than printing continuously but worry about overflow
+//#define streamData // stream data to the screen as it's received (controlled independently of saveData)
+//#define streamSignalLevel // stream current signal level to the screen
+
+// ===== EXECUTION =====
+#define infiniteLoopAcoustic // if undefined, will stop after loopCount buffers are processed
+#define loopCount 20000 // number of buffers to process before terminating the program (if infiniteLoopAcoustic is not defined)
+#define fishTimeoutAcoustic // whether to reset the fish to neutral if an acoustic word hasn't been received in a while. Not used with singleDataStream.
+#define fishTimeoutAcousticWindow 5000000 // time between good words that cause a timeout (in microseconds)
+
+#define acousticControllerControlFish // whether to start fishController to control the servos and motor
+
+#define useAGC // if undefined, will only set agc (adjustable gain control of the acoustic input board) once at startup
+//#define AGCLeds // if defined, will light the mBed LEDs to indicate the chosen agc gain index (whether or not useAGC is on)
+#define agcUpdateWindow 10000000 // how often to update the AGC gain (in microseconds)
+
+#define dataRate 20 // data rate in bps. Can be 20, 50, 77, or 100. 20 was used in Fiji. Above 50 may be less reliable.
+
+//#define threshold1 // older algorithm for processing Goertzel buffers (not sure if it still works)
+#define threshold2
+
+// ===== PINS =====
+#define agcPin0 p16
+#define agcPin1 p17
+#define agcPin2 p18
+// note lowBatteryVoltagePin is defined in FishController
+
+// ===== Sampling / Thresholding configuration =====
+// 20 bps (5/45 ms tone/silence)
+#if dataRate == 20
+#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
+#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
+#define period 80 // the number of buffers from a rising edge to when we start looking for the next rising edge
+#define bitPeriod 100 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
+#define interWordWait 300 // how many buffers of silence to expect between words
+#elif dataRate == 50
+// 50 bps (5/15 ms tone/silence)
+#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
+#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
+#define period 20 // the number of buffers from a rising edge to when we start looking for the next rising edge
+#define bitPeriod 40 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
+#define interWordWait 300 // how many buffers of silence to expect between words
+#elif dataRate == 77
+// 77 bps (5/8 ms tone/silence)
+#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
+#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
+#define period 10 // the number of buffers from a rising edge to when we start looking for the next rising edge
+#define bitPeriod 26 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
+#define interWordWait 300 // how many buffers of silence to expect between words
+#elif dataRate == 100
+// 100 bps (5/5 ms tone/silence)
+#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
+#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
+#define period 6 // the number of buffers from a rising edge to when we start looking for the next rising edge
+#define bitPeriod 20 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
+#define interWordWait 300 // how many buffers of silence to expect between words
+#endif
+
+// ===== Macros for extracting state from data words =====
+#define getSelectIndexAcoustic(d) ((d & (1 << 0)) >> 0)
+#define getPitchIndexAcoustic(d) ((d & (7 << 1)) >> 1)
+#define getYawIndexAcoustic(d) ((d & (7 << 4)) >> 4)
+#define getThrustIndexAcoustic(d) ((d & (3 << 7)) >> 7)
+#define getFrequencyIndexAcoustic(d) ((d & (3 << 9)) >> 9)
+
+#define getCurStateWordAcoustic (uint16_t)((uint16_t)newSelectButtonIndex + ((uint16_t)newPitchIndex << 1) + ((uint16_t)newYawIndex << 4) + ((uint16_t)newThrustIndex << 7) + ((uint16_t)newFrequencyIndex << 9));
+
+class AcousticController
+{
+ public:
+ // Initialization
+ AcousticController(Serial* usbSerialObject = NULL); // if serial object is null, one will be created
+ void init(Serial* usbSerialObject = NULL); // if serial object is null, one will be created
+ // Execution control
+ void run();
+ void stop();
+ // Called by toneDetector when new tone powers are computed
+ // Only public since we needed a dummy non-member function to call it from the static instance to pass a pointer to tonedetector (see comments on toneDetectorCallback)
+ void processTonePowers(int32_t* newTonePowers, uint32_t signalLevel);
+ private:
+ // Control
+ volatile uint32_t bufferCount;
+ Timer programTimer;
+ Serial* usbSerial;
+
+ #ifndef singleDataStream
+ void decodeAcousticWord(volatile bool* data); // Use Hamming code to decode the data word and check for an error
+ void processAcousticWord(uint16_t word); // Parse the decoded word into the various fish states
+ #endif
+
+ // TODO check what actually needs to be volatile
+
+ // Data detection
+ volatile bool waitingForEnd; // got data, now waiting until next bit transmission is expected (waiting until "period" buffers have elapsed since edge)
+ volatile uint16_t periodIndex; // how many buffers it's been since the last clock edge
+ volatile uint8_t fskIndex; // which set of 0/1 frequencies are next expected
+
+ #if defined(threshold2)
+ #define powerHistoryLength 50 // number of buffer results to consider when deciding if a tone is present. should be half of a bit-width
+ #define powerHistoryDetectWindow 20 // portion of powerHistory to consider as the detection zone (where a peak is expected)
+ volatile int32_t powerHistory[powerHistoryLength][numTones]; // History of Goertzel results for the last powerHistoryLength buffers
+ volatile int16_t powerHistoryIndex; // Index into powerHistory (circular buffer)
+ volatile int16_t powerHistoryDetectIndex; // Marks where in powerHistory to start window for detection; will always be detectWindow elements behind powerHistoryIndex in the circular buffer
+ volatile int32_t powerHistorySumDetect[numTones]; // Sum of powers stored in powerHistory in the detect window (stand-in for mean)
+ volatile int32_t powerHistorySumNoDetect[numTones]; // Sum of powers stored in powerHistory outside the detect window (stand-in for mean)
+ volatile int32_t powerHistoryMaxDetect[numTones]; // Max of powers stored in powerHistory in the detect window
+ volatile int32_t powerHistoryMaxNoDetect[numTones]; // Max of powers stored in powerHistory outside the detect window
+ volatile bool readyToThreshold; // Whether powerHistory has been filled at least once
+ volatile bool tonesPresent[detectWindow][numTones]; // Circular buffer of whether tones were detected in last detectWindow powers
+ volatile uint8_t detectSums[numTones]; // Rolling sum over last detectWindow tonesPresent results (when > detectWindow/2, declares data bit)
+ volatile uint8_t detectWindowIndex;
+ #elif defined(threshold1)
+ #define powerHistoryLength 50 // number of results to consider when deciding if a tone is present. should be half of a bit-width
+ volatile uint8_t detectSums[numTones]; // Rolling sum over last detectWindow buffer results (when > detectWindow/2, declares clock/data)
+ int32_t powerHistory[powerHistoryLength][numTones]; // History of Goertzel results for the last powerHistoryLength buffers
+ volatile int16_t powerHistoryIndex; // Index into powerHistory (circular buffer)
+ volatile int16_t powerHistoryDetectIndex; // Marks where in powerHistory to start window for detection; will always be detectWindow elements behind powerHistoryIndex in the circular buffer
+ int32_t powerHistorySum[numTones]; // Sum of powers stored in powerHistory (stand-in for mean)
+ int32_t powerHistoryMax[numTones]; // Max of powers stored in powerHistory
+ volatile bool readyToThreshold; // Whether powerHistory has been filled at least once
+ #endif
+
+ // Segmenting of data into bits/words
+ #if defined(singleDataStream) && defined(saveData)
+ volatile bool data[loopCount/(bitPeriod) * (numTones-1) + 50]; // Received data (each clock pulse is associated with numTones-1 bits) // TODO update for multi-hop FSK, but this is fine for one FSK group
+ volatile uint16_t dataIndex;
+ #else
+ #define dataWordLength 16 // bits per word. note that changing this also requires adjusting the decodeAcousticWord algorithm, since it currently assumes Hamming encoding
+ volatile uint8_t dataBitIndex; // current bit of that word
+ volatile bool interWord; // whether we are currently in between words (waiting out the long inter-word pause)
+ #ifdef saveData
+ volatile bool data[loopCount/(bitPeriod*dataWordLength)][dataWordLength]; // Each element is a word as an array of bits
+ volatile uint16_t dataWordIndex; // current data word
+ #else
+ volatile bool dataWord[dataWordLength]; // an array of bits making up the current word
+ #endif
+ #endif
+ volatile uint16_t lastDataWord; // not used anymore, but still updated
+
+ // Adjustable gain control (AGC)
+ volatile uint8_t currentGainIndex; // the current index into agcGains
+ volatile int32_t signalLevelSum; // the running sum of signal level readings
+ volatile uint16_t signalLevelBufferIndex; // index into signal level buffer // note we don't have a buffer anymore, but use this to know when to reset the sum
+
+ DigitalOut* gain0;
+ DigitalOut* gain1;
+ DigitalOut* gain2;
+ DigitalOut* agc[3]; // facilitates loops and whatnot
+ #ifdef AGCLeds
+ #undef debugLEDs
+ DigitalOut* agcLED0;
+ DigitalOut* agcLED1;
+ DigitalOut* agcLED2;
+ DigitalOut* agcLEDs[3];
+ #endif
+
+ // Fish reset timeout
+ volatile uint16_t timeSinceGoodWord; // measured in buffers
+
+ // Low battery monitor
+ DigitalIn* lowBatteryVoltageInput;
+ void lowBatteryCallback();
+
+ // Testing/debugging
+ #ifdef saveData
+ LocalFileSystem local("local");
+ FILE* foutDataWords;
+ #endif
+ #ifdef artificialPowers
+ #if (!defined recordStreaming || (!defined recordOutput && !defined recordSamples)) && !defined saveData
+ LocalFileSystem local("local");
+ #endif
+ FILE* finPowers;
+ int32_t nextPowers[numTones];
+ #endif
+ volatile uint16_t streamCurFishStateAcoustic; // the current state of the fish
+ volatile uint16_t streamCurFishStateEventAcoustic; // events such as decoding words, errors, etc.
+
+ // Fish state extracted from acoustic data word (index into lookup tables defined above)
+ volatile uint8_t newSelectButtonIndex;
+ volatile uint8_t newPitchIndex;
+ volatile uint8_t newYawIndex;
+ volatile uint8_t newThrustIndex;
+ volatile uint8_t newFrequencyIndex;
+};
+
+
+// Create a static instance of AcousticController to be used by anyone doing detection
+extern AcousticController acousticController;
+
+#endif
+
+#endif // #ifdef acousticControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/AcousticControl/ToneDetector.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,626 @@
+/*
+ * Author: Joseph DelPreto
+ */
+
+#include "ToneDetector.h"
+
+#ifdef acousticControl
+
+// The static instance
+ToneDetector toneDetector;
+#ifdef streamAcousticControlLog
+int32_t acousticControlLogToStream[5] = {0,0,0,0,0};
+#endif
+
+//============================================
+// Initialization
+//============================================
+
+// Constructor
+ToneDetector::ToneDetector() :
+ terminated(0),
+ fillingBuffer0(false),
+ transferComplete(false),
+ readyToBegin(false),
+ callbackFunction(0)
+ #ifdef artificialSamplesMode
+ , numTestSamples(0),
+ testSampleIndex(0),
+ sampleIndex(0)
+ #endif
+{
+}
+
+// Set a callback function that should be called after each buffer is processed
+void ToneDetector::setCallback(void (*myFunction)(int32_t* tonePowers, uint32_t signalLevel))
+{
+ callbackFunction = myFunction;
+}
+
+// Initialize the Goertzel variables, arrays, etc.
+// sampleWindow, sampleInterval, and tones must have been previously set
+// if not, this method will return without doing anything
+void ToneDetector::init()
+{
+ // Make sure sampleWindow, sampleInterval, and desired tones have been set
+ if(sampleWindow == 0 || sampleInterval == 0 || numTones == 0)
+ return;
+
+ // Initialize sample buffers
+ //printf("Sampling interval: %d us\n", sampleInterval);
+ //printf("Buffer size: %d samples\n", sampleWindow);
+ for(uint16_t i = 0; i < sampleWindow; i++) // not using memset since sizeof seems to not work with uint16_t?
+ {
+ sampleBuffer0[i] = 0;
+ sampleBuffer1[i] = 0;
+ }
+ #if defined(recordSamples) && !defined(recordStreaming)
+ savedSamplesIndex = 0;
+ for(uint16_t i = 0; i < numSavedSamples; i++) // not using memset since sizeof seems to not work with uint16_t?
+ savedSamples[i] = 0;
+ #endif
+
+ // Initialize goertzel arrays
+ tonePowersWindowIndex = 0;
+ for(int i = 0; i < numTones; i++)
+ {
+ goertzelCoefficients[i] = 0;
+ for(int w = 0; w < tonePowersWindow; w++)
+ tonePowers[i][w] = 0;
+ tonePowersSum[i] = 0;
+ }
+ readyToThreshold = false;
+
+ // Some convenience variables as doubles for precision (will cast to fixed point later)
+ double sampleFrequency = 1000.0/((double)sampleInterval/1000.0); // Hz
+ double N = (double) sampleWindow;
+
+ // Calculate the coefficient for each tone
+ //printf("Initializing Goertzel algorithm\n");
+ //printf(" Bin size: %f\n", sampleFrequency/N);
+ for(int i = 0; i < numTones; i++)
+ {
+ // Determine K and then f for desired tone (f = desiredF/Fs)
+ // tone/fs = f = K/N --> K = (int)(tone*N/fs)
+ double tone = targetTones[i];
+ int k = (int) (0.5 + ((N * tone) / sampleFrequency));
+ float f = ((float)k) / N;
+ //printf(" Desired tone %f -> %f", tone, f*sampleFrequency);
+ float cosine = cos(2.0 * PI * f);
+ // Store the result as a fixed-point number
+ goertzelCoefficients[i] = toFixedPoint(2.0*cosine);
+ //printf("\t (coefficient: %f)\n", toFloat(goertzelCoefficients[i]));
+ }
+
+ #if defined(recordOutput) && !defined(recordStreaming)
+ savedTonePowersIndex = 0;
+ for(uint16_t i = 0; i < numSavedTonePowers; i++) // not using memset since sizeof seems to not work with uint16_t?
+ {
+ for(uint8_t t = 0; t < numTones; t++)
+ savedTonePowers[i][t] = 0;
+ }
+ #endif
+
+ // We're ready to process some tunes!
+ readyToBegin = true;
+}
+
+#ifndef artificialSamplesMode
+// Configure and start the DMA channels for ADC sample gathering
+// Will have a linked list of two DMA operations, one for each buffer
+void ToneDetector::startDMA()
+{
+ // Create the Linked List Items
+ lli[0] = new MODDMA_LLI;
+ lli[1] = new MODDMA_LLI;
+
+ // Prepare DMA configuration, which will be chained (one operation for each buffer)
+ dmaConf = new MODDMA_Config;
+ dmaConf
+ ->channelNum ( MODDMA::Channel_0 )
+ ->srcMemAddr ( 0 )
+ ->dstMemAddr ( (uint32_t)sampleBuffer0 )
+ ->transferSize ( sampleWindow )
+ ->transferType ( MODDMA::p2m )
+ ->transferWidth ( MODDMA::word )
+ ->srcConn ( MODDMA::ADC )
+ ->dstConn ( 0 )
+ ->dmaLLI ( (uint32_t)lli[1] ) // Looks like it does the above setup and then calls this LLI - thus we have this setup mimic lli[0] and then the chain actually starts by calling lli[1]
+ ->attach_tc ( &TC0_callback )
+ ->attach_err ( &ERR0_callback )
+ ;
+ // Create LLI to transfer from ADC to adcBuffer0 (and then launch lli[1])
+ lli[0]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
+ lli[0]->DstAddr = (uint32_t)sampleBuffer0;
+ lli[0]->NextLLI = (uint32_t) lli[1];
+ lli[0]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
+ | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
+ | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
+ | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
+ | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
+ | dma.CxControl_DI()
+ | dma.CxControl_I();
+ // Create LLI to transfer from ADC to adcBuffer1 (and then launch lli[0] to repeat)
+ lli[1]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
+ lli[1]->DstAddr = (uint32_t)sampleBuffer1;
+ lli[1]->NextLLI = (uint32_t) lli[0];
+ lli[1]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
+ | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
+ | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
+ | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
+ | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
+ | dma.CxControl_DI()
+ | dma.CxControl_I();
+
+ // Start the DMA chain
+ fillingBuffer0 = true;
+ transferComplete = false;
+ if (!dma.Prepare(dmaConf)) {
+ error("Doh! Error preparing initial dma configuration");
+ }
+}
+
+// Configure the ADC to trigger on Timer1
+// Start the timer with the desired sampling interval
+void ToneDetector::startADC()
+{
+ // We use the ADC irq to trigger DMA and the manual says
+ // that in this case the NVIC for ADC must be disabled.
+ NVIC_DisableIRQ(ADC_IRQn);
+
+ // Power up the ADC and set PCLK
+ LPC_SC->PCONP |= (1UL << 12); // enable power
+ LPC_SC->PCLKSEL0 &= ~(3UL << 24); // Clear divider to use CCLK/8 = 12MHz directly (see page 57) // original example code comment: PCLK = CCLK/4 96M/4 = 24MHz
+
+ // Enable the ADC, 12MHz, ADC0.0
+ LPC_ADC->ADCR = (1UL << 21);
+ LPC_ADC->ADCR &= ~(255 << 8); // No clock divider (use the 12MHz directly)
+
+ // Set the pin functions to ADC (use pin p15)
+ LPC_PINCON->PINSEL1 &= ~(3UL << 14); /* P0.23, Mbed p15. */
+ LPC_PINCON->PINSEL1 |= (1UL << 14);
+
+ // Enable ADC irq flag (to DMA).
+ // Note, don't set the individual flags,
+ // just set the global flag.
+ LPC_ADC->ADINTEN = 0x100;
+
+ // (see page 586 of http://www.nxp.com/documents/user_manual/UM10360.pdf)
+ // Disable burst mode
+ LPC_ADC->ADCR &= ~(1 << 16);
+ // Have the ADC convert based on timer 1
+ LPC_ADC->ADCR |= (6 << 24); // Trigger on MAT1.0
+ LPC_ADC->ADCR |= (1 << 27); // Falling edge
+
+ // Set up timer 1
+ LPC_SC->PCONP |= (1UL << 2); // Power on Timer1
+ LPC_SC->PCLKSEL0 &= ~(3 << 4); // No clock divider (use 12MHz directly) (see page 57 of datasheet)
+ LPC_TIM1->PR = 11; // TC clocks at 1MHz since we selected 12MHz above (see page 507 of datasheet)
+ LPC_TIM1->MR0 = sampleInterval-1; // sampling interval in us
+ LPC_TIM1->MCR = 3; // Reset TCR to zero on match
+ LPC_TIM1->EMR = (3UL<<4)|1; // Make MAT1.0 toggle.
+ //NVIC_EnableIRQ(TIMER1_IRQn); // Enable timer1 interrupt NOTE: enabling the interrupt when MCR is 3 will make everything stop working. enabling the interrupt when MCR is 2 will work but the interrupt isn't actually called.
+
+ // Start the timer (which thus starts the ADC)
+ LPC_TIM1->TCR=0;
+ LPC_TIM1->TCR=1;
+}
+#endif // end if(not artificial samples mode)
+
+//============================================
+// Execution Control
+//============================================
+
+// Start acquiring and processing samples
+// Will run forever or until stop() is called
+void ToneDetector::run()
+{
+ if(!readyToBegin)
+ return;
+ terminated = false;
+ //printf("\nTone detector starting...\n");
+ #ifdef recordStreaming
+ #ifdef recordSamples
+ printf("\tSample (V)");
+ printf("\n");
+ #endif
+ #ifdef recordOutput
+ for(uint8_t t = 0; t < numTones; t++)
+ printf("\t%f Hz", targetTones[t]);
+ printf("\n");
+ #endif
+ #endif
+
+ // Set up initial buffer configuration
+ samplesWriting = sampleBuffer0;
+ samplesProcessing = sampleBuffer1;
+ fillingBuffer0 = true;
+ transferComplete = false;
+ // Start periodically sampling
+ #ifdef artificialSamplesMode
+ initTestModeSamples(); // artificially create samples
+ sampleTicker.attach_us(&toneDetector, &ToneDetector::tickerCallback, sampleInterval); // "sample" artificial samples at desired rate
+ #else
+ startDMA();
+ if(!terminated) // If DMA got an error, terminated will be set true
+ startADC();
+ #endif
+
+ #ifdef debugLEDs
+ led1 = 1; // Indicate start of tone detection
+ #endif
+ #ifdef debugPins // after LEDs to be more accurate
+ debugPin1 = 1; // Indicate start of tone detection
+ #endif
+
+ // Main loop
+ // Wait for buffers to fill and then process them
+ while(!terminated)
+ {
+ // Check if a buffer of samples is gathered and if so process it
+ if(transferComplete)
+ {
+ transferComplete = false;
+ processSamples();
+ }
+ }
+
+ #ifdef debugPins // before LEDs to be more accurate
+ debugPin1 = 0; // Indicate cessation of tone detection
+ debugPin2 = 0; // Turn off indicator that at least two buffers were processed
+ debugPin4 = 1; // Indicate completion
+ #endif
+ #ifdef debugLEDs
+ led1 = 0; // Indicate cessation of tone detection
+ led2 = 0; // Turn off indicator that at least one buffer was processed
+ led4 = 1; // Indicate completion
+ #endif
+}
+
+// Finish up (write results to file and whatnot)
+// This is separate method so that main program can time the running itself without this extra overhead
+void ToneDetector::finish()
+{
+ //printf("Tone detector finished\n");
+
+ #if defined(recordSamples) && !defined(recordStreaming)
+ // Write saved samples to file
+ LocalFileSystem local("local");
+ FILE *foutSamples = fopen("/local/samples.wp", "w"); // Open "samples.wp" on the local file system for writing
+ fprintf(foutSamples, "Sample (V)\n");
+ uint16_t savedSamplesN = savedSamplesIndex;
+ do
+ {
+ fprintf(foutSamples, "%f\n", toFloat(savedSamples[savedSamplesN])/4.0*3.3);
+ savedSamplesN++;
+ savedSamplesN %= numSavedSamples;
+ } while(savedSamplesN != savedSamplesIndex);
+ fclose(foutSamples);
+ #endif // recordSamples && !recordStreaming
+ #if defined(recordOutput) && !defined(recordStreaming)
+ // Write saved outputs to file
+ #ifndef recordSamples
+ LocalFileSystem local("local");
+ #endif // not recordSamples
+ FILE *foutOutput = fopen("/local/out.wp", "w"); // Open "out.wp" on the local file system for writing
+ for(uint8_t t = 0; t < numTones; t++)
+ fprintf(foutOutput, "%f Hz\t", tones[t]);
+ uint16_t savedTonePowersN = savedTonePowersIndex;
+ do
+ {
+ for(uint8_t t = 0; t < numTones; t++)
+ fprintf(foutOutput, "%ld \t", savedTonePowers[savedTonePowersN][t]);
+ fprintf(foutOutput, "\n");
+ savedTonePowersN++;
+ savedTonePowersN %= numSavedTonePowers;
+ } while(savedTonePowersN != savedTonePowersIndex);
+ fclose(foutOutput);
+ #endif // recordOutput
+}
+
+// Terminate the tone detector
+// Note: Will actually terminate after next time buffer1 is filled, so won't be instantaneous
+void ToneDetector::stop()
+{
+ // Stop sampling
+ #ifdef artificialSamplesMode
+ sampleTicker.detach();
+ #else
+ lli[1]->Control = 0; // Make the DMA stop after next time buffer1 is filled
+ while(!(!fillingBuffer0 && transferComplete)); // Wait for buffer1 to be filled
+ LPC_TIM1->TCR=0; // Stop the timer (and thus the ADC)
+ LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
+ #endif
+
+ // Stop the main loop
+ terminated = true;
+}
+
+//============================================
+// Sampling / Processing
+//============================================
+
+// Acquire a new sample
+#ifdef artificialSamplesMode
+// If a buffer has been filled, swap buffers and signal the main thread to process it
+void ToneDetector::tickerCallback()
+{
+ // Get a sample
+ samplesWriting[sampleIndex] = testSamples[testSampleIndex];
+ testSampleIndex++;
+ testSampleIndex %= numTestSamples;
+
+ // Increment sample index
+ sampleIndex++;
+ sampleIndex %= sampleWindow;
+
+ // See if we just finished a buffer
+ if(sampleIndex == 0)
+ {
+ // Swap writing and processing buffers
+ // Let the main tone detector thread know that processing should take place
+ if(fillingBuffer0)
+ {
+ samplesProcessing = sampleBuffer0;
+ samplesWriting = sampleBuffer1;
+ }
+ else
+ {
+ samplesProcessing = sampleBuffer1;
+ samplesWriting = sampleBuffer0;
+ }
+ transferComplete = true;
+ fillingBuffer0 = !fillingBuffer0;
+ }
+}
+#else // not artificial mode - we want real samples!
+// Callback for DMA channel 0
+void TC0_callback(void) // static method
+{
+ // Swap writing and processing buffers used by main loop
+ if(toneDetector.fillingBuffer0)
+ {
+ toneDetector.samplesProcessing = toneDetector.sampleBuffer0;
+ toneDetector.samplesWriting = toneDetector.sampleBuffer1;
+ }
+ else
+ {
+ toneDetector.samplesProcessing = toneDetector.sampleBuffer1;
+ toneDetector.samplesWriting = toneDetector.sampleBuffer0;
+ }
+ // Tell main() loop that this buffer is ready for processing
+ toneDetector.fillingBuffer0 = !toneDetector.fillingBuffer0;
+ toneDetector.transferComplete = true;
+
+ // Clear DMA IRQ flags.
+ if(toneDetector.dma.irqType() == MODDMA::TcIrq) toneDetector.dma.clearTcIrq();
+ if(toneDetector.dma.irqType() == MODDMA::ErrIrq) toneDetector.dma.clearErrIrq();
+}
+
+// Configuration callback on Error for channel 0
+void ERR0_callback(void) // static method
+{
+ // Stop sampling
+ LPC_TIM1->TCR = 0; // Stop the timer (and thus the ADC)
+ LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
+
+ // Stop the main loop (don't call stop() since that would wait for next buffer to fill)
+ toneDetector.terminated = true;
+
+ error("Oh no! My Mbed EXPLODED! :( Only kidding, go find the problem (DMA chan 0)");
+}
+#endif
+
+// Goertzelize the process buffer
+void ToneDetector::processSamples()
+{
+ #ifdef debugLEDs
+ if(fillingBuffer0)
+ led2 = 1; // Indicate that at least two buffers have been recorded
+ led3 = 1; // Indicate start of processing
+ #endif
+ #ifdef debugPins // after LEDs and timer to be more accurate
+ if(fillingBuffer0)
+ debugPin2 = 1; // Indicate that at least two buffers have been recorded
+ debugPin3 = 1; // Indicate start of processing
+ #endif
+
+ // Create variables for storing the Goertzel series
+ int32_t s0, s1, s2;
+ volatile int32_t newTonePower;
+ // Create variables for getting max input value
+ int32_t sample = 0;
+ uint32_t signalLevel = 0;
+ // For each desired tone, compute power and then reset the Goertzel
+ for(uint8_t i = 0; i < numTones; i++)
+ {
+ // Reset
+ s0 = 0;
+ s1 = 0;
+ s2 = 0;
+ // Compute the Goertzel series
+ for(uint16_t n = 0; n < sampleWindow; n++)
+ {
+ // Note: bottom 4 bits from ADC indicate channel, top 12 are the actual data
+ // Note: Assuming Q10 format, we are automatically scaling the ADC value to [0, 4] range
+ sample = ((int32_t)((samplesProcessing[n] >> 4) & 0xFFF));
+ // Get signal level indicator
+ if(i == 0)
+ {
+ if(sample-2048 >= 0)
+ signalLevel += (sample-2048);
+ else
+ signalLevel += (2048-sample);
+ }
+ // TODO check the effect of this subtraction?
+ //sample -= 2048;
+ s0 = sample + fixedPointMultiply(goertzelCoefficients[i], s1) - s2;
+ s2 = s1;
+ s1 = s0;
+ }
+ // Compute the power
+ newTonePower = fixedPointMultiply(s2,s2) + fixedPointMultiply(s1,s1) - fixedPointMultiply(fixedPointMultiply(goertzelCoefficients[i],s1),s2);
+ // Update the running sum
+ tonePowersSum[i] -= tonePowers[i][tonePowersWindowIndex];
+ tonePowersSum[i] += newTonePower;
+ // Update the history of powers
+ tonePowers[i][tonePowersWindowIndex] = newTonePower;
+ }
+ // See if first circular buffer has been filled
+ readyToThreshold = readyToThreshold || (tonePowersWindowIndex == tonePowersWindow-1);
+ // Deliver results if a callback function was provided
+ if(callbackFunction != 0 && readyToThreshold)
+ callbackFunction(tonePowersSum, signalLevel >> 7); // divide signal level by 128 is basically making it an average (125 samples/buffer)
+ #ifdef streamAcousticControlLog
+ acousticControlLogToStream[0] = tonePowers[0][tonePowersWindowIndex];
+ acousticControlLogToStream[1] = tonePowers[1][tonePowersWindowIndex];
+ acousticControlLogToStream[2] = signalLevel >> 7;
+ #endif
+ #ifdef debugLEDs
+ led3 = 0; // Indicate completion of processing
+ #endif
+ #ifdef recordSamples
+ #ifdef recordStreaming
+ for(uint16_t n = 0; n < sampleWindow; n++)
+ printf("%ld\n", (samplesProcessing[n] >> 4) & 0xFFF);
+ #else
+ for(uint16_t n = 0; n < sampleWindow; n++)
+ {
+ savedSamples[savedSamplesIndex++] = (samplesProcessing[n] >> 4) & 0xFFF;
+ savedSamplesIndex %= numSavedSamples;
+ }
+ #endif
+ #endif
+ #ifdef recordOutput
+ #ifdef recordStreaming
+ for(uint8_t t = 0; t < numTones; t++)
+ printf("%ld\t", tonePowers[t][tonePowersWindowIndex] >> 10); // used to shift 10
+ printf("\n");
+ #else
+ for(uint8_t t = 0; t < numTones; t++)
+ {
+ savedTonePowers[savedTonePowersIndex][t] = tonePowers[t][tonePowersWindowIndex];
+ }
+ savedTonePowersIndex++;
+ savedTonePowersIndex %= numSavedTonePowers;
+ #endif
+ #endif
+ // Increment window index (circularly)
+ tonePowersWindowIndex = (tonePowersWindowIndex+1) % tonePowersWindow;
+
+ #ifdef debugPins // before LEDs, timer, and recordSamples to be more accurate
+ debugPin3 = 0; // Indicate completion of processing
+ #endif
+}
+
+int32_t* ToneDetector::getTonePowers()
+{
+ return tonePowersSum;
+}
+
+//============================================
+// Testing / Debugging
+//============================================
+
+#ifdef artificialSamplesMode
+// Use artificial samples, either from a file or from summing cosine waves of given frequencies
+// Samples in a file will be interpreted as volts, so should be in range [0, 3.3]
+void ToneDetector::initTestModeSamples()
+{
+ #ifdef sampleFilename
+ LocalFileSystem local("local");
+ printf("Using samples from file: "); printf(sampleFilename); printf("\n");
+ FILE *fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
+ if(fin == 0)
+ {
+ printf(" *** Cannot open file! ***\n");
+ wait_ms(3000);
+ numTestSamples = 1;
+ testSamples = new uint32_t[numTestSamples];
+ testSamples[0] = 0;
+ testSampleIndex = 0;
+ return;
+ }
+ // See how long the file is
+ int numTestSamples = 0;
+ float val;
+ float maxVal = 0;
+ float minVal = 0;
+ float avgVal = 0;
+ int res = fscanf(fin, "%d\n", &val);
+ while(res > 0)
+ {
+ numTestSamples++;
+ res = fscanf(fin, "%f\n", &val);
+ if(val > maxVal)
+ maxVal = val;
+ if(val < minVal)
+ minVal = val;
+ avgVal = numTestSamples > 1 ? (avgVal + val)/2.0 : val;
+ }
+ printf(" Found %d samples in the file, max %f, min %f, avg %f\n", numTestSamples, maxVal, minVal, avgVal);
+ if(minVal < 0)
+ printf(" WARNING: File supposed to represent voltage input to ADC, so negative numbers will be interpreted as 0\n");
+ if(maxVal > 3.3)
+ printf(" WARNING: File supposed to represent voltage input to ADC, so numbers greater than 3.3 will be interpreted as 3.3\n");
+ fclose(fin);
+ // Read the samples
+ testSamples = new uint32_t[numTestSamples];
+ fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
+ for(int i = 0; i < numTestSamples; i++)
+ {
+ // Read the voltage
+ fscanf(fin, "%f\n", &val);
+ // Clip it like the ADC would
+ val = val > 3.3 ? 3.3 : val;
+ val = val < 0 ? 0 : val;
+ // Convert voltage to 12-bit ADC reading
+ testSamples[i] = val/3.3*4096.0;
+ // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
+ testSamples[i] = testSamples[i] << 4;
+ }
+ fclose(fin);
+ testSampleIndex = 0;
+ sampleIndex = 0;
+
+ #else // not using file for samples, will create a sum of cosine waves instead
+
+ numTestSamples = 1000;
+ testSamples = new uint32_t[numTestSamples];
+ testSampleIndex = 0;
+
+ // Adjust overall amplitude and offset - make sure it's nonnegative
+ float amplitude = 1; // volts
+ float baseline = 1.5; // volts
+ // Print out the frequencies being used
+ float frequencies[] = sumSampleFrequencies; // Test signal will be a summation of cosines at these frequencies (in Hz)
+ printf("Using samples from cosines with the following frequencies:\n");
+ for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
+ printf(" %f\n", frequencies[f]);
+ printf("\n");
+
+ // Create the samples
+ float sampleFrequency = 1000.0/((double)sampleInterval/1000.0);
+ for(uint16_t n = 0; n < numTestSamples; n++)
+ {
+ float nextSample = 0;
+ // Sum the frequencies
+ for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
+ nextSample += cos(2.0*PI*frequencies[f]*(float)n/sampleFrequency);
+ // Normalize
+ nextSample /= (float)(sizeof(frequencies)/sizeof(float));
+ // Amplify
+ nextSample *= amplitude;
+ // Positivify
+ nextSample += baseline;
+ // Convert to 12-bit ADC reading
+ testSamples[n] = ((uint32_t)(nextSample/3.3*4095.0));
+ // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
+ testSamples[n] = testSamples[n] << 4;
+ }
+ sampleIndex = 0;
+ #endif // which sample mode
+}
+#endif // artificialSamplesMode
+
+#endif // #ifdef acousticControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/AcousticControl/ToneDetector.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,182 @@
+/**
+ * Author: Joseph DelPreto
+ * A class for sampling a signal and detecting whether certain frequencies are present
+ * Implements the Goertzel algorithm for tone detection
+ * Uses fixed-point arithmetic to a > 10x speedup over a floating-point implementation
+ * Uses Q15 number format
+ *
+ * Can easily configure the sampling frequency, the tones to detect, and the length of the buffer window
+ * Can set a callback function to be called at the end of each buffer processing to see the relative powers of each target tone
+ * Make sure the callback function execution time, plus the time to process a buffer of samples, takes less time than it takes to acquire the buffer!
+ * You can check how long it takes to process a buffer of samples by enabling "timeProcess" below then using initProcessTimer() and getProcessTimes()
+ *
+ * Debugging options such as LEDs to indicate status are also available
+ * Can also choose to time each processing stage to ensure it is not taking longer the time needed to acquire the buffer of samples
+ * Can also record samples and write them to a file (max 1000 will be recorded)
+ * LED pattern if enabled:
+ * LED1 indicates tone detection in progress
+ * LED2 indicates at least two buffers have been filled
+ * LED3 turns on when processing starts and off when it finishes (so lower duty cycle is better)
+ * LED4 turns on when tone detection is stopped (and then other LEDs turn off)
+ * Debug pins will go high/low in same pattern as described above for LEDs, but using pins p5-p8
+ *
+ * Test mode can be enabled, in which case samples can be artificially generated instead of using the ADC
+ *
+ * Currently only set up for one instance of ToneDetector to be in operation
+ * Use the object toneDetector, declared in this file, for all of your tone detecting needs!
+ *
+ */
+
+#define acousticControl
+
+#ifdef acousticControl
+
+#ifndef TONE_DETECTOR_H
+#define TONE_DETECTOR_H
+
+//#define streamAcousticControlLog // for every buffer, streams goertzel powers f1, goerztel powers f2, signal level, gain, fish state/events over Serial
+
+// Configuration
+#define sampleInterval 4 // us between samples
+#define sampleWindow 125 // number of samples in a Goertzel buffer
+#define numTones 2
+#define numFSKGroups 1
+static const double targetTones[numTones] = {36000, 30000}; // Tones to detect (in Hz): first tone is a 0 bit second is a 1 bit
+
+// Test / Debugging options
+//#define artificialSamplesMode // if this is defined, should define either sampleFilename OR sumSampleFrequencies
+//#define sampleFilename "/local/2tone11.txt"
+//#define sumSampleFrequencies {10000,30000} // will be used to initialize float[] array. Test signal will be summation of cosines with these frequencies (in Hz)
+
+#define debugLEDs
+#define debugPins
+#define recordStreaming // print to COM each sample/output (save everything), as opposed to only write last few hundred to a file (but faster since don't write to file while processing)
+ // note that either recordOutput or recordSamples must be undefined to actually record anything
+//#define recordOutput // save tone powers - will either stream to COM or save the last numSavedTonePowers powers to a file (based on recordStreaming)
+//#define recordSamples // save samples - will either stream to COM or save the last numSavedSamples samples to a file (based on recordStreaming)
+
+
+#if defined(recordSamples) && !defined(recordStreaming)
+#define numSavedSamples 1000
+#endif
+#if defined(recordOutput) && !defined(recordStreaming)
+#define numSavedTonePowers 250
+#endif
+
+// Will use fixed-point arithmetic for speed
+// numbers will be int32_t with 10 bits as fractional portion
+// note: this means the 12 bits from the ADC can be used directly, implicitly scaling them to be floats in range [0,4]
+// (so if you want to change the Q format, make sure to shift the samples accordingly in the processSamples() method)
+#define toFixedPoint(floatNum) ((int32_t)((float)(floatNum) * 1024.0f))
+#define toFloat(fixedPointNum) ((float)(fixedPointNum)/1024.0f)
+#define fixedPointMultiply(a,b) ((int32_t)(((int64_t)a * (int64_t)b) >> 10))
+
+// Constants
+#define tonePowersWindow 32 // change to 5 for threshold1
+
+// Include files
+#include <stdint.h> // for types like int32_t
+#include <math.h> // for cos // TODO remove this once samples are real samples
+#define PI 3.1415926 // not included with math.h for some reason
+#include "mbed.h" // for the ticker
+#ifndef artificialSamplesMode
+#include "MODDMA.h" // DMA for reading the ADC
+#endif
+
+// Debugging
+#ifdef debugLEDs
+static DigitalOut led1(LED1);
+static DigitalOut led2(LED2);
+static DigitalOut led3(LED3);
+static DigitalOut led4(LED4);
+#endif
+#ifdef debugPins
+static DigitalOut debugPin1(p5);
+static DigitalOut debugPin2(p6);
+static DigitalOut debugPin3(p7);
+static DigitalOut debugPin4(p8);
+#endif
+
+// Define the ToneDetector!
+class ToneDetector
+{
+ public:
+ // Initialization
+ ToneDetector();
+ void init();
+ void setCallback(void (*myFunction)(int32_t* tonePowers, uint32_t signalLevel));
+ // Execution Control
+ virtual void run(); // main loop, runs forever or until stop() is called
+ virtual void stop(); // stop the main loop
+ void finish(); // write final output to files and whatnot
+ volatile bool terminated; // indicates main loop should stop (may be set by error callback or by stop())
+ // Sampling / Processing (these are public to allow DMA callbacks to access them)
+ volatile bool fillingBuffer0; // Whether sampling is currently writing to buffer 0 or buffer 1
+ volatile bool transferComplete; // Signals that samplesProcessing is ready to be processed
+ uint32_t sampleBuffer0[sampleWindow]; // Buffer of samples used by one DMA operation
+ uint32_t sampleBuffer1[sampleWindow]; // Buffer of samples used by second DMA operation
+ volatile uint32_t* samplesWriting; // Pointer to the buffer to which we should write (alternates between buffer0 and buffer1)
+ volatile uint32_t* samplesProcessing; // Pointer to the buffer which we should process (alternates between buffer0 and buffer1)
+ int32_t* getTonePowers(); // Get the most recent tone powers
+ #ifndef artificialSamplesMode
+ MODDMA dma; // DMA controller
+ #endif
+
+ private:
+ // Initialization
+ bool readyToBegin; // Whether sample window, sample interval, and tones have all been specified
+ #ifndef artificialSamplesMode
+ MODDMA_Config *dmaConf; // Overall DMA configuration
+ MODDMA_LLI *lli[2]; // Linked List Items for chaining DMA operations (will have one for each ADC buffer)
+ void startDMA();
+ void startADC();
+ #endif
+ // Sampling
+ // Goertzel Processing (arrays will have an element per desired tone)
+ // Tone detecting
+ int32_t goertzelCoefficients[numTones]; // Pre-computed coefficients based on target tones
+ int32_t tonePowersSum[numTones]; // Result of Goertzel algorithm (summed over tonePowersWindow to smooth results)
+ int32_t tonePowers[numTones][tonePowersWindow]; // For each tone, store the most recent tonePowersWindow powers (in circular buffers)
+ uint8_t tonePowersWindowIndex; // Index into the circular buffers of tonePowers
+ bool readyToThreshold; // Whether thresholding is ready, or first buffer is still being filled
+ void (*callbackFunction)(int32_t* tonePowers, uint32_t signalLevel); // Called when new powers are computed
+ void processSamples(); // Process a buffer of samples to detect tones
+
+ // Testing / Debugging
+ #ifdef artificialSamplesMode
+ Ticker sampleTicker; // Triggers a sample to be read from the pre-filled array
+ void initTestModeSamples(); // Creates the samples, either from a file or by summing tones
+ void tickerCallback(); // Called each time a sample is read
+ uint32_t* testSamples; // Array of pre-stored sample values to use
+ uint16_t numTestSamples; // Length of the testSamples array (need not be same as buffer length)
+ volatile uint16_t testSampleIndex; // Current index into testSamples
+ volatile uint16_t sampleIndex; // Index into current sample buffer
+ #endif
+ #ifndef recordStreaming
+ #ifdef recordSamples
+ uint16_t savedSamplesIndex;
+ uint32_t savedSamples[numSavedSamples];
+ #endif
+ #ifdef recordOutput
+ uint16_t savedTonePowersIndex;
+ int32_t savedTonePowers[numSavedTonePowers][numTones];
+ #endif
+ #endif
+};
+
+// DMA IRQ callback methods
+void TC0_callback(); // Callback for when DMA channel 0 has filled a buffer
+void ERR0_callback(); // Error on dma channel 0
+
+// Create a static instance of ToneDetector to be used by anyone doing detection
+// This allows the ticker callback to be a member function
+extern ToneDetector toneDetector;
+#ifdef streamAcousticControlLog
+extern int32_t acousticControlLogToStream[5]; // goertzel powers f1, goerztel powers f2, signal level, gain, events (events is deprecated)
+#endif
+
+#endif // ifndef TONE_DETECTOR_H
+
+#endif // #ifdef acousticControl
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/BNO055/BNO055.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,280 @@
+#include "BNO055.h"
+#include "mbed.h"
+
+BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
+ //Set I2C fast and bring reset line high
+ _i2c.frequency(400000);
+ address = BNOAddress;
+ accel_scale = 0.001f;
+ rate_scale = 1.0f/16.0f;
+ angle_scale = 1.0f/16.0f;
+ temp_scale = 1;
+ }
+
+void BNO055::reset(){
+//Perform a power-on-reset
+ readchar(BNO055_SYS_TRIGGER_ADDR);
+ rx = rx | 0x20;
+ writechar(BNO055_SYS_TRIGGER_ADDR,rx);
+//Wait for the system to come back up again (datasheet says 650ms)
+ wait_ms(675);
+}
+
+bool BNO055::check(){
+//Check we have communication link with the chip
+ readchar(BNO055_CHIP_ID_ADDR);
+ if (rx != 0xA0) return false;
+//Grab the chip ID and software versions
+ tx[0] = BNO055_CHIP_ID_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,7,false);
+ ID.id = rawdata[0];
+ ID.accel = rawdata[1];
+ ID.mag = rawdata[2];
+ ID.gyro = rawdata[3];
+ ID.sw[0] = rawdata[4];
+ ID.sw[1] = rawdata[5];
+ ID.bootload = rawdata[6];
+ setpage(1);
+ tx[0] = BNO055_UNIQUE_ID_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,ID.serial,16,false);
+ setpage(0);
+ return true;
+ }
+
+void BNO055::SetExternalCrystal(bool yn){
+// Read the current status from the device
+ readchar(BNO055_SYS_TRIGGER_ADDR);
+ if (yn) rx = rx | 0x80;
+ else rx = rx & 0x7F;
+ writechar(BNO055_SYS_TRIGGER_ADDR,rx);
+}
+
+void BNO055::set_accel_units(char units){
+ readchar(BNO055_UNIT_SEL_ADDR);
+ if(units == MPERSPERS){
+ rx = rx & 0xFE;
+ accel_scale = 0.01f;
+ }
+ else {
+ rx = rx | units;
+ accel_scale = 0.001f;
+ }
+ writechar(BNO055_UNIT_SEL_ADDR,rx);
+}
+
+void BNO055::set_anglerate_units(char units){
+ readchar(BNO055_UNIT_SEL_ADDR);
+ if (units == DEG_PER_SEC){
+ rx = rx & 0xFD;
+ rate_scale = 1.0f/16.0f;
+ }
+ else {
+ rx = rx | units;
+ rate_scale = 1.0f/900.0f;
+ }
+ writechar(BNO055_UNIT_SEL_ADDR,rx);
+}
+
+void BNO055::set_angle_units(char units){
+ readchar(BNO055_UNIT_SEL_ADDR);
+ if (units == DEGREES){
+ rx = rx & 0xFB;
+ angle_scale = 1.0f/16.0f;
+ }
+ else {
+ rx = rx | units;
+ rate_scale = 1.0f/900.0f;
+ }
+ writechar(BNO055_UNIT_SEL_ADDR,rx);
+}
+
+void BNO055::set_temp_units(char units){
+ readchar(BNO055_UNIT_SEL_ADDR);
+ if (units == CENTIGRADE){
+ rx = rx & 0xEF;
+ temp_scale = 1;
+ }
+ else {
+ rx = rx | units;
+ temp_scale = 2;
+ }
+ writechar(BNO055_UNIT_SEL_ADDR,rx);
+}
+
+void BNO055::set_orientation(char units){
+ readchar(BNO055_UNIT_SEL_ADDR);
+ if (units == WINDOWS) rx = rx &0x7F;
+ else rx = rx | units;
+ writechar(BNO055_UNIT_SEL_ADDR,rx);
+}
+
+void BNO055::setmode(char omode){
+ writechar(BNO055_OPR_MODE_ADDR,omode);
+ op_mode = omode;
+}
+
+void BNO055::setpowermode(char pmode){
+ writechar(BNO055_PWR_MODE_ADDR,pmode);
+ pwr_mode = pmode;
+}
+
+void BNO055::get_accel(void){
+ tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ accel.rawx = (rawdata[1] << 8 | rawdata[0]);
+ accel.rawy = (rawdata[3] << 8 | rawdata[2]);
+ accel.rawz = (rawdata[5] << 8 | rawdata[4]);
+ accel.x = float(accel.rawx)*accel_scale;
+ accel.y = float(accel.rawy)*accel_scale;
+ accel.z = float(accel.rawz)*accel_scale;
+}
+
+void BNO055::get_gyro(void){
+ tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
+ gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
+ gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
+ gyro.x = float(gyro.rawx)*rate_scale;
+ gyro.y = float(gyro.rawy)*rate_scale;
+ gyro.z = float(gyro.rawz)*rate_scale;
+}
+
+void BNO055::get_mag(void){
+ tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ mag.rawx = (rawdata[1] << 8 | rawdata[0]);
+ mag.rawy = (rawdata[3] << 8 | rawdata[2]);
+ mag.rawz = (rawdata[5] << 8 | rawdata[4]);
+ mag.x = float(mag.rawx);
+ mag.y = float(mag.rawy);
+ mag.z = float(mag.rawz);
+}
+
+void BNO055::get_lia(void){
+ tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ lia.rawx = (rawdata[1] << 8 | rawdata[0]);
+ lia.rawy = (rawdata[3] << 8 | rawdata[2]);
+ lia.rawz = (rawdata[5] << 8 | rawdata[4]);
+ lia.x = float(lia.rawx)*accel_scale;
+ lia.y = float(lia.rawy)*accel_scale;
+ lia.z = float(lia.rawz)*accel_scale;
+}
+
+void BNO055::get_grv(void){
+ tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
+ gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
+ gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
+ gravity.x = float(gravity.rawx)*accel_scale;
+ gravity.y = float(gravity.rawy)*accel_scale;
+ gravity.z = float(gravity.rawz)*accel_scale;
+}
+
+void BNO055::get_quat(void){
+ tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,8,0);
+ quat.raww = (rawdata[1] << 8 | rawdata[0]);
+ quat.rawx = (rawdata[3] << 8 | rawdata[2]);
+ quat.rawy = (rawdata[5] << 8 | rawdata[4]);
+ quat.rawz = (rawdata[7] << 8 | rawdata[6]);
+ quat.w = float(quat.raww)/16384.0f;
+ quat.x = float(quat.rawx)/16384.0f;
+ quat.y = float(quat.rawy)/16384.0f;
+ quat.z = float(quat.rawz)/16384.0f;
+}
+
+void BNO055::get_angles(void){
+ tx[0] = BNO055_EULER_H_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address+1,rawdata,6,0);
+ euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
+ euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
+ euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
+ euler.yaw = float(euler.rawyaw)*angle_scale;
+ euler.roll = float(euler.rawroll)*angle_scale;
+ euler.pitch = float(euler.rawpitch)*angle_scale;
+}
+
+
+void BNO055::get_temp(void){
+ readchar(BNO055_TEMP_ADDR);
+ temperature = rx / temp_scale;
+}
+
+void BNO055::get_calib(void){
+ readchar(BNO055_CALIB_STAT_ADDR);
+ calib = rx;
+}
+
+void BNO055::read_calibration_data(void){
+ char tempmode = op_mode;
+ setmode(OPERATION_MODE_CONFIG);
+ wait_ms(20);
+ tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address,calibration,22,false);
+ setmode(tempmode);
+ wait_ms(10);
+}
+
+void BNO055::write_calibration_data(void){
+ char tempmode = op_mode;
+ setmode(OPERATION_MODE_CONFIG);
+ wait_ms(20);
+ tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
+ _i2c.write(address,tx,1,true);
+ _i2c.write(address,calibration,22,false);
+ setmode(tempmode);
+ wait_ms(10);
+}
+
+void BNO055::set_mapping(char orient){
+ switch (orient){
+ case 0:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
+ break;
+ case 1:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+ break;
+ case 2:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+ break;
+ case 3:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
+ break;
+ case 4:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
+ break;
+ case 5:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
+ break;
+ case 6:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
+ break;
+ case 7:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
+ break;
+ default:
+ writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+ writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/BNO055/BNO055.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,280 @@
+#ifndef BNO055_H
+#define BNO055_H
+
+#include "mbed.h"
+//
+#define BNOAddress (0x28 << 1)
+//Register definitions
+/* Page id register definition */
+#define BNO055_PAGE_ID_ADDR 0x07
+/* PAGE0 REGISTER DEFINITION START*/
+#define BNO055_CHIP_ID_ADDR 0x00
+#define BNO055_ACCEL_REV_ID_ADDR 0x01
+#define BNO055_MAG_REV_ID_ADDR 0x02
+#define BNO055_GYRO_REV_ID_ADDR 0x03
+#define BNO055_SW_REV_ID_LSB_ADDR 0x04
+#define BNO055_SW_REV_ID_MSB_ADDR 0x05
+#define BNO055_BL_REV_ID_ADDR 0x06
+/* Accel data register */
+#define BNO055_ACCEL_DATA_X_LSB_ADDR 0x08
+#define BNO055_ACCEL_DATA_X_MSB_ADDR 0x09
+#define BNO055_ACCEL_DATA_Y_LSB_ADDR 0x0A
+#define BNO055_ACCEL_DATA_Y_MSB_ADDR 0x0B
+#define BNO055_ACCEL_DATA_Z_LSB_ADDR 0x0C
+#define BNO055_ACCEL_DATA_Z_MSB_ADDR 0x0D
+/* Mag data register */
+#define BNO055_MAG_DATA_X_LSB_ADDR 0x0E
+#define BNO055_MAG_DATA_X_MSB_ADDR 0x0F
+#define BNO055_MAG_DATA_Y_LSB_ADDR 0x10
+#define BNO055_MAG_DATA_Y_MSB_ADDR 0x11
+#define BNO055_MAG_DATA_Z_LSB_ADDR 0x12
+#define BNO055_MAG_DATA_Z_MSB_ADDR 0x13
+/* Gyro data registers */
+#define BNO055_GYRO_DATA_X_LSB_ADDR 0x14
+#define BNO055_GYRO_DATA_X_MSB_ADDR 0x15
+#define BNO055_GYRO_DATA_Y_LSB_ADDR 0x16
+#define BNO055_GYRO_DATA_Y_MSB_ADDR 0x17
+#define BNO055_GYRO_DATA_Z_LSB_ADDR 0x18
+#define BNO055_GYRO_DATA_Z_MSB_ADDR 0x19
+/* Euler data registers */
+#define BNO055_EULER_H_LSB_ADDR 0x1A
+#define BNO055_EULER_H_MSB_ADDR 0x1B
+#define BNO055_EULER_R_LSB_ADDR 0x1C
+#define BNO055_EULER_R_MSB_ADDR 0x1D
+#define BNO055_EULER_P_LSB_ADDR 0x1E
+#define BNO055_EULER_P_MSB_ADDR 0x1F
+/* Quaternion data registers */
+#define BNO055_QUATERNION_DATA_W_LSB_ADDR 0x20
+#define BNO055_QUATERNION_DATA_W_MSB_ADDR 0x21
+#define BNO055_QUATERNION_DATA_X_LSB_ADDR 0x22
+#define BNO055_QUATERNION_DATA_X_MSB_ADDR 0x23
+#define BNO055_QUATERNION_DATA_Y_LSB_ADDR 0x24
+#define BNO055_QUATERNION_DATA_Y_MSB_ADDR 0x25
+#define BNO055_QUATERNION_DATA_Z_LSB_ADDR 0x26
+#define BNO055_QUATERNION_DATA_Z_MSB_ADDR 0x27
+/* Linear acceleration data registers */
+#define BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR 0x28
+#define BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR 0x29
+#define BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR 0x2A
+#define BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR 0x2B
+#define BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR 0x2C
+#define BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR 0x2D
+/* Gravity data registers */
+#define BNO055_GRAVITY_DATA_X_LSB_ADDR 0x2E
+#define BNO055_GRAVITY_DATA_X_MSB_ADDR 0x2F
+#define BNO055_GRAVITY_DATA_Y_LSB_ADDR 0x30
+#define BNO055_GRAVITY_DATA_Y_MSB_ADDR 0x31
+#define BNO055_GRAVITY_DATA_Z_LSB_ADDR 0x32
+#define BNO055_GRAVITY_DATA_Z_MSB_ADDR 0x33
+/* Temperature data register */
+#define BNO055_TEMP_ADDR 0x34
+/* Status registers */
+#define BNO055_CALIB_STAT_ADDR 0x35
+#define BNO055_SELFTEST_RESULT_ADDR 0x36
+#define BNO055_INTR_STAT_ADDR 0x37
+#define BNO055_SYS_CLK_STAT_ADDR 0x38
+#define BNO055_SYS_STAT_ADDR 0x39
+#define BNO055_SYS_ERR_ADDR 0x3A
+/* Unit selection register */
+#define BNO055_UNIT_SEL_ADDR 0x3B
+#define BNO055_DATA_SELECT_ADDR 0x3C
+/* Mode registers */
+#define BNO055_OPR_MODE_ADDR 0x3D
+#define BNO055_PWR_MODE_ADDR 0x3E
+#define BNO055_SYS_TRIGGER_ADDR 0x3F
+#define BNO055_TEMP_SOURCE_ADDR 0x40
+/* Axis remap registers */
+#define BNO055_AXIS_MAP_CONFIG_ADDR 0x41
+#define BNO055_AXIS_MAP_SIGN_ADDR 0x42
+/* Accelerometer Offset registers */
+#define ACCEL_OFFSET_X_LSB_ADDR 0x55
+#define ACCEL_OFFSET_X_MSB_ADDR 0x56
+#define ACCEL_OFFSET_Y_LSB_ADDR 0x57
+#define ACCEL_OFFSET_Y_MSB_ADDR 0x58
+#define ACCEL_OFFSET_Z_LSB_ADDR 0x59
+#define ACCEL_OFFSET_Z_MSB_ADDR 0x5A
+/* Magnetometer Offset registers */
+#define MAG_OFFSET_X_LSB_ADDR 0x5B
+#define MAG_OFFSET_X_MSB_ADDR 0x5C
+#define MAG_OFFSET_Y_LSB_ADDR 0x5D
+#define MAG_OFFSET_Y_MSB_ADDR 0x5E
+#define MAG_OFFSET_Z_LSB_ADDR 0x5F
+#define MAG_OFFSET_Z_MSB_ADDR 0x60
+/* Gyroscope Offset registers*/
+#define GYRO_OFFSET_X_LSB_ADDR 0x61
+#define GYRO_OFFSET_X_MSB_ADDR 0x62
+#define GYRO_OFFSET_Y_LSB_ADDR 0x63
+#define GYRO_OFFSET_Y_MSB_ADDR 0x64
+#define GYRO_OFFSET_Z_LSB_ADDR 0x65
+#define GYRO_OFFSET_Z_MSB_ADDR 0x66
+/* Radius registers */
+#define ACCEL_RADIUS_LSB_ADDR 0x67
+#define ACCEL_RADIUS_MSB_ADDR 0x68
+#define MAG_RADIUS_LSB_ADDR 0x69
+#define MAG_RADIUS_MSB_ADDR 0x6A
+
+/* Page 1 registers */
+#define BNO055_UNIQUE_ID_ADDR 0x50
+
+//Definitions for unit selection
+#define MPERSPERS 0x00
+#define MILLIG 0x01
+#define DEG_PER_SEC 0x00
+#define RAD_PER_SEC 0x02
+#define DEGREES 0x00
+#define RADIANS 0x04
+#define CENTIGRADE 0x00
+#define FAHRENHEIT 0x10
+#define WINDOWS 0x00
+#define ANDROID 0x80
+
+//Definitions for power mode
+#define POWER_MODE_NORMAL 0x00
+#define POWER_MODE_LOWPOWER 0x01
+#define POWER_MODE_SUSPEND 0x02
+
+//Definitions for operating mode
+#define OPERATION_MODE_CONFIG 0x00
+#define OPERATION_MODE_ACCONLY 0x01
+#define OPERATION_MODE_MAGONLY 0x02
+#define OPERATION_MODE_GYRONLY 0x03
+#define OPERATION_MODE_ACCMAG 0x04
+#define OPERATION_MODE_ACCGYRO 0x05
+#define OPERATION_MODE_MAGGYRO 0x06
+#define OPERATION_MODE_AMG 0x07
+#define OPERATION_MODE_IMUPLUS 0x08
+#define OPERATION_MODE_COMPASS 0x09
+#define OPERATION_MODE_M4G 0x0A
+#define OPERATION_MODE_NDOF_FMC_OFF 0x0B
+#define OPERATION_MODE_NDOF 0x0C
+
+typedef struct values{
+ int16_t rawx,rawy,rawz;
+ float x,y,z;
+ }values;
+
+typedef struct angles{
+ int16_t rawroll,rawpitch,rawyaw;
+ float roll, pitch, yaw;
+ } angles;
+
+typedef struct quaternion{
+ int16_t raww,rawx,rawy,rawz;
+ float w,x,y,z;
+ }quaternion;
+
+typedef struct chip{
+ char id;
+ char accel;
+ char gyro;
+ char mag;
+ char sw[2];
+ char bootload;
+ char serial[16];
+ }chip;
+
+/** Class for operating Bosch BNO055 sensor over I2C **/
+class BNO055
+{
+public:
+
+/** Create BNO055 instance **/
+ BNO055(PinName SDA, PinName SCL);
+
+/** Perform a power-on reset of the BNO055 **/
+ void reset();
+/** Check that the BNO055 is connected and download the software details
+and serial number of chip and store in ID structure **/
+ bool check();
+/** Turn the external timing crystal on/off **/
+ void SetExternalCrystal(bool yn);
+/** Set the operation mode of the sensor **/
+ void setmode(char mode);
+/** Set the power mode of the sensor **/
+ void setpowermode(char mode);
+
+/** Set the output units from the accelerometer, either MPERSPERS or MILLIG **/
+ void set_accel_units(char units);
+/** Set the output units from the gyroscope, either DEG_PER_SEC or RAD_PER_SEC **/
+ void set_anglerate_units(char units);
+/** Set the output units from the IMU, either DEGREES or RADIANS **/
+ void set_angle_units(char units);
+/** Set the output units from the temperature sensor, either CENTIGRADE or FAHRENHEIT **/
+ void set_temp_units(char units);
+/** Set the data output format to either WINDOWS or ANDROID **/
+ void set_orientation(char units);
+/** Set the mapping of the exes/directions as per page 25 of datasheet
+ range 0-7, any value outside this will set the orientation to P1 (default at power up) **/
+ void set_mapping(char orient);
+
+/** Get the current values from the accelerometer **/
+ void get_accel(void);
+/** Get the current values from the gyroscope **/
+ void get_gyro(void);
+/** Get the current values from the magnetometer **/
+ void get_mag(void);
+/** Get the corrected linear acceleration **/
+ void get_lia(void);
+/** Get the current gravity vector **/
+ void get_grv(void);
+/** Get the output quaternion **/
+ void get_quat(void);
+/** Get the current Euler angles **/
+ void get_angles(void);
+/** Get the current temperature **/
+ void get_temp(void);
+
+/** Read the calibration status register and store the result in the calib variable **/
+ void get_calib(void);
+/** Read the offset and radius values into the calibration array**/
+ void read_calibration_data(void);
+/** Write the contents of the calibration array into the registers **/
+ void write_calibration_data(void);
+
+/** Structures containing 3-axis data for acceleration, rate of turn and magnetic field.
+ x,y,z are the scale floating point values and
+ rawx, rawy, rawz are the int16_t values read from the sensors **/
+ values accel,gyro,mag,lia,gravity;
+/** Stucture containing the Euler angles as yaw, pitch, roll as scaled floating point
+ and rawyaw, rawroll & rollpitch as the int16_t values loaded from the registers **/
+ angles euler;
+/** Quaternion values as w,x,y,z (scaled floating point) and raww etc... as int16_t loaded from the
+ registers **/
+ quaternion quat;
+
+/** Current contents of calibration status register **/
+ char calib;
+/** Contents of the 22 registers containing offset and radius values used as calibration by the sensor **/
+ char calibration[22];
+/** Structure containing sensor numbers, software version and chip UID **/
+ chip ID;
+/** Current temperature **/
+ int temperature;
+
+ private:
+
+ I2C _i2c;
+ char rx,tx[2],address; //I2C variables
+ char rawdata[22]; //Temporary array for input data values
+ char op_mode;
+ char pwr_mode;
+ float accel_scale,rate_scale,angle_scale;
+ int temp_scale;
+
+void readchar(char location){
+ tx[0] = location;
+ _i2c.write(address,tx,1,true);
+ _i2c.read(address,&rx,1,false);
+}
+
+void writechar(char location, char value){
+ tx[0] = location;
+ tx[1] = value;
+ _i2c.write(address,tx,2);
+}
+
+void setpage(char value){
+ writechar(BNO055_PAGE_ID_ADDR,value);
+}
+ };
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/BrushlessMotor.lib Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/jetfishteam/code/ESC/#39e83e74b8a1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/BuoyancyControlUnit/BuoyancyControlUnit.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,239 @@
+#include <BuoyancyControlUnit/BuoyancyControlUnit.h>
+
+// The static instance
+BuoyancyControlUnit buoyancyControlUnit;
+
+//============================================
+// Initialization
+//============================================
+
+// Constructor
+BuoyancyControlUnit::BuoyancyControlUnit() :
+ // Initialize variables
+ //bcuPWM(bcuPwmPin),
+ bcuDirA(bcuDirAPin),
+ bcuDirB(bcuDirBPin),
+ //bcuCurrent(bcuCurrentPin),
+ wiper(wiperPin),
+ pressureSensor(PIN_IMU_TX,PIN_IMU_RX)
+{
+ setDepth = 0; // input command
+ curDepth = 0; // actual depth based on sensor
+
+ depthLastTime = 0.0;
+ prevDepthErr = 0.0;
+ depthInt = 0.0;
+
+ posLastTime = 0.0;
+ prevPosErr = 0.0;
+ posInt = 0.0;
+ VRef = 0;
+
+ resetFlag = 0;
+ inDepthLoop = 0;
+ inPosLoop = 0;
+}
+
+
+void BuoyancyControlUnit::start()
+{
+ bcuDirA.write(0.0); // apply nothing to start
+ bcuDirB.write(0.0); // apply nothing to start
+ //bcuEncoder.reset(); // gives it a reference to return to
+
+ pressureSensor.MS5837Init();
+ timer.start();
+
+ // pressure sensor takes just over 0.3 seconds to read
+ depthControl.attach(&buoyancyControlUnit, &BuoyancyControlUnit::setDepthFuncVoid, 0.4);
+ posControl.attach(&buoyancyControlUnit, &BuoyancyControlUnit::setEncoderPosVoid, 0.1);
+}
+
+void BuoyancyControlUnit::stop()
+{
+ //bcuPWM.write(0.0);
+ // return bcu back to start position
+ // have to rethink how to call this because we don't want the while loop stopping everything else
+// while(curPos > 15){
+// this->setEncoderPosition(0);
+// }
+
+ depthControl.detach();
+ posControl.detach();
+
+ bcuDirA = 0.0;
+ bcuDirB = 0.0;
+}
+
+void BuoyancyControlUnit::returnToZero()
+{
+ depthControl.detach();
+ resetFlag = 1;
+}
+
+//============================================
+// Processing
+//============================================
+void BuoyancyControlUnit::set(float depthDesIn)
+{
+ setDepth = depthDesIn;
+}
+
+void BuoyancyControlUnit::setDepthFuncVoid(){
+ while(inPosLoop);
+ inDepthLoop = 1;
+ this->setDepthFunc(setDepth);
+ inDepthLoop = 0;
+}
+void BuoyancyControlUnit::setEncoderPosVoid(){
+ while(inDepthLoop);
+ inPosLoop = 1;
+
+ if(resetFlag){
+ posRef = 0;
+ } else {
+ depthErr = setDepth - curDepth;
+ if(depthErr < 5 && depthErr > -5){
+ posRef = curPos;
+ }
+ }
+
+ this->setEncoderPosition(posRef);
+ inPosLoop = 0;
+}
+
+void BuoyancyControlUnit::setDepthFunc(float depthDesIn){
+ setDepth = depthDesIn;
+// pressureSensor.Barometer_MS5837();
+// curDepth = pressureSensor.MS5837_Pressure();
+/*
+TODO: Map measured depth to 0 - 30 using h = P/rg.
+#define MIN_DEPTH 1019.0 //mbar
+curDepth = curDepth * (fishMaxPitch - fishMinPitch)/(MAX_DEPTH - MIN_DEPTH)
+*/
+ curDepth = (curPos * 1.25); // fake depth readings
+
+ depthErr = setDepth - curDepth;
+
+/* float depthdt = timer.read() - depthLastTime;
+
+ depthInt += depthErr * depthdt;
+ depthDer = (depthErr - prevDepthErr)/depthdt;
+
+ posRef += KpDepth*depthErr + KiDepth*depthInt + KdDepth*depthDer;
+*/
+ posRef += KpDepth * depthErr;
+
+ if (posRef > 30){
+ posRef = 30;
+ }
+ else if (posRef < 0){
+ posRef = 0;
+ }
+
+// if(posRef < 0){
+// posRef = 0; // limit position to only values greater than 0
+// depthInt = 0; // reset these values so they don't build while the bcu can't reach this depth value
+// depthDer = 0;
+// }
+
+// depthLastTime = timer.read();
+// prevDepthErr = depthErr;
+}
+
+
+void BuoyancyControlUnit::setEncoderPosition(float setPosIn) {
+
+ setPos = setPosIn;
+ if(setPos < 0){ setPos = 0; }
+ curPos = wiper * (30.0); //map ADC reading to system level voltage, try adjusting pitch max and min values (0 - 30)
+ posErr = setPos - curPos;
+ float posdt = timer.read() - posLastTime;
+
+ posInt += posErr * posdt;
+ posDer = (posErr - prevPosErr)/posdt;
+
+ VRef = KpEnc * posErr + KiEnc * posInt + KdEnc * posDer;
+// VRef = KpEnc * posErr;
+
+ /* Add constrain + map here */
+ if (VRef > 6.0){
+ VRef = 6.0;
+ }
+ else if (VRef < -6.0){
+ VRef = -6.0;
+ }
+
+ // Map Vref to 0.7 - 1.0 duty cycle
+ if (VRef < 0){
+ VRef = VRef * (0.3/(-6.0)) + 0.7;
+ bcuDir = 1;
+ }
+ else if (VRef > 0){
+ VRef = VRef * (0.3/(6.0)) + 0.7;
+ bcuDir = 0;
+ }
+
+ posLastTime = timer.read();
+ prevPosErr = posErr;
+
+ if(resetFlag){
+ if(posErr > 10 || posErr < -10){
+ this->setV(VRef);
+ } else {
+ resetFlag = 0;
+ this->stop();
+ }
+ } else {
+ if(posErr > 5 || posErr < -5) { // TODO: determine appropriate tolerance (final error from target pos?)
+ this->setV(VRef);
+ #ifdef debugBCU
+ DigitalOut test(LED1);
+ test = 0;
+ #endif
+ } else {
+ bcuDirA = 0.0;
+ bcuDirB = 0;
+ #ifdef debugBCU
+ //Testing whether fishController is running
+ DigitalOut test(LED1);
+ test = 1;
+ #endif
+ }
+ }
+}
+
+void BuoyancyControlUnit::setV(float Vin){
+ float setV;
+
+ if(bcuDir == 1){
+ bcuDirA.write(setV);
+ bcuDirB.write(0.0);
+ } else if(bcuDir == 0){
+ bcuDirA.write(0.0);
+ bcuDirB.write(setV);
+ }
+
+}
+
+void BuoyancyControlUnit::runBackwards() {
+ this->stop();
+ this->setV(-0.2);
+}
+
+void BuoyancyControlUnit::runForwards() {
+ this->stop();
+ this->setV(0.2);
+}
+
+bool BuoyancyControlUnit::getBCUdir() { return bcuDir; }
+float BuoyancyControlUnit::getVset() { return VRef; }
+float BuoyancyControlUnit::getSetDepth() { return setDepth; }
+float BuoyancyControlUnit::getCurDepth() { return curDepth; }
+float BuoyancyControlUnit::getSetPos() { return setPos; }
+float BuoyancyControlUnit::getCurPos() { return curPos; }
+float BuoyancyControlUnit::readPressure()
+{
+ pressureSensor.Barometer_MS5837();
+ return pressureSensor.MS5837_Pressure();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/BuoyancyControlUnit/BuoyancyControlUnit.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,111 @@
+
+/*
+ * BuoyancyControlUnit.h
+ *
+ * Author: Cyndia Cao, Robert Katzschmann
+ */
+
+#ifndef BUOYANCYCONTROLUNIT_H_
+#define BUOYANCYCONTROLUNIT_H_
+
+#include "mbed.h"
+#include "MS5837/MS5837.h"
+#include "QEI/QEI.h"
+
+
+//#define bcuPwmPin p25
+#define bcuDirAPin p24 //now serves as bcuPWMAPin
+#define bcuDirBPin p25 //now serves as bcuPWMBPin
+#define wiperPin p17 // BCU potentiometer wiper
+//#define bcuCurrentPin p20
+
+#define encoderPinA p16
+#define encoderPinB p17
+#define count2rev 12 //https://www.pololu.com/product/3081/blog
+#define gearRatio 75 // need to check
+
+#define PIN_IMU_TX p28
+#define PIN_IMU_RX p27
+
+//#define maxBCUCurrent 0.5 // need to measure
+//#define minBCUCurrent 0.1 // need to measure
+
+#define KpDepth 5 // random dummy val
+#define KdDepth 0.0 // ignore for now; start with P control
+#define KiDepth 0.0
+
+#define KpEnc 0.0005
+#define KdEnc 0.00003
+#define KiEnc 0.000000000
+
+
+class BuoyancyControlUnit
+{
+public:
+ // Initialization
+ BuoyancyControlUnit();
+ void start();
+ void stop();
+ void set(float depthDesIn);
+ void setDepthFunc(float depthDesIn);
+ void setEncoderPosition(float setPosIn);
+ void setV(float Vin);
+
+ void setDepthFuncVoid();
+ void setEncoderPosVoid();
+
+ void returnToZero();
+ void runBackwards();
+ void runForwards();
+
+ bool getBCUdir();
+ float getVset();
+ float getSetPos();
+ float getCurPos();
+ float getCurDepth();
+ float getSetDepth();
+ float readPressure();
+
+private:
+ volatile bool resetFlag;
+ volatile bool inDepthLoop;
+ volatile bool inPosLoop;
+
+ volatile float setDepth;
+ volatile float curDepth;
+ volatile bool bcuDir;
+
+ volatile float posRef;
+ volatile float depthErr;
+ volatile float prevDepthErr;
+ volatile float depthDer;
+ volatile float depthInt;
+ volatile float depthLastTime;
+
+ volatile float setPos;
+ volatile float curPos;
+
+ volatile float VRef;
+ volatile float posErr;
+ volatile float prevPosErr;
+ volatile float posDer;
+ volatile float posInt;
+ volatile float posLastTime;
+
+ Timer timer;
+ //PwmOut bcuPWM;
+ PwmOut bcuDirA;
+ PwmOut bcuDirB;
+ AnalogIn wiper;
+// AnalogIn bcuCurrent; // actually is a voltage value proportional to current
+// QEI bcuEncoder;
+ MS5837 pressureSensor;
+
+ Ticker depthControl;
+ Ticker posControl;
+};
+
+// Create a static instance of BuoyancyControlUnit to be used by anyone controlling the Buoyancy Control Unit
+extern BuoyancyControlUnit buoyancyControlUnit;
+
+#endif /* BUOYANCYCONTROLUNIT_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/ButtonBoard.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,321 @@
+// buttons.cpp
+
+#include "ButtonBoard.h"
+
+extern "C" void mbed_reset();
+
+ButtonBoard::ButtonBoard(PinName sda, PinName scl, PinName int1, PinName int2) :
+ _i2c(sda, scl),
+ _int1(int1),
+ _int2(int2),
+ _callbackFunction(NULL),
+ _button_state(0)
+{
+ // Initialize callback table
+ for (int i=0; i < BTTN_COUNT; i++)
+ {
+ _callback_table_valid[i] = false;
+ }
+
+ // Set I2C frequency to fast-mode 400KHz
+ _i2c.frequency(400000L);
+
+ ///// Configuration
+ bool bd1_failure = false;
+ bool bd2_failure = false;
+
+ // Configure input latching
+ out_buf[0] = 0x44; // Input latch register
+ out_buf[1] = 0x00; // Don't need latching on output
+ out_buf[1] = 0x00; // Latch them all
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 3);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 3);
+
+ // Disable pull-ups
+ out_buf[0] = 0x46; // Pull-up/down enable register
+ out_buf[1] = 0x00; // Don't need on outputs
+ out_buf[2] = 0x00; // Don't need
+ out_buf[3] = 0xFF; // Select pull-ups
+ out_buf[4] = 0xFF;
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 5);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 5);
+
+ // Configure outputs as open-drain
+ out_buf[0] = 0x4F; // Output port config register
+ out_buf[1] = 0x02; // Port 1 to open drain
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 2);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 2);
+
+ // Reset output register high
+ _led_ports = 0xFFFF; // All LED's off
+ out_buf[0] = 0x02; // Output register
+ out_buf[1] = _led_ports & 0xFF;
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 2);
+ out_buf[2] = _led_ports>>8;
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 2);
+
+ // Configure ports as input or outputs
+ out_buf[0] = 0x06; // Configuration registers
+ out_buf[1] = 0x00; // Port 1 -> Output
+ out_buf[2] = 0xFF; // Port 2 <- Input
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 3);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 3);
+
+ // Read input registers to clear interrupts
+ out_buf[0] = 0x00; // Input registers
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 1, true);
+ bd1_failure |= _i2c.read(ADDR_BOARD_1, out_buf, 2); // Read registers
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 1, true);
+ bd2_failure |= _i2c.read(ADDR_BOARD_2, out_buf, 2); // Read registers
+
+ // Disable interrupt masking on inputs
+ out_buf[0] = 0x4A; // Interrupt mask register
+ out_buf[1] = 0xFF; // Mask outputs
+ out_buf[2] = 0xFF; // Don't mask inputs
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 3);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 3);
+
+ // Disable interrupt masking on inputs
+ out_buf[0] = 0x4A; // Interrupt mask register
+ out_buf[1] = 0xFF; // Mask outputs
+ out_buf[2] = 0x00; // Don't mask inputs
+ bd1_failure |= _i2c.write(ADDR_BOARD_1, out_buf, 3);
+ bd2_failure |= _i2c.write(ADDR_BOARD_2, out_buf, 3);
+
+ if (bd1_failure)
+ {
+ // serial.printf("Nack recieved while configuring button board 1\n");
+ bd1_failure = false;
+ }
+ if (bd2_failure)
+ {
+ // serial.printf("Nack recieved while configuring button board 2\n");
+ bd2_failure = false;
+ }
+
+ // Enable mbed interrupt lines
+ _int1.fall(this, &ButtonBoard::_int1_handler);
+ _int2.fall(this, &ButtonBoard::_int2_handler);
+ _int1.mode(PullUp);
+ _int2.mode(PullUp);
+
+ // Register callbacks
+ //registerCallback(BTTN_RESET, &mbed_reset);
+
+}
+
+ButtonBoard::~ButtonBoard()
+{
+}
+
+void ButtonBoard::_int1_handler()
+{
+ _fall_handler(ADDR_BOARD_1);
+}
+
+void ButtonBoard::_int2_handler()
+{
+ _fall_handler(ADDR_BOARD_2);
+}
+
+void ButtonBoard::_fall_handler(char board)
+{
+ char int_status;
+ char input_port;
+ char masked_port;
+
+ int board_offset;
+ if (board == ADDR_BOARD_1)
+ {
+ board_offset = 0;
+ }
+ else
+ {
+ board_offset = BTTN_COUNT_BOARD_1;
+ }
+
+ // Poll board for interrupt source byte and input reg byte
+ out_buf[0] = 0x4d; // Port 1 Interrupt status
+ _i2c.write(board, out_buf, 1);
+ _i2c.read(board, &int_status, 1);
+
+ out_buf[0] = 0x01; // Port 1 input register
+ _i2c.write(board, out_buf, 1);
+ _i2c.read(board, &input_port, 1);
+
+ // Use int status to mask input port (input goes low when button depressed)
+ // int status will always indicate which button changed
+ // masked_port will also indicate button if button is pressed, but will be 0 if button is released
+ masked_port = int_status & (~input_port);
+ if(masked_port)
+ _button_state |= int_status;
+ else
+ _button_state &= ~int_status;
+
+ for (int i=0; i<8; i++)
+ {
+ // For every high bit in masked port
+ if (masked_port & (1<<i))
+ {
+ // Call corresponding callback
+ int cb_i = i+board_offset;
+ if (_callback_table_valid[cb_i] == true)
+ {
+ _callback_table[cb_i].call();
+ }
+ }
+ }
+ // Call master callback
+ if(_callbackFunction != NULL)
+ _callbackFunction(int_status, masked_port, _button_state);
+}
+
+
+void ButtonBoard::registerCallback(uint32_t button, FunctionPointer p)
+{
+ if (button < BTTN_COUNT)
+ {
+ _callback_table[button] = p;
+ _callback_table_valid[button] = true;
+ }
+}
+
+void ButtonBoard::registerCallback(void (*p)(char buttonMask, bool pressed, char curState))
+{
+ _callbackFunction = p;
+}
+
+//void ButtonBoard::setLED(uint32_t led, bool val)
+//{
+// if (led > BTTN_COUNT)
+// {
+// // invalid, skip
+// return;
+// }
+//
+// char board;
+//
+// if (led < BTTN_COUNT_BOARD_1)
+// {
+// // Address first board
+// board = ADDR_BOARD_1;
+// }
+// else
+// {
+// // Address second board
+// board = ADDR_BOARD_2;
+// led = led - BTTN_COUNT_BOARD_1;
+// }
+//
+// bool fail = false;
+//
+// // Read port state
+// char port_state;
+// out_buf[0] = 0x02; // Port 0 output register
+//
+// fail |= _i2c.write(board, out_buf, 1, true);
+// fail |= _i2c.read(board, &port_state, 1);
+//
+// if (val == true)
+// {
+// // Turn LED on by clearing bit
+// port_state &= ~(1<<led);
+// }
+// else
+// {
+// // Turn LED off by raising bit
+// port_state |= (1<<led);
+// }
+//
+// // Now write to board
+// out_buf[0] = 0x02; // Port 0 output register
+// out_buf[1] = port_state;
+//
+// fail |= _i2c.write(board, out_buf, 2);
+//
+// if (fail)
+// {
+// // serial.printf("Nack recieved when writing LED %d on Board %d", led, board);
+// }
+// else
+// {
+// _led_ports = port_state<<(8*board) + _led_ports & (~(0xFF<<(8*board)));
+// }
+//}
+
+void ButtonBoard::setLEDs(char mask, bool turnOn, char board /* = ADDR_BOARD_1 */)
+{
+ bool fail = false;
+
+ // Read port state
+ char port_state;
+ out_buf[0] = 0x02; // Port 0 output register
+
+ fail |= _i2c.write(board, out_buf, 1, true);
+ fail |= _i2c.read(board, &port_state, 1);
+
+ if(turnOn == true)
+ {
+ // Turn LEDs on by clearing bits
+ port_state &= ~(mask);
+ }
+ else
+ {
+ // Turn LEDs off by raising bits
+ port_state |= (mask);
+ }
+
+ // Now write to board
+ out_buf[0] = 0x02; // Port 0 output register
+ out_buf[1] = port_state;
+
+ fail |= _i2c.write(board, out_buf, 2);
+
+ if(fail)
+ {
+ // serial.printf("Nack recieved when writing LED %d on Board %d", led, board);
+ //printf("button board write failed\n");
+ }
+ else
+ {
+ //_led_ports = port_state<<(8*board) + _led_ports & (~(0xFF<<(8*board)));
+ _led_ports = port_state;
+ }
+}
+
+char ButtonBoard::getLEDs(char ledMask, char board /* = ADDR_BOARD_1 */)
+{
+ return ~_led_ports & ledMask;
+// bool fail = false;
+//
+// // Read port state
+// char port_state;
+// out_buf[0] = 0x02; // Port 0 output register
+//
+// fail |= _i2c.write(board, out_buf, 1, true);
+// fail |= _i2c.read(board, &port_state, 1);
+//
+// return ~port_state & ledMask;
+}
+
+char ButtonBoard::getButtons(char buttonMask, char board /* = ADDR_BOARD_1 */)
+{
+ return _button_state & buttonMask;
+}
+
+//uint16_t ButtonBoard::readInputs()
+//{
+// char b1_p1;
+// char b2_p1;
+//
+// out_buf[0] = 0x01;
+// _i2c.write(ADDR_BOARD_1, out_buf, 1, true);
+// _i2c.read(ADDR_BOARD_1, &b1_p1, 1);
+//
+// _i2c.write(ADDR_BOARD_2, out_buf, 1, true);
+// _i2c.read(ADDR_BOARD_2, &b2_p1, 1);
+//
+// uint16_t out = (b2_p1<<8) | b1_p1;
+//
+// return out;
+//}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/ButtonBoard.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,53 @@
+// buttons.h
+
+#ifndef BUTTONS_H
+#define BUTTONS_H
+
+#include "mbed.h"
+
+#define ADDR_BOARD_1 0x40
+#define ADDR_BOARD_2 0x40
+
+#define BTTN_COUNT_BOARD_1 6
+#define BTTN_COUNT_BOARD_2 6
+#define BTTN_COUNT (BTTN_COUNT_BOARD_1 + BTTN_COUNT_BOARD_2)
+
+class ButtonBoard
+{
+private:
+ I2C _i2c;
+ InterruptIn _int1;
+ InterruptIn _int2;
+
+ char out_buf[8];
+
+ // Callback functions
+ FunctionPointer _callback_table[BTTN_COUNT];
+ bool _callback_table_valid[BTTN_COUNT];
+ void (*_callbackFunction)(char buttonMask, bool pressed, char curState);
+
+ // Interrupt handlers
+ void _int1_handler();
+ void _int2_handler();
+ void _fall_handler(char board);
+
+ // status
+ volatile uint16_t _led_ports;
+ volatile uint8_t _button_state;
+
+public:
+ ButtonBoard(PinName sda, PinName scl, PinName int1, PinName int2);
+ ~ButtonBoard();
+
+ void registerCallback(uint32_t button, FunctionPointer p);
+ void registerCallback(void (*p)(char buttonMask, bool pressed, char curState));
+ //void setLED(uint32_t led, bool val);
+ void setLEDs(char mask, bool turnOn, char board = ADDR_BOARD_1);
+ char getLEDs(char ledMask, char board = ADDR_BOARD_1);
+ char getButtons(char buttonMask, char board = ADDR_BOARD_1);
+
+
+ //uint16_t readInputs();
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/FishController.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,488 @@
+
+#include "FishController.h"
+
+// The static instance
+FishController fishController;
+
+// Function to reset mbed
+extern "C" void mbed_reset();
+
+// Auto mode
+float autoModeCommands[][4] = {FISH_STRAIGHT, FISH_UP, FISH_STRAIGHT, FISH_DOWN};
+uint32_t autoModeDurations[] = {4000, 2000, 2000, 2000}; // durations in milliseconds (is it really? -> check if statement that compares autoModeDurations elements to 10*indexcount
+const uint8_t autoModeLength = sizeof(autoModeDurations)/sizeof(autoModeDurations[0]);
+
+//============================================
+// Initialization
+//============================================
+
+// Constructor
+FishController::FishController():
+ // Initialize variables
+ autoMode(false),
+ ignoreExternalCommands(false),
+ tickerInterval(fishControllerTickerInterval),
+ inTickerCallback(false),
+ servoLeft(servoLeftPin),
+ servoRight(servoRightPin),
+ #ifdef FISH4
+ curTime(0),
+ fullCycle(true),
+ raiser(3.5),
+ // Outputs for motor and servos
+ //motorPWM(motorPWMPin),
+ //motorOutA(motorOutAPin),
+ //motorOutB(motorOutBPin),
+ servoLeft(servoLeftPin),
+ servoRight(servoRightPin),
+ //brushlessMotor(p25),
+ brushlessOffTime(30000),
+ #endif
+/* #ifdef FISH6 // these are declared in BCU class
+ pressureSensor(pressureSensorPinSDA, pressureSensorPinSCL, ms5837_addr_no_CS),
+ imuSensor(imuSensorPinSDA, imuSensorPinSCL)
+ #endif*/
+ // Button board
+ buttonBoard(buttonBoardSDAPin, buttonBoardSCLPin, buttonBoardInt1Pin, buttonBoardInt2Pin) // sda, scl, int1, int2
+
+{
+ streamFishStateEventController = 0;
+
+ newSelectButton = resetSelectButtonValue;
+ newPitch = resetPitchValue;
+ newYaw = resetYawValue;
+ newThrust = resetThrustValue;
+ newFrequency = resetFrequencyValue;
+ newPeriodHalf = resetPeriodHalfValue;
+
+ selectButton = newSelectButton;
+ pitch = newPitch;
+ yaw = newYaw;
+ thrust = newThrust;
+ frequency = newFrequency;
+#ifdef FISH4
+ periodHalf = newPeriodHalf;
+ thrustCommand = 0;
+ dutyCycle = 0;
+ brushlessOff = false;
+#endif
+
+ buttonBoard.registerCallback(&FishController::buttonCallback);
+ buttonBoard.setLEDs(255, false);
+
+ autoModeIndex = 0;
+ autoModeCount = 0;
+
+}
+
+// Set the desired state
+// They will take affect at the next appropriate time in the control cycle
+void FishController::setSelectButton(bool newSelectButtonValue, bool master /* = false*/)
+{
+ if(!ignoreExternalCommands || master)
+ newSelectButton = newSelectButtonValue;
+}
+void FishController::setPitch(float newPitchValue, bool master /* = false*/)
+{
+ if(!ignoreExternalCommands || master)
+ {
+ newPitch = newPitchValue;
+ setLEDs(BTTN_PITCH_UP, (newPitch-fishMinPitch) > (fishMaxPitch - newPitch));
+ setLEDs(BTTN_PITCH_DOWN, (newPitch-fishMinPitch) < (fishMaxPitch - newPitch));
+ }
+}
+void FishController::setYaw(float newYawValue, bool master /* = false*/)
+{
+ if(!ignoreExternalCommands || master)
+ {
+ newYaw = newYawValue;
+ setLEDs(BTTN_YAW_LEFT, (newYaw-fishMinYaw) < (fishMaxYaw - newYaw));
+ setLEDs(BTTN_YAW_RIGHT, (newYaw-fishMinYaw) > (fishMaxYaw - newYaw));
+ }
+}
+void FishController::setThrust(float newThrustValue, bool master /* = false*/)
+{
+ if(!ignoreExternalCommands || master)
+ {
+ newThrust = newThrustValue;
+ setLEDs(BTTN_FASTER, newThrust>fishMinThrust);
+ // If we're in button-control mode, keep the no-thrust light on as an indicator
+ if(!ignoreExternalCommands)
+ setLEDs(BTTN_SLOWER, newThrust==fishMinThrust);
+ else
+ setLEDs(BTTN_SLOWER, true);
+ }
+}
+void FishController::setFrequency(float newFrequencyValue, float newPeriodHalfValue /* = -1 */, bool master /* = false*/)
+{
+ if(!ignoreExternalCommands || master)
+ {
+ newFrequency = newFrequencyValue;
+ newPeriodHalf = newPeriodHalfValue > -1 ? newPeriodHalfValue : (1.0/(2.0*newFrequency));
+ }
+}
+// Get the (possible pending) state
+bool FishController::getSelectButton() {return newSelectButton;}
+float FishController::getPitch() {return newPitch;}
+float FishController::getYaw() {return newYaw;}
+float FishController::getThrust() {return newThrust;}
+float FishController::getFrequency() {return newFrequency;}
+float FishController::getPeriodHalf() {return newPeriodHalf;}
+
+void FishController::start()
+{
+
+ // Blink button board LEDs to indicate startup
+ for(uint8_t i = 0; i < 3; i++)
+ {
+ buttonBoard.setLEDs(255, true);
+ wait_ms(500);
+ buttonBoard.setLEDs(255, false);
+ wait_ms(500);
+ }
+
+#ifdef FISH6
+ buoyancyControlUnit.start();
+ pumpWithValve.start();
+#endif
+
+ // Start control ticker callback
+ ticker.attach_us(&fishController, &FishController::tickerCallback, tickerInterval);
+#ifdef debugFishState
+ printf("Starting...\n");
+#endif
+
+
+}
+
+void FishController::stop()
+{
+ // Stop updating the fish
+ while(inTickerCallback); // wait for commands to settle
+ ticker.detach(); // stop updating commands
+ wait_ms(5); // wait a bit to make sure it stops
+
+ // Reset fish state to neutral
+ newSelectButton = resetSelectButtonValue;
+ newPitch = resetPitchValue;
+ newYaw = resetYawValue;
+ newThrust = resetThrustValue;
+ newFrequency = resetFrequencyValue;
+ newPeriodHalf = resetPeriodHalfValue;
+ // Send commands to fish (multiple times to make sure we get in the right part of the cycle to actually update it)
+ for(int i = 0; i < 200; i++)
+ {
+ tickerCallback();
+ wait_ms(10);
+ }
+ // Make sure commands are sent to motors and applied
+ wait(1);
+
+ #ifdef FISH4
+ // Put dive planes in a weird position to indicate stopped
+ servoLeft = 0.3;
+ servoRight = 0.3;
+ #endif
+
+ #ifdef FISH6
+ pumpWithValve.stop();
+ buoyancyControlUnit.stop();
+ #endif // FISH6
+
+
+ // Light the LEDs to indicate termination
+ buttonBoard.setLEDs(255, true);
+}
+
+//============================================
+// Processing
+//============================================
+#ifdef FISH4
+void FishController::tickerCallback()
+{
+ inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
+
+ // get the current elapsed time since last reset (us)
+ curTime += tickerInterval;
+
+ // see if brushless should be shut down
+ brushlessOff = curTime > (periodHalf-brushlessOffTime);
+
+ // update every half cycle
+ if(curTime > periodHalf)
+ {
+ // read new yaw value every half cycle
+ yaw = newYaw; // a value from -1 to 1
+
+ // Read frequency only every full cycle
+ if(fullCycle)
+ {
+ // Read other new inputs
+ thrust = newThrust; // a value from 0 to 1
+ frequency = newFrequency;
+ periodHalf = newPeriodHalf;
+ // Adjust thrust if needed
+ if(yaw < 0.0)
+ thrustCommand = (1.0 + 0.75*yaw)*thrust; // 0.7 can be adjusted to a power of 2 if needed
+ else
+ thrustCommand = thrust;
+ fullCycle = false;
+ }
+ else
+ {
+ // Reverse for the downward slope
+ if(yaw > 0.0)
+ thrustCommand = -(1.0 - 0.75*yaw)*thrust;
+ else
+ thrustCommand = -thrust;
+ fullCycle = true;
+ }
+
+ // Reset time
+ curTime = 0;
+ }
+
+ // Update the servos
+
+ pitch = newPitch;
+ servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
+ servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
+
+ // Testing whether fishController is running
+ // DigitalOut test(LED1);
+ // test = 1;
+
+ // Update the duty cycle
+ /*dutyCycle = raiser * sin(PI2 * frequency * curTime); // add factor 4.0 to get a cut off sinus
+ if(dutyCycle > 1)
+ dutyCycle = 1;
+ if(dutyCycle < -1)
+ dutyCycle = -1;
+ dutyCycle *= thrustCommand;
+ if(dutyCycle >= 0 && dutyCycle < 0.01)
+ dutyCycle = 0;
+ if(dutyCycle < 0 && dutyCycle > -0.01)
+ dutyCycle = 0;
+ // Update the brushed motor
+ if(dutyCycle >= 0)
+ {
+ motorOutA.write(0);
+ motorOutB.write(1);
+ motorPWM.write(dutyCycle);
+ }
+ else
+ {
+ motorOutA.write(1);
+ motorOutB.write(0);
+ motorPWM.write(-1 * dutyCycle);
+ }*/
+ // Update the brushless motor
+ //brushlessMotor = dutyCycle * !brushlessOff;
+ //brushlessMotor.pulsewidth_us(dutyCycle*500+1500);
+ //brushlessMotor();
+
+
+#ifdef debugFishState
+ printDebugState();
+#endif
+ //printf("%f\n", dutyCycle);
+ //printf("%f %f\r\n", pitch, servoLeft.read());
+ inTickerCallback = false;
+}
+#endif
+
+#ifdef FISH6
+void FishController::tickerCallback()
+{
+ inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
+
+ //set current state to newly commanded value
+ frequency = newFrequency;
+ yaw = newYaw;
+ thrust = newThrust;
+ pitch = newPitch;
+
+ // Update dive planes
+ servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
+ servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
+
+ pumpWithValve.set(frequency, yaw, thrust);
+
+ /* TURNING OFF BCU FOR FIRST OPEN WORLD TEST - AUGUST 21, 2019*/
+ //buoyancyControlUnit.set(pitch); //1100 - 1180 seems to follow well
+
+ //Testing whether fishController.tickerCallback() is running
+ //DigitalOut test(LED1);
+ //test = 1;
+
+
+#ifdef debugFishState
+ printDebugState();
+#endif
+
+ inTickerCallback = false;
+}
+#endif
+
+
+// button will be mask indicating which button triggered this interrupt
+// pressed will indicate whether that button was pressed or released
+// buttonState will be a mask that indicates which buttons are currently pressed
+void FishController::buttonCallback(char button, bool pressed, char state) // static
+{
+ //printf("button %d\t pressed: %d\t state: %d\n", button, pressed, state);
+ //fishController.buttonBoard.setLEDs(button, !fishController.buttonBoard.getLEDs(button));
+ // Only act on button presses (not releases)
+ if(!pressed)
+ return;
+
+ DigitalOut* simBatteryLow;
+ float newYaw, newThrust, newPitch;
+ switch(state)
+ {
+ case BTTN_YAW_LEFT:
+ newYaw = fishController.newYaw;
+ newYaw -= (fishMaxYaw - fishMinYaw)/4.0;
+ newYaw = newYaw < fishMinYaw ? fishMinYaw : newYaw;
+ fishController.setYaw(newYaw, true);
+ fishController.streamFishStateEventController = 6;
+ break;
+ case BTTN_YAW_RIGHT:
+ newYaw = fishController.newYaw;
+ newYaw += (fishMaxYaw - fishMinYaw)/4.0;
+ newYaw = newYaw > fishMaxYaw ? fishMaxYaw : newYaw;
+ fishController.setYaw(newYaw, true);
+ fishController.streamFishStateEventController = 7;
+ break;
+ case BTTN_FASTER:
+ newThrust = fishController.newThrust;
+ newThrust += (fishMaxThrust - fishMinThrust)/4.0;
+ newThrust = newThrust > fishMaxThrust ? fishMaxThrust : newThrust;
+ fishController.setThrust(newThrust, true);
+ fishController.streamFishStateEventController = 8;
+ break;
+ case BTTN_SLOWER:
+ newThrust = fishController.newThrust;
+ newThrust -= (fishMaxThrust - fishMinThrust)/4.0;
+ newThrust = newThrust < fishMinThrust ? fishMinThrust : newThrust;
+ fishController.setThrust(newThrust, true);
+ fishController.streamFishStateEventController = 9;
+ break;
+ case BTTN_PITCH_UP:
+ newPitch = fishController.newPitch;
+ newPitch += (fishMaxPitch - fishMinPitch)/4.0;
+ newPitch = newPitch > fishMaxPitch ? fishMaxPitch : newPitch;
+ fishController.setPitch(newPitch, true);
+ fishController.streamFishStateEventController = 10;
+ break;
+ case BTTN_PITCH_DOWN:
+ newPitch = fishController.newPitch;
+ newPitch -= (fishMaxPitch - fishMinPitch)/4.0;
+ newPitch = newPitch < fishMinPitch ? fishMinPitch : newPitch;
+ fishController.setPitch(newPitch, true);
+ fishController.streamFishStateEventController = 11;
+ break;
+ case BTTN_SHUTDOWN_PI: // signal a low battery signal to trigger the pi to shutdown
+ fishController.streamFishStateEventController = 12;
+ simBatteryLow = new DigitalOut(lowBatteryVoltagePin);
+ simBatteryLow->write(0);
+ break;
+ case BTTN_RESET_MBED:
+ fishController.streamFishStateEventController = 13; // ... if you see this, it didn't happen :)
+ mbed_reset();
+ break;
+ case BTTN_AUTO_MODE:
+ fishController.streamFishStateEventController = 14;
+ if(fishController.autoMode)
+ fishController.stopAutoMode();
+ else
+ fishController.startAutoMode();
+ break;
+ case BTTN_BTTN_MODE:
+ fishController.setIgnoreExternalCommands(!fishController.getIgnoreExternalCommands());
+ break;
+ default:
+ fishController.streamFishStateEventController = 15;
+ break;
+ }
+}
+
+void FishController::setIgnoreExternalCommands(bool ignore)
+{
+ ignoreExternalCommands = ignore;
+}
+
+bool FishController::getIgnoreExternalCommands()
+{
+ return ignoreExternalCommands;
+}
+
+void FishController::startAutoMode()
+{
+ // Start ignoring external commands so as not to interfere with auto mode
+ // But remember what the previous setting was so we can restore it after auto mode
+ ignoreExternalCommandsPreAutoMode = ignoreExternalCommands;
+ setIgnoreExternalCommands(true);
+ // Reset state
+ autoModeCount = 0;
+ autoModeIndex = 0;
+ // Start executing the auto loop
+ autoMode = true;
+ autoModeTicker.attach_us(&fishController, &FishController::autoModeCallback, 10000);
+}
+
+void FishController::stopAutoMode()
+{
+ autoModeTicker.detach();
+ // Auto mode was terminated - put fish into a neutral position
+ setSelectButton(resetSelectButtonValue, true);
+ setPitch(resetPitchValue, true);
+ setYaw(resetYawValue, true);
+ setThrust(resetThrustValue, true);
+ setFrequency(resetFrequencyValue, resetPeriodHalfValue, true);
+ // Restore external mode to what is was previously
+ setIgnoreExternalCommands(ignoreExternalCommandsPreAutoMode);
+ autoMode = false;
+}
+
+void FishController::autoModeCallback()
+{
+ // Assign the current state (stored as pitch, yaw, thrust, frequency)
+ setPitch(autoModeCommands[autoModeIndex][0], true);
+ setYaw(autoModeCommands[autoModeIndex][1], true);
+ setThrust(autoModeCommands[autoModeIndex][2], true);
+ setFrequency(autoModeCommands[autoModeIndex][3], 1.0/(2.0*autoModeCommands[autoModeIndex][3]), true);
+ // See if we advance to the next command
+ autoModeCount++;
+ if(autoModeCount*10 > autoModeDurations[autoModeIndex])
+ {
+ autoModeCount = 0;
+ autoModeIndex = (autoModeIndex+1) % autoModeLength; // loop continuously through commands
+ }
+}
+
+#ifdef debugFishState
+void FishController::printDebugState()
+{
+ printf("pitch: %f yaw: %f thrust: %f frequency: %.8f\r\n",
+ pitch, yaw, thrust, frequency);
+}
+
+#endif
+
+void FishController::setLEDs(char mask, bool turnOn)
+{
+ buttonBoard.setLEDs(mask, turnOn);
+}
+
+#ifdef debugBCU
+/* BCU + Pressure Sensor Helper Functions */
+
+float FishController::getBCUVset(){ return buoyancyControlUnit.getVset(); }
+float FishController::getBCUSetDepth(){ return buoyancyControlUnit.getSetDepth(); }
+float FishController::getBCUCurDepth(){ return buoyancyControlUnit.getCurDepth(); }
+float FishController::getBCUSetPos(){ return buoyancyControlUnit.getSetPos(); }
+float FishController::getBCUCurPos(){ return buoyancyControlUnit.getCurPos(); }
+float FishController::getreadPressure(){ return buoyancyControlUnit.readPressure(); }
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/FishController.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,237 @@
+/*
+ * AcousticController.h
+ * Author: Joseph DelPreto
+ */
+
+#ifndef FISH_CONTROLLER_H
+#define FISH_CONTROLLER_H
+
+// comment out if no debug wanted
+#define debugFishState
+
+// Fish version (only define one of them)
+#define FISH6
+//#define FISH4
+
+#include "mbed.h"
+#include "string"
+#include "ButtonBoard.h"
+#ifdef FISH4
+#include "Servo.h"
+#include "esc.h" // brushless motor controller
+#endif
+#ifdef FISH6
+#include "Servo.h"
+#include "PumpWithValve/PumpWithValve.h"
+#include "BuoyancyControlUnit/BuoyancyControlUnit.h"
+#endif
+
+
+// Control
+#define fishControllerTickerInterval 1000 // how often to call the control ticker, in microseconds
+
+// Constants
+#define PI2 6.2831853 // PI is not included with math.h for some reason
+// Values to use for resetting the fish to neutral
+#define resetSelectButtonValue 0
+#define resetPitchValue 0.5
+#define resetYawValue 0
+#define resetThrustValue 0
+#define resetFrequencyValue 0.0000012 // cycles/us
+#define resetPeriodHalfValue 416666 // 1/(2*frequency) -> us
+
+// Value ranges
+#ifdef FISH4
+#define fishMinPitch ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
+#define fishMaxPitch ((float)(0.8))
+#endif
+
+#ifdef FISH6
+#define fishMinPitch ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
+#define fishMaxPitch ((float)(0.8))
+//#define fishMinPitch ((float)(0.0)) // seems appropriate for BCUs
+//#define fishMaxPitch ((float)(30.0))
+#endif
+
+#define fishMinYaw ((float)(-1.0))
+#define fishMaxYaw ((float)(1.0))
+
+#define fishMinThrust ((float)(0.0))
+#ifdef FISH4
+#define fishMaxThrust ((float)(0.75))
+#endif
+#ifdef FISH6
+#define fishMaxThrust ((float)(1.0))
+#endif
+
+#define fishMinFrequency ((float)(0.0000009))
+#define fishMaxFrequency ((float)(0.0000016))
+
+// Preset states for auto mode definition
+// Each one is pitch, yaw, thrust, frequency
+#define FISH_STRAIGHT {resetPitchValue, resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
+#define FISH_UP {fishMaxPitch , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
+#define FISH_DOWN {fishMinPitch , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
+#define FISH_LEFT {resetPitchValue, fishMaxYaw , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
+#define FISH_RIGHT {resetPitchValue, fishMinYaw , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
+#define FISH_STOP {resetPitchValue, resetYawValue , resetThrustValue , resetFrequencyValue}
+
+// Pins
+#define lowBatteryVoltagePin p16
+
+#ifdef FISH4
+#define motorPWMPin p23
+#define motorOutAPin p11
+#define motorOutBPin p12
+#define servoLeftPin p21
+#define servoRightPin p26 //p24
+#endif
+
+#ifdef FISH6
+// NOTE: FISH6 pins are defined in BCU and Valve classes
+#define pressureSensorPinSDA p28
+#define pressureSensorPinSCL p27
+#define imuSensorPinSDA p28
+#define imuSensorPinSCL p27
+#define servoLeftPin p21
+#define servoRightPin p26
+#endif
+
+
+#define buttonBoardSDAPin p9
+#define buttonBoardSCLPin p10
+#define buttonBoardInt1Pin p29
+#define buttonBoardInt2Pin p30
+
+/* Button board commands
+ Commented indexes go from top left (0) to bottom right (5) as follows:
+ /=========================|
+ / ______________________ |
+ / | (0:8) (1:16) (2:32) | |
+ fish nose | |(3:1) (4:2) (5:4) | | fish tail
+ \ ----------------------| |
+ \ |
+ \=========================|
+ The numbers after the colons are the values to use for that button
+*/
+#define BTTN_FASTER 1 // 3
+#define BTTN_SLOWER 2 // 4
+#define BTTN_YAW_LEFT 4 // 5
+#define BTTN_YAW_RIGHT 8 // 0
+#define BTTN_PITCH_UP 16 // 1 // swims down
+#define BTTN_PITCH_DOWN 32 // 2 // swims up
+#define BTTN_RESET_MBED 36 // 2 and 5
+#define BTTN_SHUTDOWN_PI 9 // 0 and 3
+#define BTTN_AUTO_MODE 33 // 2 and 3
+#define BTTN_BTTN_MODE 12 // 0 and 5
+
+
+class FishController
+{
+ public:
+ // Initialization
+ FishController();
+ void start();
+ void stop();
+ // Processing
+ void tickerCallback();
+ // Debug / Logging
+ volatile uint8_t streamFishStateEventController; // will indicate the last button board event - up to the caller to reset it if desired
+ #ifdef debugFishState
+ void printDebugState();
+ #endif
+ // LEDs
+ void setLEDs(char mask, bool turnOn);
+ // Set New State (which will take affect at next appropriate point in control cycle)
+ void setSelectButton(bool newSelectButtonValue, bool master= false);
+ void setPitch(float newPitchValue, bool master = false);
+ void setYaw(float newYawValue, bool master = false);
+ void setThrust(float newThrustValue, bool master = false);
+ void setFrequency(float newFrequencyValue, float newPeriodHalfValue = -1, bool master = false);
+ // Get (possible pending) State
+ bool getSelectButton();
+ float getPitch();
+ float getYaw();
+ float getThrust();
+ float getFrequency();
+ float getPeriodHalf();
+ // Auto mode
+ volatile bool autoMode;
+ void startAutoMode();
+ void stopAutoMode();
+ void autoModeCallback();
+
+ void setIgnoreExternalCommands(bool ignore);
+ bool getIgnoreExternalCommands();
+
+ // BCU Helper Functions
+ float getBCUVset();
+ float getBCUSetDepth();
+ float getBCUCurDepth();
+ float getBCUSetPos();
+ float getBCUCurPos();
+ float getreadPressure();
+
+ private:
+ // Misc State
+ volatile bool ignoreExternalCommands;
+ // Ticker for controlling tail
+ Ticker ticker;
+ const uint16_t tickerInterval;
+ volatile bool inTickerCallback;
+
+ // State which will be applied at the next appropriate time in the control cycle
+ volatile bool newSelectButton;
+ volatile float newPitch;
+ volatile float newYaw;
+ volatile float newThrust;
+ volatile float newFrequency;
+ volatile float newPeriodHalf;
+
+ // State currently executing on fish
+ volatile bool selectButton;
+ volatile float pitch;
+ volatile float yaw;
+ volatile float thrust;
+ volatile float frequency;
+
+ // Servos (Fish 6)
+ Servo servoLeft;
+ Servo servoRight;
+
+
+#ifdef FISH4
+ volatile float thrustCommand;
+ volatile float periodHalf;
+ volatile float dutyCycle;
+ volatile bool brushlessOff;
+ volatile uint32_t curTime;
+ volatile bool fullCycle;
+ const float raiser;
+ // Outputs for motor and servos
+ //PwmOut motorPWM;
+ //DigitalOut motorOutA;
+ //DigitalOut motorOutB;
+ Servo servoLeft;
+ Servo servoRight;
+ //PwmOut brushlessMotor;
+ const uint32_t brushlessOffTime;
+#endif
+
+ // Button control
+ ButtonBoard buttonBoard;
+ static void buttonCallback(char button, bool pressed, char state);
+
+ // Auto mode
+ Ticker autoModeTicker;
+ uint32_t autoModeCount;
+ uint16_t autoModeIndex;
+ bool ignoreExternalCommandsPreAutoMode;
+};
+
+// Create a static instance of FishController to be used by anyone doing detection
+extern FishController fishController;
+extern volatile uint8_t streamFishStateEvent;
+extern volatile uint16_t streamCurFishState;
+
+#endif // ifndef FISH_CONTROLLER_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/MODDMA.lib Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/jetfishteam/code/MODDMA/#aa486e03742a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/MS5837/MS5837.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,100 @@
+#include <stdlib.h>
+#include "MS5837.h"
+
+
+/*
+ * Sensor operating function according data sheet
+ */
+
+void MS5837::MS5837Init(void)
+{
+ MS5837Reset();
+ MS5837ReadProm();
+ return;
+}
+
+/* Send soft reset to the sensor */
+void MS5837::MS5837Reset(void)
+{
+ /* transmit out 1 byte reset command */
+ ms5837_tx_data[0] = ms5837_reset;
+ if ( i2c.write( device_address, ms5837_tx_data, 1 ) );
+ //printf("send soft reset");
+ wait_ms(20);
+}
+
+/* read the sensor calibration data from rom */
+void MS5837::MS5837ReadProm(void)
+{
+ uint8_t i,j;
+ for (i=0; i<8; i++) {
+ j = i;
+ ms5837_tx_data[0] = ms5837_PROMread + (j<<1);
+ if ( i2c.write( device_address, ms5837_tx_data, 1 ) );
+ if ( i2c.read( device_address, ms5837_rx_data, 2 ) );
+ C[i] = ms5837_rx_data[1] + (ms5837_rx_data[0]<<8);
+ }
+}
+
+/* Start the sensor pressure conversion */
+void MS5837::MS5837ConvertD1(void)
+{
+ ms5837_tx_data[0] = ms5837_convD1;
+ if ( i2c.write( device_address, ms5837_tx_data, 1 ) );
+}
+
+/* Start the sensor temperature conversion */
+void MS5837:: MS5837ConvertD2(void)
+{
+ ms5837_tx_data[0] = ms5837_convD2;
+ if ( i2c.write( device_address, ms5837_tx_data, 1 ) );
+}
+
+/* Read the previous started conversion results */
+int32_t MS5837::MS5837ReadADC(void)
+{
+ int32_t adc;
+ wait_ms(150);
+ ms5837_tx_data[0] = ms5837_ADCread;
+ if ( i2c.write( device_address, ms5837_tx_data, 1 ) );
+ if ( i2c.read( device_address, ms5837_rx_data, 3 ) );
+ adc = ms5837_rx_data[2] + (ms5837_rx_data[1]<<8) + (ms5837_rx_data[0]<<16);
+ return (adc);
+}
+
+/* return the results */
+float MS5837::MS5837_Pressure (void)
+{
+ return P_MS5837;
+}
+float MS5837::MS5837_Temperature (void)
+{
+ return T_MS5837;
+}
+
+/* Sensor reading and calculation procedure */
+void MS5837::Barometer_MS5837(void)
+{
+ int32_t dT, temp;
+ int64_t OFF, SENS, press;
+
+ //no need to do this everytime!
+ //MS5837Reset(); // reset the sensor
+ //MS5837ReadProm(); // read the calibration values
+
+
+ MS5837ConvertD1(); // start pressure conversion
+ D1 = MS5837ReadADC(); // read the pressure value
+ MS5837ConvertD2(); // start temperature conversion
+ D2 = MS5837ReadADC(); // read the temperature value
+
+ /* calculation according MS5837-01BA data sheet DA5837-01BA_006 */
+ dT = D2 - (C[5]* 256);
+ OFF = (int64_t)C[2] * (1<<16) + ((int64_t)dT * (int64_t)C[4]) / (1<<7);
+ SENS = (int64_t)C[1] * (1<<15) + ((int64_t)dT * (int64_t)C[3]) / (1<<8);
+
+ temp = 2000 + (dT * C[6]) / (1<<23);
+ T_MS5837 = (float) temp / 100.0f; // result of temperature in deg C in this var
+ press = (((int64_t)D1 * SENS) / (1<<21) - OFF) / (1<<13);
+ P_MS5837 = (float) press / 10.0f; // result of pressure in mBar in this var
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/MS5837/MS5837.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,64 @@
+#include "mbed.h"
+
+#ifndef MS5837_H
+#define MS5837_H
+
+#define MS5837_RX_DEPTH 3 //
+#define MS5837_TX_DEPTH 2 //
+
+// choose your connection here
+#define ms5837_addr_no_CS 0x76 //0b1110110
+
+#define ms5837_reset 0x1E // Sensor Reset
+
+#define ms5837_convD1_256 0x40 // Convert D1 OSR 256
+#define ms5837_convD1_512 0x42 // Convert D1 OSR 512
+#define ms5837_convD1_1024 0x44 // Convert D1 OSR 1024
+#define ms5837_convD1_2048 0x46 // Convert D1 OSR 2048
+#define ms5837_convD1_4096 0x48 // Convert D1 OSR 4096
+#define ms5837_convD1_8192 0x4A // Convert D1 OSR 8192
+
+#define ms5837_convD1 ms5837_convD1_4096 // choose your sampling rate here
+
+#define ms5837_convD2_256 0x50 // Convert D2 OSR 256
+#define ms5837_convD2_512 0x52 // Convert D2 OSR 512
+#define ms5837_convD2_1024 0x54 // Convert D2 OSR 1024
+#define ms5837_convD2_2048 0x56 // Convert D2 OSR 2048
+#define ms5837_convD2_4096 0x58 // Convert D2 OSR 4096
+#define ms5837_convD2_8192 0x5A // Convert D2 OSR 8192
+
+#define ms5837_convD2 ms5837_convD2_4096 // choose your sampling rate here
+
+#define ms5837_ADCread 0x00 // read ADC command
+#define ms5837_PROMread 0xA0 // read PROM command base address
+
+class MS5837{
+private:
+ int D1, D2, Temp, C[8];
+ float T_MS5837, P_MS5837;
+ /* Data buffers */
+ char ms5837_rx_data[MS5837_RX_DEPTH];
+ char ms5837_tx_data[MS5837_TX_DEPTH];
+
+public:
+ MS5837 (PinName sda, PinName scl,
+ char ms5837_addr = ms5837_addr_no_CS )
+ : i2c( sda, scl ), device_address( ms5837_addr << 1 ) {
+ }
+ void MS5837Init(void);
+ void MS5837Reset(void);
+ void MS5837ReadProm(void);
+ void MS5837ConvertD1(void);
+ void MS5837ConvertD2(void);
+ int32_t MS5837ReadADC(void);
+ float MS5837_Pressure (void);
+ float MS5837_Temperature (void);
+ void Barometer_MS5837(void);
+
+
+private:
+ I2C i2c;
+ char device_address;
+
+};
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/PumpWithValve/PumpWithValve.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,166 @@
+#include <PumpWithValve/PumpWithValve.h>
+
+// The static instance
+PumpWithValve pumpWithValve;
+
+void flipFlowUpStatic()
+{
+ pumpWithValve.flipFlowUp();
+}
+
+void flipFlowDownStatic()
+{
+ pumpWithValve.flipFlowDown();
+}
+
+
+//============================================
+// Initialization
+//============================================
+
+// Constructor
+PumpWithValve::PumpWithValve() :
+ // Initialize variables
+ pumpPWM(pumpPwmPin),
+ valvePWM(valvePwmPin),
+ //valveEncoder(encoderPinA, encoderPinB, NC, count2rev), // use X2 encoding by default
+ //valveCurrent(valveCurrentPin),
+ hallSignal(hallInterruptPin),
+ valveLED(LED4)
+{
+ hallSignal.rise(&flipFlowUpStatic);
+ hallSignal.fall(&flipFlowDownStatic);
+
+ frequency = 0;
+ thrust = 0;
+ yaw = 0;
+
+ valveSide = true;
+ valveV1 = 0;
+ valveV2 = 0;
+ Vfreq = 0;
+ VfreqAdjusted = 0;
+ Vset = 0;
+ dVFreq = 0;
+ freqErr = 0;
+ prevFreqErr = 0;
+
+ timer.start();
+}
+
+
+void PumpWithValve::start()
+{
+ valvePWM.write(0.0); // apply nothing to start
+ pumpPWM.write(0.0);
+ periodSide1 = 0.0;
+ periodSide2 = 0.0;
+
+ timer.reset();
+ valveControl.attach(&pumpWithValve, &PumpWithValve::setVoid, 0.08);
+}
+
+void PumpWithValve::stop()
+{
+ valveControl.detach();
+ valvePWM.write(0.0);
+ pumpPWM.write(0.0);
+}
+
+void PumpWithValve::flipFlowUp()
+{
+ // when the hall sensor sees a rising edge, we have rotated 180 degrees
+ // --> want to adjust the applied voltage based on the side we are on
+ valveSide = true; //change boolean state, keeps track of which half of the rotation we are on
+ valveLED = 1;
+ periodSide1 = timer.read_us();
+ timer.reset();
+ freqAct = 1/(periodSide1 + periodSide2);
+}
+
+void PumpWithValve::flipFlowDown()
+{
+ valveSide = false;
+ valveLED = 0;
+ periodSide2 = timer.read_us();
+ timer.reset();
+ freqAct = 1/(periodSide1 + periodSide2);
+}
+
+//============================================
+// Processing
+//============================================
+void PumpWithValve::set(float freq_in, float yaw_in, float thrust_in){
+ thrust = thrust_in;
+ yaw = yaw_in;
+ frequency = freq_in;
+}
+
+void PumpWithValve::setVoid() {
+ //Centrifugal Pump
+ pumpPWM.write(thrust);
+
+ // set speed of the valve motor through the frequency value
+ if (periodSide1 == 0 || periodSide2 == 0) {
+ Vfreq = frequency * 400000; //just setting directly the voltage, scaled up; need to tune this value
+ this->calculateYawMethod1();
+ }
+ else { // don't be fooled by initialization values
+ // Failure mode - if it has been a full (desired) period since a hall sensor has been read
+ if (timer.read_us() > 1.0 / frequency) {
+ pumpWithValve.stop(); // may have to add a condition that allows for sudden input changes
+ }
+ else {
+ freqErr = frequency - freqAct;
+ dVFreq = KpFreq * freqErr + KdFreq * (freqErr - prevFreqErr);
+ prevFreqErr = freqErr; //reset previous frequency error
+ Vfreq += dVFreq;
+ this->calculateYawMethod1();
+ }
+ }
+}
+
+
+void PumpWithValve::calculateYawMethod1()
+{
+ // split tail frequency voltage into voltage on either side of the valve
+ // TODO figure out ideal relationship between yaw and offf between V1 and V2
+ // is it additive or multiplicative? start with super simple math
+
+ valveV1 = (1.0 + valveOffsetGain * yaw) * Vfreq;
+ valveV2 = (1.0 - valveOffsetGain * yaw) * Vfreq;
+
+ // TODO need to decide whether to give up frequency or yaw when we are at input limits
+ if (valveV1 > 1.0) {valveV1 = 1.0;}
+ else if (valveV1 < 0.0) {valveV1 = 0.05;}
+ if (valveV2 > 1.0) {valveV2 = 1.0;}
+ else if (valveV2 < 0.0) {valveV2 = 0.05;}
+
+ // write valve voltage based on which side the hall sensor says we are on
+ if (valveSide) { Vset = valveV1; }
+ else { Vset = valveV2; }
+
+ valvePWM.write(Vset);
+}
+
+void PumpWithValve::calculateYawMethod2()
+{
+
+ if (yaw < 0.0 && !valveSide) {
+ Vset = (1.0 + valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
+ if (Vset > 1.0) { VfreqAdjusted = 1.0; }
+ }
+ else if (yaw > 0.0 && valveSide) {
+ Vset = (1.0 - valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
+ if (Vset < 0.0) { VfreqAdjusted = 0.05; } // needs to keep turning
+ }
+ else {
+ Vset = Vfreq;
+ VfreqAdjusted = Vfreq;
+ }
+ valvePWM.write(VfreqAdjusted);
+
+}
+
+float PumpWithValve::getVset() { return Vset;}
+bool PumpWithValve::getVside() { return valveSide; }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/PumpWithValve/PumpWithValve.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,80 @@
+
+/*
+ * PumpWithValve.h
+ *
+ * Author: Cyndia Cao, Robert Katzschmann
+ */
+
+#ifndef PUMPWITHVALVE_H_
+#define PUMPWITHVALVE_H_
+
+#include "mbed.h"
+
+#define valvePwmPin p22
+#define pumpPwmPin p23
+#define hallInterruptPin p12
+//#define encoderPinA p25
+//#define encoderPinB p24
+//#define valveCurrentPin p19
+
+#define count2rev 12
+#define valveMotorGearRatio 297.92
+#define KpFreq 10000.0 // frequency on the order of 10^-7
+#define KdFreq 0.00
+#define valveOffsetGain 0.5
+
+class PumpWithValve
+{
+public:
+ // Initialization
+ PumpWithValve();
+ void start();
+ void stop();
+
+ void flipFlowUp();
+ void flipFlowDown();
+ void set(float freq_in, float yaw_in, float thrust_in);
+ void setVoid();
+
+ float getVset();
+ bool getVside();
+
+protected:
+ void calculateYawMethod1();
+ void calculateYawMethod2();
+
+private:
+
+ volatile float frequency;
+ volatile float yaw;
+ volatile float thrust;
+ volatile float periodSide1;
+ volatile float periodSide2;
+
+ volatile bool valveSide;
+ volatile float valveV1;
+ volatile float valveV2;
+ volatile float Vfreq;
+ volatile float VfreqAdjusted;
+ volatile float Vset;
+
+ volatile float freqAct;
+ volatile float freqErr;
+ volatile float prevFreqErr;
+ volatile float dVFreq;
+
+ PwmOut pumpPWM;
+ PwmOut valvePWM;
+ //QEI valveEncoder;
+ //AnalogIn valveCurrent; // actually is a voltage value proportional to current
+ InterruptIn hallSignal;
+ Timer timer;
+ DigitalOut valveLED;
+
+ Ticker valveControl;
+};
+
+// Create a static instance of PumpWithValve to be used by anyone controlling the pump with valve
+extern PumpWithValve pumpWithValve;
+
+#endif /* PUMPWITHVALVE_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/QEI/QEI.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,289 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | ^ | | | | |
+ * ---+ ^ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ +-----+ +-----+ +-----+
+ * Channel B ^ | | | | | |
+ * ------+ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ ^
+ * 90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ +--+ ^ ^ +--+ ^
+ * ^ ^ ^ | | ^ ^ | | ^
+ * Index ------------+ +--------+ +-----------
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ * Rev. count 0 1 2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+/**
+ * Includes
+ */
+#include "QEI.h"
+
+QEI::QEI(PinName channelA,
+ PinName channelB,
+ PinName index,
+ int pulsesPerRev,
+ Encoding encoding) : channelA_(channelA), channelB_(channelB),
+ index_(index) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+ pulsesPerRev_ = pulsesPerRev;
+ encoding_ = encoding;
+
+ //Workout what the current state is.
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+ prevState_ = currState_;
+
+ //X2 encoding uses interrupts on only channel A.
+ //X4 encoding uses interrupts on channel A,
+ //and on channel B.
+ channelA_.rise(this, &QEI::encode);
+ channelA_.fall(this, &QEI::encode);
+
+ //If we're using X4 encoding, then attach interrupts to channel B too.
+ if (encoding == X4_ENCODING) {
+ channelB_.rise(this, &QEI::encode);
+ channelB_.fall(this, &QEI::encode);
+ }
+ //Index is optional.
+ if (index != NC) {
+ index_.rise(this, &QEI::index);
+ }
+
+}
+
+void QEI::reset(void) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+
+}
+
+int QEI::getCurrentState(void) {
+
+ return currState_;
+
+}
+
+int QEI::getPulses(void) {
+
+ return pulses_;
+
+}
+
+int QEI::getRevolutions(void) {
+
+ return revolutions_;
+
+}
+
+// +-------------+
+// | X2 Encoding |
+// +-------------+
+//
+// When observing states two patterns will appear:
+//
+// Counter clockwise rotation:
+//
+// 10 -> 01 -> 10 -> 01 -> ...
+//
+// Clockwise rotation:
+//
+// 11 -> 00 -> 11 -> 00 -> ...
+//
+// We consider counter clockwise rotation to be "forward" and
+// counter clockwise to be "backward". Therefore pulse count will increase
+// during counter clockwise rotation and decrease during clockwise rotation.
+//
+// +-------------+
+// | X4 Encoding |
+// +-------------+
+//
+// There are four possible states for a quadrature encoder which correspond to
+// 2-bit gray code.
+//
+// A state change is only valid if of only one bit has changed.
+// A state change is invalid if both bits have changed.
+//
+// Clockwise Rotation ->
+//
+// 00 01 11 10 00
+//
+// <- Counter Clockwise Rotation
+//
+// If we observe any valid state changes going from left to right, we have
+// moved one pulse clockwise [we will consider this "backward" or "negative"].
+//
+// If we observe any valid state changes going from right to left we have
+// moved one pulse counter clockwise [we will consider this "forward" or
+// "positive"].
+//
+// We might enter an invalid state for a number of reasons which are hard to
+// predict - if this is the case, it is generally safe to ignore it, update
+// the state and carry on, with the error correcting itself shortly after.
+void QEI::encode(void) {
+
+ int change = 0;
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+
+ if (encoding_ == X2_ENCODING) {
+
+ //11->00->11->00 is counter clockwise rotation or "forward".
+ if ((prevState_ == 0x3 && currState_ == 0x0) ||
+ (prevState_ == 0x0 && currState_ == 0x3)) {
+
+ pulses_++;
+
+ }
+ //10->01->10->01 is clockwise rotation or "backward".
+ else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+ (prevState_ == 0x1 && currState_ == 0x2)) {
+
+ pulses_--;
+
+ }
+
+ } else if (encoding_ == X4_ENCODING) {
+
+ //Entered a new valid state.
+ if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+ //2 bit state. Right hand bit of prev XOR left hand bit of current
+ //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+ change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+ if (change == 0) {
+ change = -1;
+ }
+
+ pulses_ -= change;
+ }
+
+ }
+
+ prevState_ = currState_;
+
+}
+
+void QEI::index(void) {
+
+ revolutions_++;
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/QEI/QEI.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,244 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | ^ | | | | |
+ * ---+ ^ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ +-----+ +-----+ +-----+
+ * Channel B ^ | | | | | |
+ * ------+ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ ^
+ * 90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ +--+ ^ ^ +--+ ^
+ * ^ ^ ^ | | ^ ^ | | ^
+ * Index ------------+ +--------+ +-----------
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ * Rev. count 0 1 2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+#ifndef QEI_H
+#define QEI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID 0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class QEI {
+
+public:
+
+ typedef enum Encoding {
+
+ X2_ENCODING,
+ X4_ENCODING
+
+ } Encoding;
+
+ /**
+ * Constructor.
+ *
+ * Reads the current values on channel A and channel B to determine the
+ * initial state.
+ *
+ * Attaches the encode function to the rise/fall interrupt edges of
+ * channels A and B to perform X4 encoding.
+ *
+ * Attaches the index function to the rise interrupt edge of channel index
+ * (if it is used) to count revolutions.
+ *
+ * @param channelA mbed pin for channel A input.
+ * @param channelB mbed pin for channel B input.
+ * @param index mbed pin for optional index channel input,
+ * (pass NC if not needed).
+ * @param pulsesPerRev Number of pulses in one revolution.
+ * @param encoding The encoding to use. Uses X2 encoding by default. X2
+ * encoding uses interrupts on the rising and falling edges
+ * of only channel A where as X4 uses them on both
+ * channels.
+ */
+ QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+ /**
+ * Reset the encoder.
+ *
+ * Sets the pulses and revolutions count to zero.
+ */
+ void reset(void);
+
+ /**
+ * Read the state of the encoder.
+ *
+ * @return The current state of the encoder as a 2-bit number, where:
+ * bit 1 = The reading from channel B
+ * bit 2 = The reading from channel A
+ */
+ int getCurrentState(void);
+
+ /**
+ * Read the number of pulses recorded by the encoder.
+ *
+ * @return Number of pulses which have occured.
+ */
+ int getPulses(void);
+
+ /**
+ * Read the number of revolutions recorded by the encoder on the index channel.
+ *
+ * @return Number of revolutions which have occured on the index channel.
+ */
+ int getRevolutions(void);
+
+private:
+
+ /**
+ * Update the pulse count.
+ *
+ * Called on every rising/falling edge of channels A/B.
+ *
+ * Reads the state of the channels and determines whether a pulse forward
+ * or backward has occured, updating the count appropriately.
+ */
+ void encode(void);
+
+ /**
+ * Called on every rising edge of channel index to update revolution
+ * count by one.
+ */
+ void index(void);
+
+ Encoding encoding_;
+
+ InterruptIn channelA_;
+ InterruptIn channelB_;
+ InterruptIn index_;
+
+ int pulsesPerRev_;
+ int prevState_;
+ int currState_;
+
+ volatile int pulses_;
+ volatile int revolutions_;
+
+};
+
+#endif /* QEI_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/ROSControl/ROSController.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,197 @@
+/*
+ * Author: Joseph DelPreto
+ */
+
+#include "ROSController.h"
+
+#ifdef rosControl
+
+// The static instance
+ROSController rosController;
+
+// Define global methods that call methods of the singleton, to make callbacks easier
+// TODO find out how to get a function pointer to a member function of a specific object (so we won't need these methods)
+void processROSMessageStatic(const fish_msgs::JoystickState& msg)
+{
+ rosController.processROSMessage(msg);
+}
+void lowBatteryCallbackROSStatic()
+{
+ rosController.lowBatteryCallback();
+}
+
+// Initialization
+ROSController::ROSController(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */):
+ subscriber(rosTopicName, &processROSMessageStatic),
+ terminated(false)
+{
+ #ifdef debugLEDsROS
+ rosLEDs[0] = new DigitalOut(LED1);
+ rosLEDs[1] = new DigitalOut(LED2);
+ rosLEDs[2] = new DigitalOut(LED3);
+ rosLEDs[3] = new DigitalOut(LED4);
+ #endif
+ init(serialObject, usbSerialObject);
+}
+
+void ROSController::init(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */)
+{
+ // Create serial object or use provided one
+ if(serialObject == NULL)
+ {
+ serialObject = new Serial(rosDefaultTX, rosDefaultRX);
+ serialObject->baud(rosDefaultBaud);
+ }
+ serial = serialObject;
+ // Create usb serial object or use provided one
+ if(usbSerialObject == NULL)
+ {
+ usbSerialObject = new Serial(USBTX, USBRX);
+ usbSerialObject->baud(rosDefaultBaudUSB);
+ }
+ usbSerial = usbSerialObject;
+
+ // Will check for low battery at startup and using an interrupt
+ lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
+ lowBatteryVoltageInput->mode(PullUp);
+ detectedLowBattery = false;
+ lowBatteryTicker.attach(&lowBatteryCallbackROSStatic, 5);
+
+ // ROS setup
+ nodeHandle.initNode();
+ nodeHandle.subscribe(subscriber);
+
+ // Debug
+ #ifdef debugLEDsROS
+ rosLEDs[0]->write(1);
+ rosLEDs[1]->write(0);
+ rosLEDs[2]->write(0);
+ rosLEDs[3]->write(0);
+ #endif
+}
+
+// Process a received ROS message
+void ROSController::processROSMessage(const fish_msgs::JoystickState& msg)
+{
+ #ifdef debugLEDsROS
+ rosLEDs[2]->write(1);
+ #endif
+
+ // Extract the desired fish state from msg
+ // TODO check that this matches your message format
+ bool selectButton = 0;
+ float pitch = ((msg.depth_ctrl+128) * (rosMaxPitch - rosMinPitch) / 255.0) + rosMinPitch;
+ float yaw = ((msg.yaw_ctrl+127) * (rosMaxYaw - rosMinYaw) / 254.0) + rosMinYaw; // 0 MUST map to 0
+ float thrust = ((msg.speed_ctrl+128) * (rosMaxThrust - rosMinThrust) / 255.0) + rosMinThrust;
+ float frequency = ((msg.freq_ctrl+128) * (rosMaxFrequency- rosMinFrequency) / 255.0) + rosMinFrequency;
+
+ // Apply the new state to the fish
+ fishController.setSelectButton(selectButton);
+ fishController.setPitch(pitch);
+ fishController.setYaw(yaw);
+ fishController.setThrust(thrust);
+ fishController.setFrequency(frequency);
+
+ #ifdef printStatusROSController
+ usbSerial->printf("Start %d\t Pitch %f\t Yaw %f\t Thrust %f\t Freq %.8f\r\n", selectButton, pitch, yaw, thrust, frequency);
+ #endif
+ #ifdef debugLEDsROS
+ rosLEDs[2]->write(0);
+ #endif
+}
+
+// Stop the controller (will also stop the fish controller)
+//
+void ROSController::stop()
+{
+ terminated = true;
+}
+
+// Main loop
+// This is blocking - will not return until terminated by timeout or by calling stop() in another thread
+void ROSController::run()
+{
+
+ #ifdef rosControllerControlFish
+ // Start the fish controller
+ fishController.start();
+ #endif
+
+ #ifdef printStatusROSController
+ usbSerial->printf("\r\nStarting to listen for ROS commands\r\n");
+ #endif
+
+ #ifdef debugLEDsROS
+ rosLEDs[0]->write(1);
+ rosLEDs[1]->write(1);
+ rosLEDs[2]->write(0);
+ rosLEDs[3]->write(0);
+ #endif
+
+ // Process any incoming ROS messages
+ programTimer.reset();
+ programTimer.start();
+ while(!terminated)
+ {
+ // Handle any messages that have been received
+ nodeHandle.spinOnce();
+ // See if we've run for the desired amount of time
+ #ifndef infiniteLoopROS
+ if(programTimer.read_ms() > runTimeROS)
+ stop();
+ #endif
+ }
+ programTimer.stop();
+ #ifdef debugLEDsROS
+ rosLEDs[0]->write(0);
+ rosLEDs[1]->write(0);
+ rosLEDs[2]->write(0);
+ rosLEDs[3]->write(0);
+ #endif
+
+ // TODO stop the ROS node handle / unsubscribe
+ // note this is for when we call stop() on the mbed, independent of when the main ROS program terminates on the Pi
+
+ // Stop the fish controller
+ #ifdef rosControllerControlFish
+ fishController.stop();
+ // If battery died, wait a bit for pi to clean up and shutdown and whatnot
+ if(lowBatteryVoltageInput == 0)
+ {
+ wait(90); // Give the Pi time to shutdown
+ fishController.setLEDs(255, false);
+ }
+ #endif
+
+ #ifdef printStatusROSController
+ usbSerial->printf("\r\nROS controller done!\r\n");
+ #endif
+}
+
+
+void ROSController::lowBatteryCallback()
+{
+ if(lowBatteryVoltageInput == 0 && detectedLowBattery)
+ {
+ // Stop the ROS controller
+ // This will end the main loop, causing main to terminate
+ // Main will also stop the fish controller once this method ends
+ stop();
+ // Also force the pin low to signal the Pi
+ // (should have already been done, but just in case)
+ // TODO check that this really forces it low after this method ends and the pin object may be deleted
+ DigitalOut simBatteryLow(lowBatteryVoltagePin);
+ simBatteryLow = 0;
+ #ifdef printStatusROSController
+ usbSerial->printf("\r\nLow battery! Shutting down.\r\n");
+ wait(0.5); // wait for the message to actually flush
+ #endif
+ }
+ else if(lowBatteryVoltageInput == 0)
+ {
+ detectedLowBattery = true;
+ }
+}
+
+#endif // #ifdef rosControl
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/ROSControl/ROSController.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,84 @@
+/*
+ * ROSController.h
+ *
+ * Author: Joseph DelPreto
+ */
+
+//#define rosControl
+
+#ifdef rosControl
+
+#ifndef ROSCONTROL_ROSCONTROLLER_H_
+#define ROSCONTROL_ROSCONTROLLER_H_
+
+#include "FishController.h"
+#include "mbed.h"
+#include <ros.h>
+#include <std_msgs/Empty.h>
+#include <fish_msgs/JoystickState.h>
+
+// ROS setup
+// TODO replace this with your topic name
+#define rosTopicName "to_serial"
+
+// Pins and comm
+#define rosDefaultBaudUSB 115200
+#define rosDefaultBaud 115200
+#define rosDefaultTX p13
+#define rosDefaultRX p14
+// note lowBatteryVoltagePin is defined in FishController
+
+//#define printStatusROSController // whether to print what's going on (i.e. when it gets commands, etc.)
+#define debugLEDsROS // LED1: initialized LED2: running LED3: receiving a message LED4: done (others turn off)
+#define runTimeROS 10000 // how long to run for (in milliseconds) if inifiniteLoopROS is undefined
+#define infiniteLoopROS // if defined, will run forever (or until stop() is called from another thread)
+
+#define rosControllerControlFish // whether to start fishController to control the servos and motor
+
+// Map bytes sent over ROS serial to ranges for each fish property
+#define rosMinPitch fishMinPitch
+#define rosMaxPitch fishMaxPitch
+#define rosMinYaw fishMinYaw
+#define rosMaxYaw fishMaxYaw
+#define rosMinThrust fishMinThrust
+#define rosMaxThrust fishMaxThrust
+#define rosMinFrequency fishMinFrequency
+#define rosMaxFrequency fishMaxFrequency
+
+class ROSController
+{
+ public:
+ // Initialization
+ ROSController(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if objects are null, ones will be created
+ void init(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if serial objects are null, ones will be created
+ // Execution control
+ void run();
+ void stop();
+ void lowBatteryCallback();
+ // ROS
+ ros::NodeHandle nodeHandle;
+ ros::Subscriber<fish_msgs::JoystickState> subscriber;
+ void processROSMessage(const fish_msgs::JoystickState& msg);
+ private:
+ Timer programTimer;
+ bool terminated;
+ Serial* usbSerial;
+ Serial* serial;
+
+ // Low battery monitor
+ DigitalIn* lowBatteryVoltageInput;
+ Ticker lowBatteryTicker;
+ bool detectedLowBattery;
+
+ // Debug LEDs
+ #ifdef debugLEDsROS
+ DigitalOut* rosLEDs[4];
+ #endif
+};
+
+// Create a static instance of ROSController to be used by anyone doing ROS control
+extern ROSController rosController;
+
+#endif /* ROSCONTROL_ROSCONTROLLER_H_ */
+
+#endif // #ifdef rosControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/SerialControl/SerialController.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,249 @@
+/*
+ * Author: Joseph DelPreto
+ */
+
+#include "SerialController.h"
+
+#ifdef serialControl
+
+// The static instance
+SerialController serialController;
+
+void lowBatteryCallbackSerialStatic()
+{
+ serialController.lowBatteryCallback();
+}
+
+// Initialization
+SerialController::SerialController(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */):
+ terminated(false)
+{
+ #ifdef debugLEDsSerial
+ serialLEDs[0] = new DigitalOut(LED1);
+ serialLEDs[1] = new DigitalOut(LED2);
+ serialLEDs[2] = new DigitalOut(LED3);
+ serialLEDs[3] = new DigitalOut(LED4);
+ #endif
+ init(serialObject, usbSerialObject);
+}
+
+void SerialController::init(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */)
+{
+ // Create serial object or use provided one
+ if(serialObject == NULL)
+ {
+ serialObject = new Serial(serialDefaultTX, serialDefaultRX);
+ serialObject->baud(serialDefaultBaud);
+ }
+ serial = serialObject;
+ // Create usb serial object or use provided one
+ if(usbSerialObject == NULL)
+ {
+ usbSerialObject = new Serial(USBTX, USBRX);
+ usbSerialObject->baud(serialDefaultBaudUSB);
+ }
+ usbSerial = usbSerialObject;
+
+ // Will check for low battery at startup and using an interrupt
+ lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
+ lowBatteryVoltageInput->mode(PullUp);
+ // NOTE Switching to polling instead of interrupt
+ // since it seems like the low battery detector chip may have spurious falls
+ // that cause the interrupt to trigger when the battery isn't actually low
+ //lowBatteryInterrupt = new InterruptIn(lowBatteryVoltagePin);
+ //lowBatteryInterrupt->fall(lowBatteryCallbackSerialStatic);
+ detectedLowBattery = false;
+ lowBatteryTicker.attach(&lowBatteryCallbackSerialStatic, 5);
+
+ #ifdef debugLEDsSerial
+ serialLEDs[0]->write(1);
+ serialLEDs[1]->write(0);
+ serialLEDs[2]->write(0);
+ serialLEDs[3]->write(0);
+ #endif
+}
+
+// Parse the received word into the desired fish state
+// FORMAT: 5 successive bytes indicating selectButton, Pitch, Yaw, Thrust, Frequency
+// a null termination character (0) ends the word
+// each one maps 1-255 to the range specified by the min and max values for that property
+void SerialController::processSerialWord(uint8_t* word)
+{
+ // Scale the bytes into the desired ranges for each property
+ bool selectButton = (word[0] > 127);
+ float pitch = word[1];
+ pitch = ((pitch) * (serialMaxPitch - serialMinPitch) / 6.0) + serialMinPitch;
+
+ float yaw = word[2];
+ yaw = ((yaw) * (serialMaxYaw - serialMinYaw) / 6.0) + serialMinYaw;
+
+ float thrust = word[3];
+ thrust = ((thrust) * (serialMaxThrust - serialMinThrust) / 3.0) + serialMinThrust;
+
+ float frequency = word[4];
+ frequency = ((frequency) * (serialMaxFrequency- serialMinFrequency) / 3.0) + serialMinFrequency;
+
+ // Apply the new state to the fish
+ fishController.setSelectButton(selectButton);
+ fishController.setPitch(pitch);
+ fishController.setYaw(yaw);
+ fishController.setThrust(thrust);
+ fishController.setFrequency(frequency, 1.0/(2.0*frequency));
+
+ #ifdef printStatusSerialController
+ //usbSerial->printf("%s", word);
+ usbSerial->printf("Start %d\t Pitch %f\t Yaw %f\t Thrust %f\t Freq %.8f\r\n", selectButton, pitch, yaw, thrust, frequency);
+ #endif
+}
+
+// Stop the controller (will also stop the fish controller)
+//
+void SerialController::stop()
+{
+ terminated = true;
+}
+
+// Main loop
+// This is blocking - will not return until terminated by timeout or by calling stop() in another thread
+void SerialController::run()
+{
+
+ #ifdef serialControllerControlFish
+ // Start the fish controller
+ fishController.start();
+ #endif
+
+ #ifdef enableAutoMode
+ fishController.startAutoMode();
+ #endif
+
+ // Moved to ticker instead of interrupt (see comments in init), so don't need this check
+ // Check for low battery voltage (also have the interrupt, but check that we're not starting with it low)
+ //if(lowBatteryVoltageInput == 0)
+ // lowBatteryCallback();
+
+ #ifdef printStatusSerialController
+ usbSerial->printf("\r\nStarting to listen for serial commands 2\r\n");
+ #endif
+
+ #ifdef debugLEDsSerial
+ serialLEDs[0]->write(1);
+ serialLEDs[1]->write(1);
+ serialLEDs[2]->write(0);
+ serialLEDs[3]->write(0);
+ #endif
+
+ // Process any incoming serial commands
+ uint8_t serialBuffer[20];
+ uint32_t serialBufferIndex = 0;
+ programTimer.reset();
+ programTimer.start();
+ while(!terminated)
+ {
+ if(serial->readable())
+ {
+ #ifdef debugLEDsSerial
+ serialLEDs[2]->write(1);
+
+ #endif
+
+ //int nextByte = serial->putc('t');
+ //usbSerial->printf("Processed <%s>: ", nextByte);
+ uint8_t nextByte = serial->getc();
+ serialBuffer[serialBufferIndex++] = nextByte;
+ //usbSerial->printf("%c", serialBufferIndex);
+ // If we've received a complete command, process it now
+ if(nextByte == 8)
+ {
+ //usbSerial->printf("Got zero!\n");
+ //usbSerial->printf((char*) (serialBuffer));
+ processSerialWord(serialBuffer);
+
+ serialBufferIndex = 0;
+ }
+ }
+ else
+ {
+ #ifdef debugLEDsSerial
+ serialLEDs[2]->write(0);
+ #endif
+ }
+ #ifndef infiniteLoopSerial
+ if(programTimer.read_ms() > runTimeSerial)
+ stop(); //terminated is made true, break out of while loop
+ #endif
+
+ #ifdef print2Pi
+ if(programTimer.read_ms() - printTime > dataPeriod){
+ serial->printf("Start %d\t Pitch %f\t Yaw %f\t Thrust %f\t Freq %.8f\r\n", fishController.getSelectButton(), fishController.getPitch(), fishController.getYaw(), fishController.getThrust(), fishController.getFrequency());
+ printTime = programTimer.read_ms();
+ }
+ //serial->printf("test");
+ #endif
+
+ #ifdef debugBCUControl
+ usbSerial->printf("V %f\t SDepth %f\t CDepth %f\t sPos %f\t CPos %f\r\n", fishController.getBCUVset(), fishController.getBCUSetDepth(), fishController.getBCUCurDepth(), fishController.getBCUSetPos(), fishController.getBCUCurPos());
+ wait_ms(250);
+ #endif
+
+ #ifdef debugSensor
+ usbSerial->printf("Pressure: %f\r\n", fishController.getreadPressure());
+ wait_ms(250);
+ #endif
+ }
+ programTimer.stop();
+ #ifdef debugLEDsSerial
+ serialLEDs[0]->write(0);
+ serialLEDs[1]->write(0);
+ serialLEDs[2]->write(0);
+ serialLEDs[3]->write(0);
+ #endif
+
+ // Stop the fish controller
+ #ifdef serialControllerControlFish
+ fishController.stop();
+
+ #ifdef enableAutoMode
+ fishController.stopAutoMode();
+ #endif
+
+ // If battery died, wait a bit for pi to clean up and shutdown and whatnot
+ if(lowBatteryVoltageInput == 0)
+ {
+ wait(90); // Give the Pi time to shutdown
+ fishController.setLEDs(255, false);
+ }
+ #endif
+
+ #ifdef printStatusSerialController
+ usbSerial->printf("\r\nSerial controller done!\r\n");
+ #endif
+}
+
+
+void SerialController::lowBatteryCallback()
+{
+ if(lowBatteryVoltageInput == 0 && detectedLowBattery)
+ {
+ // Stop the serial controller
+ // This will end the main loop, causing main to terminate
+ // Main will also stop the fish controller once this method ends
+ stop();
+ // Also force the pin low to signal the Pi
+ // (should have already been done, but just in case)
+ // TODO check that this really forces it low after this method ends and the pin object may be deleted
+ DigitalOut simBatteryLow(lowBatteryVoltagePin);
+ simBatteryLow = 0;
+ #ifdef printStatusSerialController
+ usbSerial->printf("\r\nLow battery! Shutting down.\r\n");
+ wait(0.5); // wait for the message to actually flush
+ #endif
+ }
+ else if(lowBatteryVoltageInput == 0)
+ {
+ detectedLowBattery = true;
+ }
+}
+
+#endif // #ifdef serialControl
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/SerialControl/SerialController.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,83 @@
+/*
+ * JoystickController.h
+ *
+ * Author: Joseph DelPreto
+ */
+
+#define serialControl
+
+#ifdef serialControl
+
+#ifndef SERIALCONTROL_SERIALCONTROLLER_H_
+#define SERIALCONTROL_SERIALCONTROLLER_H_
+
+#include "FishController.h"
+#include "mbed.h"
+#include "string"
+#include "SerialBase.h"
+
+#define serialDefaultBaudUSB 115200
+#define serialDefaultBaud 115200
+#define serialDefaultTX p13
+#define serialDefaultRX p14
+// note lowBatteryVoltagePin is defined in FishController
+
+//#define debugBCUControl // whether to print BCU control values (setDepth, curDepth, Vset, etc.)
+//#define debugSensor // whether to print sensor values being read
+//#define print2Pi // whether to print data to Pi serial monitor
+#define printStatusSerialController // whether to print what's going on (i.e. when it gets commands, etc.)
+//#define debugLEDsSerial // LED1: initialized LED2: running LED3: receiving a character LED4: done (others turn off)
+//#define runTimeSerial 10000 // how long to run for (in milliseconds) if infiniteLoopSerial is undefined
+#define infiniteLoopSerial // if defined, will run forever (or until stop() is called from another thread)
+
+#define serialControllerControlFish // whether to start fishController to control the servos and motor
+#define enableAutoMode // whether to start automode (ignores commands, follows commands defined in FishController.cpp
+
+// Map bytes sent over serial (1-255) to ranges for each fish property
+#define serialMinPitch fishMinPitch
+#define serialMaxPitch fishMaxPitch
+#define serialMinYaw fishMinYaw
+#define serialMaxYaw fishMaxYaw
+#define serialMinThrust fishMinThrust
+#define serialMaxThrust fishMaxThrust
+#define serialMinFrequency fishMinFrequency
+#define serialMaxFrequency fishMaxFrequency
+#define dataPeriod 1000
+
+class SerialController
+{
+public:
+ // Initialization
+ SerialController(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if objects are null, ones will be created
+ void init(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if serial objects are null, ones will be created
+ // Execution control
+ void run();
+ void stop();
+ void lowBatteryCallback();
+ float printTime;
+private:
+ Timer programTimer;
+ bool terminated;
+ Serial* usbSerial;
+ Serial* serial;
+
+ void processSerialWord(uint8_t* word);
+
+ // Low battery monitor
+ DigitalIn* lowBatteryVoltageInput;
+ //InterruptIn* lowBatteryInterrupt;
+ Ticker lowBatteryTicker;
+ bool detectedLowBattery;
+
+ // Debug LEDs
+ #ifdef debugLEDsSerial
+ DigitalOut* serialLEDs[4];
+ #endif
+};
+
+// Create a static instance of SerialController to be used by anyone doing serial control
+extern SerialController serialController;
+
+#endif /* SERIALCONTROL_SERIALCONTROLLER_H_ */
+
+#endif // #ifdef serialControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/Servo/.hgignore Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,19 @@ +syntax: regexp +\.hgignore$ +\.git$ +\.svn$ +\.orig$ +\.msub$ +\.meta$ +\.ctags +\.uvproj$ +\.uvopt$ +\.project$ +\.cproject$ +\.launch$ +\.project$ +\.cproject$ +\.launch$ +Makefile$ +\.ewp$ +\.eww$ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/Servo/Servo.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,74 @@
+/* mbed R/C Servo Library
+ *
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Servo.h"
+#include "mbed.h"
+
+static float clamp(float value, float min, float max) {
+ if(value < min) {
+ return min;
+ } else if(value > max) {
+ return max;
+ } else {
+ return value;
+ }
+}
+
+Servo::Servo(PinName pin) : _pwm(pin) {
+ calibrate();
+ write(0.5);
+}
+
+void Servo::write(float percent) {
+ float offset = _range * 2.0 * (percent - 0.5);
+ _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+ _p = clamp(percent, 0.0, 1.0);
+}
+
+void Servo::position(float degrees) {
+ float offset = _range * (degrees / _degrees);
+ _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+}
+
+void Servo::calibrate(float range, float degrees) {
+ _range = range;
+ _degrees = degrees;
+}
+
+float Servo::read() {
+ return _p;
+}
+
+Servo& Servo::operator= (float percent) {
+ write(percent);
+ return *this;
+}
+
+Servo& Servo::operator= (Servo& rhs) {
+ write(rhs.read());
+ return *this;
+}
+
+Servo::operator float() {
+ return read();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/Servo/Servo.h Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,98 @@
+/* mbed R/C Servo Library
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_SERVO_H
+#define MBED_SERVO_H
+
+#include "mbed.h"
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * // Continuously sweep the servo through it's full range
+ * #include "mbed.h"
+ * #include "Servo.h"
+ *
+ * Servo myservo(p21);
+ *
+ * int main() {
+ * while(1) {
+ * for(int i=0; i<100; i++) {
+ * myservo = i/100.0;
+ * wait(0.01);
+ * }
+ * for(int i=100; i>0; i--) {
+ * myservo = i/100.0;
+ * wait(0.01);
+ * }
+ * }
+ * }
+ * @endcode
+ */
+class Servo {
+
+public:
+ /** Create a servo object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ Servo(PinName pin);
+
+ /** Set the servo position, normalised to it's full range
+ *
+ * @param percent A normalised number 0.0-1.0 to represent the full range.
+ */
+ void write(float percent);
+
+ /** Read the servo motors current position
+ *
+ * @param returns A normalised number 0.0-1.0 representing the full range.
+ */
+ float read();
+
+ /** Set the servo position
+ *
+ * @param degrees Servo position in degrees
+ */
+ void position(float degrees);
+
+ /** Allows calibration of the range and angles for a particular servo
+ *
+ * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
+ * @param degrees Angle from centre to maximum/minimum position in degrees
+ */
+ void calibrate(float range = 0.0005, float degrees = 45.0);
+
+ /** Shorthand for the write and read functions */
+ Servo& operator= (float percent);
+ Servo& operator= (Servo& rhs);
+ operator float();
+
+protected:
+ PwmOut _pwm;
+ float _range;
+ float _degrees;
+ float _p;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/data_stream_format.txt Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,31 @@ +Day 1 +uint32: goertzel 36k (0) +uint32: goertzel 30k (1) +uint16: signal level (average over last buffer) +uint8 : codeword (21) in most significant 5 bits, gain index in least significant 3 bits +uint16: fish state event in upper 5 bits, fish state in lower 11 bits + +Day 2 +3 bytes: goertzel 36k (0) >> 8 +3 bytes: goertzel 30k (1) >> 8 +2 bytes: (signal level >> 2) in lower 11 bits, codeword (21) in upper 5 bits +2 bytes: fish state event in upper 5 bits, fish state in lower 11 bits + + +fish state events: + 0: no event + 1: bad word timing + 2: bad word timing + 3: bad word hamming + 4: timeout + 5: good acoustic word + 6: button: yaw left + 7: button: yaw right + 8: button: faster + 9: button: slower + 10: button: pitch up + 11: button: pitch down + 12: button: shutdown pi + 13: button: reset mbed + 14: button: auto mode (toggle) + 15: button: invalid combination \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robotic_fish_6/main.cpp Tue Jan 14 19:17:05 2020 +0000
@@ -0,0 +1,51 @@
+
+// NOTE look at the below h files to define whether that control mode is enabled
+
+#include "mbed.h"
+//#include "AcousticControl/AcousticController.h" // also need to define acousticControl in ToneDetector.h
+#include "SerialControl/SerialController.h"
+//#include "ROSControl/ROSController.h"
+
+Serial pc(USBTX, USBRX);
+
+int main()
+{
+ /* ACOUSTIC CONTROL */
+ //#ifdef acousticControl
+// pc.baud(115200);
+// // Initialize the acoustic controller
+// acousticController.init(&pc); // if no serial object is provided, it will create one on the USB pins
+// // Start the controller
+// // NOTE this is a blocking method, and if infiniteLoopAcoustic is defined it will run forever (or until low battery callback or button board reset command)
+// // It can be stopped by the acousticController.stop() method, but you have to
+// // control threading here to actually be able to call that
+// // The acoustic controller hasn't been tested with multi-threading though
+// acousticController.run();
+// #endif
+
+ /* SERIAL CONTROL */
+ #ifdef serialControl
+
+ pc.baud(115200);
+ pc.printf("Beginning serial control. \n");
+ // Initialize the serial controller
+ serialController.init(NULL, &pc);
+ // Start the controller
+ // NOTE this is a blocking method, and if infiniteLoopSerial is defined it will run forever (or until low battery callback or button board reset command)
+ // It can be stopped by the serialController.stop() method, but you have to
+ // control threading here to actually be able to call that
+ serialController.run();
+ #endif
+
+ /* ROS CONTROL */
+ //#ifdef rosControl
+// pc.baud(115200);
+// // Initialize the ROS controller
+// rosController.init(NULL, &pc);
+// // Start the controller
+// // NOTE this is a blocking method, and if infiniteLoopSerial is defined it will run forever (or until low battery callback or button board reset command)
+// // It can be stopped by the rosController.stop() method, but you have to
+// // control threading here to actually be able to call that
+// rosController.run();
+// #endif
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/makefile Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,8 @@ +PROJECT := fish_6_0_with_ROS +DEVICES := LPC1768 +GCC4MBED_DIR := $(GCC4MBED_DIR) +USER_LIBS := $(ROS_LIB_DIR) +NO_FLOAT_SCANF := 1 +NO_FLOAT_PRINTF := 1 + +include $(GCC4MBED_DIR)/build/gcc4mbed.mk
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/mbed.bld Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7cff1c4259d7 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robotic_fish_6/mbed.lib Tue Jan 14 19:17:05 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/juansal12/code/mbed/#44931ff4a3ed