SerialController.cpp

00001 /*
00002  * Author: Joseph DelPreto
00003  */
00004 
00005 #include "SerialController.h"
00006 
00007 #ifdef serialControl
00008 
00009 // The static instance
00010 SerialController serialController;
00011 
00012 void lowBatteryCallbackSerialStatic()
00013 {
00014     serialController.lowBatteryCallback();
00015 }
00016 
00017 // Initialization
00018 SerialController::SerialController(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */):
00019         terminated(false)
00020 {
00021     #ifdef debugLEDsSerial
00022     serialLEDs[0] = new DigitalOut(LED1);
00023     serialLEDs[1] = new DigitalOut(LED2);
00024     serialLEDs[2] = new DigitalOut(LED3);
00025     serialLEDs[3] = new DigitalOut(LED4);
00026     #endif
00027     init(serialObject, usbSerialObject);
00028 }
00029 
00030 void SerialController::init(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */)
00031 {
00032     // Create serial object or use provided one
00033     if(serialObject == NULL)
00034     {
00035         serialObject = new Serial(serialDefaultTX, serialDefaultRX);
00036         serialObject->baud(serialDefaultBaud);
00037     }
00038     serial = serialObject;
00039     // Create usb serial object or use provided one
00040     if(usbSerialObject == NULL)
00041     {
00042         usbSerialObject = new Serial(USBTX, USBRX);
00043         usbSerialObject->baud(serialDefaultBaudUSB);
00044     }
00045     usbSerial = usbSerialObject;
00046 
00047     // Will check for low battery at startup and using an interrupt
00048     lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
00049     lowBatteryVoltageInput->mode(PullUp);
00050     // NOTE Switching to polling instead of interrupt
00051     // since it seems like the low battery detector chip may have spurious falls
00052     // that cause the interrupt to trigger when the battery isn't actually low
00053     //lowBatteryInterrupt = new InterruptIn(lowBatteryVoltagePin);
00054     //lowBatteryInterrupt->fall(lowBatteryCallbackSerialStatic);
00055     detectedLowBattery = false;
00056     lowBatteryTicker.attach(&lowBatteryCallbackSerialStatic, 5);
00057 
00058     #ifdef debugLEDsSerial
00059     serialLEDs[0]->write(1);
00060     serialLEDs[1]->write(0);
00061     serialLEDs[2]->write(0);
00062     serialLEDs[3]->write(0);
00063     #endif
00064 }
00065 
00066 // Parse the received word into the desired fish state
00067 // FORMAT: 5 successive bytes indicating selectButton, Pitch, Yaw, Thrust, Frequency
00068 //         a null termination character (0) ends the word
00069 //         each one maps 1-255 to the range specified by the min and max values for that property
00070 void SerialController::processSerialWord(uint8_t* word)
00071 {
00072     // Scale the bytes into the desired ranges for each property
00073     bool selectButton = (word[0] > 127);
00074     float pitch = word[1];
00075     pitch = ((pitch) * (serialMaxPitch - serialMinPitch) / 6.0) + serialMinPitch;
00076 
00077     float yaw = word[2];
00078     yaw = ((yaw) * (serialMaxYaw - serialMinYaw) / 6.0) + serialMinYaw;
00079 
00080     float thrust = word[3];
00081     thrust = ((thrust) * (serialMaxThrust - serialMinThrust) / 3.0) + serialMinThrust;
00082 
00083     float frequency = word[4];
00084     frequency = ((frequency) * (serialMaxFrequency- serialMinFrequency) / 3.0) + serialMinFrequency;
00085 
00086     // Apply the new state to the fish
00087     fishController.setSelectButton(selectButton);
00088     fishController.setPitch(pitch);
00089     fishController.setYaw(yaw);
00090     fishController.setThrust(thrust);
00091     fishController.setFrequency(frequency, 1.0/(2.0*frequency));
00092 
00093     #ifdef printStatusSerialController
00094     //usbSerial->printf("%s", word);
00095     usbSerial->printf("Start %d\t Pitch %f\t Yaw %f\t Thrust %f\t Freq %.8f\r\n", selectButton, pitch, yaw, thrust, frequency);
00096     #endif
00097 }
00098 
00099 // Stop the controller (will also stop the fish controller)
00100 //
00101 void SerialController::stop()
00102 {
00103     terminated = true;
00104 }
00105 
00106 // Main loop
00107 // This is blocking - will not return until terminated by timeout or by calling stop() in another thread
00108 void SerialController::run()
00109 {
00110     
00111     #ifdef serialControllerControlFish
00112     // Start the fish controller
00113     fishController.start();
00114     #endif
00115     
00116     #ifdef enableAutoMode
00117     fishController.startAutoMode();
00118     #endif
00119 
00120     // Moved to ticker instead of interrupt (see comments in init), so don't need this check
00121     // Check for low battery voltage (also have the interrupt, but check that we're not starting with it low)
00122     //if(lowBatteryVoltageInput == 0)
00123     //  lowBatteryCallback();
00124 
00125     #ifdef printStatusSerialController
00126     usbSerial->printf("\r\nStarting to listen for serial commands 2\r\n");
00127     #endif
00128 
00129     #ifdef debugLEDsSerial
00130     serialLEDs[0]->write(1);
00131     serialLEDs[1]->write(1);
00132     serialLEDs[2]->write(0);
00133     serialLEDs[3]->write(0);
00134     #endif
00135 
00136     // Process any incoming serial commands
00137     uint8_t serialBuffer[20];
00138     uint32_t serialBufferIndex = 0;
00139     programTimer.reset();
00140     programTimer.start();
00141     while(!terminated)
00142     {
00143         if(serial->readable())
00144         {
00145             #ifdef debugLEDsSerial
00146             serialLEDs[2]->write(1);
00147             
00148             #endif
00149     
00150             //int nextByte = serial->putc('t');
00151             //usbSerial->printf("Processed <%s>: ", nextByte);
00152             uint8_t nextByte = serial->getc();
00153             serialBuffer[serialBufferIndex++] = nextByte;
00154             //usbSerial->printf("%c", serialBufferIndex);
00155             // If we've received a complete command, process it now
00156             if(nextByte == 8)
00157             {
00158                 //usbSerial->printf("Got zero!\n");
00159                 //usbSerial->printf((char*) (serialBuffer));
00160                 processSerialWord(serialBuffer);
00161                 
00162                 serialBufferIndex = 0;
00163             }
00164         }
00165         else
00166         {
00167             #ifdef debugLEDsSerial
00168             serialLEDs[2]->write(0);
00169             #endif
00170         }
00171         #ifndef infiniteLoopSerial
00172         if(programTimer.read_ms() > runTimeSerial)
00173             stop(); //terminated is made true, break out of while loop
00174         #endif
00175         
00176         #ifdef print2Pi
00177         if(programTimer.read_ms() - printTime > dataPeriod){
00178             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());
00179             printTime = programTimer.read_ms();
00180         }
00181         //serial->printf("test");
00182         #endif
00183         
00184         #ifdef debugBCUControl 
00185         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());
00186         wait_ms(250);
00187         #endif 
00188         
00189         #ifdef debugSensor
00190         usbSerial->printf("Pressure: %f\r\n", fishController.getreadPressure());
00191         wait_ms(250);
00192         #endif 
00193     }
00194     programTimer.stop();
00195     #ifdef debugLEDsSerial
00196     serialLEDs[0]->write(0);
00197     serialLEDs[1]->write(0);
00198     serialLEDs[2]->write(0);
00199     serialLEDs[3]->write(0);
00200     #endif
00201 
00202     // Stop the fish controller
00203     #ifdef serialControllerControlFish
00204     fishController.stop();
00205     
00206     #ifdef enableAutoMode
00207     fishController.stopAutoMode();
00208     #endif
00209     
00210     // If battery died, wait a bit for pi to clean up and shutdown and whatnot
00211     if(lowBatteryVoltageInput == 0)
00212     {
00213         wait(90); // Give the Pi time to shutdown
00214         fishController.setLEDs(255, false);
00215     }
00216     #endif
00217 
00218     #ifdef printStatusSerialController
00219     usbSerial->printf("\r\nSerial controller done!\r\n");
00220     #endif
00221 }
00222 
00223 
00224 void SerialController::lowBatteryCallback()
00225 {
00226     if(lowBatteryVoltageInput == 0 && detectedLowBattery)
00227     {
00228         // Stop the serial controller
00229         // This will end the main loop, causing main to terminate
00230         // Main will also stop the fish controller once this method ends
00231         stop();
00232         // Also force the pin low to signal the Pi
00233         // (should have already been done, but just in case)
00234         // TODO check that this really forces it low after this method ends and the pin object may be deleted
00235         DigitalOut simBatteryLow(lowBatteryVoltagePin);
00236         simBatteryLow = 0;
00237         #ifdef printStatusSerialController
00238         usbSerial->printf("\r\nLow battery! Shutting down.\r\n");
00239         wait(0.5); // wait for the message to actually flush
00240         #endif
00241     }
00242     else if(lowBatteryVoltageInput == 0)
00243     {
00244         detectedLowBattery = true;
00245     }
00246 }
00247 
00248 #endif // #ifdef serialControl
00249